Updated modularized code

Code/Control/Main_Code/CameraDefn.h

+#ifndef CAMERADEFN_H
+#define CAMERADEFN_H
+
+#include "Constants.h"
+#include "Camera.h"
+
+openmv::rpc_scratch_buffer<256> scratch_buffer; // All RPC objects share this buffer.
+openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, consider making address and rate as constructor argument
+
+Camera cam(&interface);
+
+void CameraDefn_Setup(){
+  interface.begin(); //communication between ESP and OpenMV
+}
+
+float CameraDefn_BallDetect(int x, int y){
+  return cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y);
+}
+
+float CameraDefn_GoalFind(int8_t goal[3], int id, int tx, int ty, int tz, int rx, int ry, int rz){
+  return cam.exe_goalfinder(goal[0], goal[1], goal[2], id, tx, ty, tz, rx, ry, rz);
+}
+
+#endif

Code/Control/Main_Code/Constants.h

@@ -12,6 +12,7 @@ const double RESOLUTION_H = 240.0;
 const int ENDDEMO_TAG = 0;
 const uint32_t THISNODE_ID = 88789821;
 int BASE_SPEED = 50; //125;
+int GROUND_LEVEL = 1000; // Pressure at ground level
 
 // The debug LEDs that we will be using. Description at:
 const int DEBUG_STATE = 31;

Code/Control/Main_Code/LIDAR.h

@@ -13,25 +13,24 @@ int lidarThres = 300; // mm
 void LIDAR_Setup(){
   pinMode(LED, OUTPUT);
   digitalWrite(LED, HIGH);
-  if (distanceSensor.begin() == 0)
-    Serial.println("Sensor online!");
+//  if (distanceSensor.begin() == 0)
+//    Serial.println("Sensor online!");
   distanceSensor.startRanging();
   distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
   digitalWrite(LED, LOW);
 }
 
 float LIDAR_BallCapture(){
-  int dist = 0;
-  int ball_cap = 0;
-  dist = distanceSensor.getDistance();
-  Serial.println(dist);
-  if ((dist < lidarThres) && (dist > 0)){ // if we capture the ball 
-    ball_cap = 1; 
-    Serial.println("find ball");
-  }else{
-    ball_cap = 2;
-    Serial.println("no ball");
+  int LIDARVal = 0;
+  int ballCapture = 0;
+  LIDARVal = distanceSensor.getDistance();
+  Serial.print("LIDAR value: ");
+  Serial.println(LIDARVal);
+  if ((LIDARVal < lidarThres) && (LIDARVal > 0)){
+    // Ball captured
+    ballCapture = 1; 
   }
+  return ballCapture;
 }
 
 #endif

Code/Control/Main_Code/Localization.h

@@ -15,6 +15,7 @@ float hoverAltitude = -1;
 
 // Function Declarations
 float altitudeCalc();
+float altitudeControl();
 
 void Localization_Setup(){
   if (!baro.begin()) {
@@ -25,14 +26,11 @@ void Localization_Setup(){
 }
 
 float Localization_Init(){
-  float altitude = altitudeCalc() - groundLevel;
-  return altitude;
+  return altitudeCalc();
 }
 
-void Localization_GoalFound(float altitude){
-  if (hoverAltitude == -1){
-    hoverAltitude = altitude;
-  }
+void Localization_GoalFound(){
+  altitudeControl();
 }
 
 void Localization_BallDetected(){
@@ -40,11 +38,14 @@ void Localization_BallDetected(){
 }
 
 float altitudeControl(){
+  if (hoverAltitude == -1){
+    hoverAltitude = altitudeCalc();
+  }
   return hoverAltitude;
 }
 
 float altitudeCalc(){
-  return baro.getAltitude();
+  return baro.getAltitude() - groundLevel;
 }
 
 #endif

Code/Control/Main_Code/Main_Code.ino

@@ -4,62 +4,54 @@
 #include <Wire.h>
 #include "Debug.h"
 #include "LIDAR.h"
-#include "Camera.h"
 #include <Arduino.h>
 #include "Constants.h"
 #include "utilities.h"
+#include "CameraDefn.h"
 #include "Propulsion.h"
 #include "ColorDetect.h"
-#include "Localization.h"
+//#include "Localization.h"
 // #include "Adafruit_VL53L0X.h"
 using namespace std;
 
-//Identify all the variables that will be used by the robot
-int findcolor = 1; //based on the same code from openMV. Determine the color you want to find
-char newPTCL = '1';
-int pauseState = 0;
-int displayTracking = 0;
-
-int ballCapture = 0;
-int ballDetect  = 0;
-int goalFind    = 0;
-
-float altitude  = 0;
+// Identify all the variables that will be used by the robot
+int   findcolor       = 1; //based on the same code from openMV. Determine the color you want to find
+int   pauseState      = 0;
+int   displayTracking = 0;
+int   ballCapture     = 0;
+int   ballDetect      = 0;
+int   goalFind        = 0;
+float altitude        = 0;
 
 // Function Declarations
 
-
-//Create the interface that will be used by the camera
-openmv::rpc_scratch_buffer<256> scratch_buffer; // All RPC objects share this buffer.
-openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, consider making address and rate as constructor argument
-
-Camera cam(&interface);
-
 void setup() {
   Serial.begin(BAUD_RATE);
   Wire.begin();
   AFMS.begin();  // create with the default frequency 1.6KHz
-  interface.begin(); //communication between ESP and OpenMV
-    
+  
+  CameraDefn_Setup();
   ColorDetect_Setup(findcolor);
   Debug_Setup();
   PID_Setup();
   LIDAR_Setup();
-  Localization_Setup();
+  #ifdef LOCALIZATION_H
+    Localization_Setup();
+  #endif
   #ifdef MESH_H
     Mesh_Setup();
   #endif
 }
 
 void loop(){
+  /* ---------- MAIN LOOP ---------- */
   Debug_LoopStart();
   #ifdef MESH_H
-    MeshUpdate();
+    Mesh_LoopStart();
   #endif
   
   /* --------- PAUSE STATE --------- */
   while(pauseState == 1){
-    Serial.println("pause");
     #ifdef MESH_H
       Mesh_PauseState();
     #endif
@@ -68,38 +60,41 @@ void loop(){
   }
   /* ------- END PAUSE STATE ------- */
   
+  /* -------- INITIALIZATION ------- */
   int id = -1;
   int  x = 0,  y = 0;
   int tx = 0, ty = 0, tz = 0;
   int rx = 0, ry = 0, rz = 0;
   int8_t goal[3] = {0,1,2};
   
+  ballCapture = LIDAR_BallCapture();
+  ballDetect  = CameraDefn_BallDetect(x, y);
+  goalFind    = CameraDefn_GoalFind(goal, id, tx, ty, tz, rx, ry, rz);
+  /* ----- END INITIALIZATION ------ */
+  
   /* -------- STANDBY STATE -------- */
   Debug_StandbyState();
   /* ------ END STANDBY STATE ------ */
 
-  altitude = Localization_Init();
-  
   /* --------- INTERRUPTS ---------- */
-  altitudeBarrier(altitude);
+  #ifdef LOCALIZATION_H
+    altitudeBarrier();
+  #endif
   
   if(insideWindyArea()){
     // leave windy area
     // flip horizontal inputs
   }
-  /* ------ END OF INTERRUPTS ------ */
+  /* -------- END INTERRUPTS ------- */
 
-  /* ---------- MAIN LOOP ---------- */
-  // Have we captured the ball?
-  ballCapture = LIDAR_BallCapture();
-  ballDetect  = cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y);
-  goalFind    = cam.exe_goalfinder(goal[0], goal[1], goal[2], id, tx, ty, tz, rx, ry, rz);
-  
+  /* ---------- MAIN LOGIC --------- */
   if(ballCapture){ // Ball captured
 	  if(goalFind){ // Ball captured and Goal found
       Debug_GoalFound();
       PID_GoalFound(tx/1.00, ty/1.00);
-      Localization_GoalFound(altitude);
+      #ifdef LOCALIZATION_H
+        Localization_GoalFound();
+	    #endif
 	    Propulsion_GoalFound();
     }
     else { // Ball captured but Goal not found
@@ -110,7 +105,9 @@ void loop(){
   else { // Ball not captured
 	  if(ballDetect) { // Ball not captured but detected
       Debug_BallDetected();
-      Localization_BallDetected();
+      #ifdef LOCALIZATION_H
+        Localization_BallDetected();
+      #endif
       PID_BallDetected(x/1.00, y/1.00);
       Propulsion_BallDetected();
     }
@@ -118,19 +115,23 @@ void loop(){
       Propulsion_BallSeeking();
     }
   }
-
-  // -------- END MAIN LOOP -------- //
+  /* ------- END MAIN LOGIC -------- */
+  /* -------- END MAIN LOOP -------- */
 }
 
-void altitudeBarrier(int altitude){
+#ifdef LOCALIZATION_H
+void altitudeBarrier(){
+  float altitude = altitudeCalc();
   if(altitude < lowHeightBarrier){
-    moveVertical((lowHeightBarrier - altitude)*255/lowHeightBarrier);
+    move_V((lowHeightBarrier - altitude)*255/lowHeightBarrier);
   }
   else if(altitude > ceilingHeight - highHeightBarrier){
-    moveVertical(((ceilingHeight - highHeightBarrier) - altitude)*255/highHeightBarrier);
+    move_V(((ceilingHeight - highHeightBarrier) - altitude)*255/highHeightBarrier);
   }	
 }
+#endif
 
 float insideWindyArea(){
+  
   return 0;
 }

Code/Control/Main_Code/Mesh.h.bkp

@@ -8,7 +8,7 @@
 
 ArnholdMesh thisNode;
 
-void MeshSetup(){
+void Mesh_Setup(){
   thisNode.setPanel(&panel);
   thisNode.setStationInfo(STATION_SSID, STATION_PASSWORD);
   thisNode.setMQTTInfo(MQTT_BROKER, MQTT_TOPIC);
@@ -16,6 +16,10 @@ void MeshSetup(){
   thisNode.init();
 }
 
+void Mesh_LoopStart(){
+  MeshUpdate();
+}
+
 void MeshUpdate(){
   thisNode.update();
 }

Code/Control/Main_Code/PID.h

@@ -33,11 +33,6 @@ void PID_GoalFound(int InputxVal, int InputyVal){
   Inputy = InputyVal;
   PID_x.Compute();
   PID_y.Compute();
-
-//  Serial.print("Outputx: ");
-//  Serial.println(Outputx);
-//  Serial.print("Outputy: ");
-//  Serial.println(Outputy);
 }
 
 void PID_BallDetected(int InputxVal, int InputyVal){

Code/Control/Main_Code/Propulsion.h

 #ifndef PROPULSION_H
 #define PROPULSION_H
 
+#include "Debug.h"
 #include "Constants.h"
+#include "Localization.h"
 #include <Adafruit_MotorShield.h>
 
-int zero = 0;
-
 // Function Declarations
-void moveVertical(int vel);
-void moveHorizontal(int vel_hori, int base_speed);
+void move_V(int V_Trn);
+void move_H(int V_Rot, int V_Trn);
+void hover(float altitude);
 
 // Create the motor shield object with the default I2C address
 Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
@@ -18,69 +19,84 @@ Adafruit_DCMotor *motor_HL = AFMS.getMotor(3);
 Adafruit_DCMotor *motor_HR = AFMS.getMotor(4);
 
 void Propulsion_PauseState(){
-  moveVertical(zero);
-  moveHorizontal(zero,zero);
+  move_V(0);
+  move_H(0, 0);
 }
 
 void Propulsion_GoalFound(){
-  moveVertical(Outputy);
-  moveHorizontal(Outputx, BASE_SPEED);
+  move_V(Outputy);
+  move_H(Outputx, BASE_SPEED);
 }
 
 void Propulsion_GoalSeeking(){
-  moveVertical(0);
-  moveHorizontal(SEEKING_SPEED, 0);
+//  move_V(0);
+  hover(altitudeCalc());
+  move_H(SEEKING_SPEED, 0);
 }
 
 void Propulsion_BallDetected(){
-  moveVertical(Outputy);
-  moveHorizontal(Outputx, BASE_SPEED);
+  move_V(Outputy);
+  move_H(Outputx, BASE_SPEED);
 }
 
 void Propulsion_BallSeeking(){
-  moveVertical(0);
-  moveHorizontal(SEEKING_SPEED, 0);
+//  move_V(0);
+  hover(altitudeCalc());
+  move_H(SEEKING_SPEED, 0);
 }
 
-//vel value should be between -255 to 255 with positive values moving the blimp upward.
-void moveVertical(int vel){
-    if (vel > 0) { //up
-      panel.singleLED(DEBUG_VERTICALSPEED, abs(vel), 0, 0);
-      motor_VL->setSpeed(abs((int) vel));
-      motor_VR->setSpeed(abs((int) vel)); 
-      motor_VL->run(BACKWARD);
-      motor_VR->run(BACKWARD);
-    } else if(vel < 0) { //down
-      panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(vel));
-      motor_VL->setSpeed(abs((int) Outputy));  
-      motor_VR->setSpeed(abs((int) Outputy)); 
-      motor_VL->run(FORWARD);
-      motor_VR->run(FORWARD);
-    } else {
-      panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0);
-      motor_VL->setSpeed(0);
-      motor_VR->setSpeed(0);
-    }
+// V_Trn value should be between -255 to 255 with positive values moving the blimp upward.
+void move_V(int V_Trn){
+  if (V_Trn > 0) {
+    // Move up
+    panel.singleLED(DEBUG_VERTICALSPEED, abs(V_Trn), 0, 0);
+    motor_VL->setSpeed(abs((int) V_Trn));
+    motor_VR->setSpeed(abs((int) V_Trn)); 
+    motor_VL->run(BACKWARD);
+    motor_VR->run(BACKWARD);
+  }
+  else if(V_Trn < 0) {
+    // Move down
+    panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(V_Trn));
+    motor_VL->setSpeed(abs((int) V_Trn));  
+    motor_VR->setSpeed(abs((int) V_Trn)); 
+    motor_VL->run(FORWARD);
+    motor_VR->run(FORWARD);
+  }
+  else {
+    // No motor action
+    panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0);
+    motor_VL->setSpeed(0);
+    motor_VR->setSpeed(0);
+  }
 }
 
-void moveHorizontal(int vel_hori, int base_speed){
-  int lspeed = vel_hori + base_speed;
-  int rspeed = vel_hori + base_speed;
+void move_H(int V_Rot, int V_Trn){
+  int lspeed = -1*V_Rot + V_Trn;
+  int rspeed =  1*V_Rot + V_Trn;
 
   if (lspeed > 0){
     motor_HL->run(FORWARD);
-  } else {
+  }
+  else {
     motor_HL->run(BACKWARD);
   }
 
   if (rspeed > 0){
     motor_HR->run(FORWARD);
-  } else {
+  }
+  else {
     motor_HR->run(BACKWARD);
   }
+  
   displaySpeed(lspeed, rspeed);
   motor_HL->setSpeed(min(MAX_SPEED, abs(rspeed)));
   motor_HR->setSpeed(min(MAX_SPEED, abs(lspeed)));
 }
 
+void hover(float altitude){
+  float hoverAltitude = altitudeControl();
+  move_V(BASE_SPEED*(hoverAltitude-altitudeCalc()));
+}
+
 #endif