Progress with code modularization

Code/Control/Main_Code/ColorDetect.cpp

+#include <string>
+
+void ColorDetect_Setup(){
+  translateCodetoThreshold(findcolor);
+}
+
 //Interpret a message to change what color the camera is detecting
 void setColor(String msg) {
   if (msg.length() < 3){

Code/Control/Main_Code/Constants.cpp

+// Identify all the global constants that will be used by the robot
+const int BAUD_RATE = 115200;
+const int MAX_SPEED = 255;
+const int SEEKING_SPEED = 70;
+const double RESOLUTION_W = 320.0;
+const double RESOLUTION_H = 240.0;
+const int ENDDEMO_TAG = 0;
+const uint32_t THISNODE_ID = 88789821;
+int BASE_SPEED = 50; //125;
\ No newline at end of file

Code/Control/Main_Code/Debug.cpp

+#include "LedPanel.h"
+#include "Constants.cpp"
+
+LedPanel panel(NUMPIXELS,PIN_PIXELS);
+
+//The debug LEDs that we will be using. Description at:
+const int DEBUG_STATE = 31;
+const int DEBUG_KP = 30;
+const int DEBUG_KI = 29;
+const int DEBUG_KD = 28;
+const int DEBUG_BASESPEED = 27;
+const int DEBUG_THRESHOLD_MIN = 26;
+const int DEBUG_THRESHOLD_MAX = 25;
+const int DEBUG_VERTICALSPEED = 17;
+const int DEBUG_RSPEED = 16;
+const int DEBUG_LSPEED = 24;
+
+void Debug_Setup(){
+  panel.beginPanel();
+}
+
+void Debug_LoopStart(){
+  panel.singleLED(DEBUG_BASESPEED, BASE_SPEED, BASE_SPEED, BASE_SPEED);
+}
+
+void Debug_PauseState(){
+  panel.singleLED(DEBUG_STATE, 10, 10, 0);
+}
+
+void Debug_StandbyState(){
+  panel.singleLED(DEBUG_STATE, 10, 0, 0);
+}
+
+void Debug_GoalFound(){
+  panel.singleLED(DEBUG_STATE, 0, 10, 0);
+}
+
+void Debug_BallDetected(){
+  panel.singleLED(DEBUG_STATE, 0, 10, 0);
+}
+
+void Debug_GoalSeeking(){
+  // panel.resetPanel();
+  panel.singleLED(DEBUG_STATE, 10, 10, 10);
+}
 
 void displaySpeed(int lspeed, int rspeed){
   //Serial.println("display speed");

Code/Control/Main_Code/LIDAR.cpp

@@ -5,4 +5,28 @@ SFEVL53L1X distanceSensor;
 int budgetIndex = 4;
 int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500};
 int LED = LED_BUILTIN;
+int lidarThres = 300; // mm 
 
+void LIDAR_Setup(){
+  pinMode(LED, OUTPUT);
+  digitalWrite(LED, HIGH);
+  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");
+  }
+}

Code/Control/Main_Code/Localization.cpp

+#include "Constants.cpp"
 #include <Adafruit_MPL3115A2.h>
 
 float groundLevel = 120;
@@ -8,10 +9,6 @@ float ceilingHeight = 20;
 Adafruit_MPL3115A2 baro;
 
 void LocalizationSetup(){
-	BarometerSetup();
-}
-
-void BarometerSetup(){
   if (!baro.begin()) {
     Serial.println("Barometer not found");
     while(1);
@@ -23,6 +20,12 @@ void LocalizationInit(){
   float altitude = altitudeCalc() - groundLevel;
 }
 
+void Localization_GoalFound(){
+  if (hoverAltitude == -1){
+    hoverAltitude = altitude;
+  }
+}
+
 float altitudeCalc(){
   return baro.getAltitude();
-}
\ No newline at end of file
+}

Code/Control/Main_Code/Main_Code.ino

@@ -2,155 +2,113 @@
 #include <vector>
 #include <Wire.h>
 #include "Camera.h"
+#include "Constants.cpp"
 #include <Arduino.h>
-#include "LedPanel.h"
 #include "utilities.h"
 // #include "Adafruit_VL53L0X.h"
 
-double Setpointx, Inputx, Outputx;
-double Setpointy, Inputy, Outputy;
-
-// Identify all the global constants that will be used by the robot
-const int BAUD_RATE = 115200;
-const int MAX_SPEED = 255;
-const int SEEKING_SPEED = 70;
-const double RESOLUTION_W = 320.0;
-const double RESOLUTION_H = 240.0;
-const int ENDDEMO_TAG = 0;
-const uint32_t THISNODE_ID = 88789821;
-
 //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 pau = 0;
+int pause = 0;
 int displayTracking = 0;
 int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
-int BASE_SPEED = 50; //125;
-int ball_capture = 0;
 
-//The debug LEDs that we will be using. Description at:
-const int DEBUG_STATE = 31;
-const int DEBUG_KP = 30;
-const int DEBUG_KI = 29;
-const int DEBUG_KD = 28;
-const int DEBUG_BASESPEED = 27;
-const int DEBUG_THRESHOLD_MIN = 26;
-const int DEBUG_THRESHOLD_MAX = 25;
-const int DEBUG_VERTICALSPEED = 17;
-const int DEBUG_RSPEED = 16;
-const int DEBUG_LSPEED = 24;
+int ballCapture = 0;
+int ballDetect  = 0;
+int goalFind    = 0;
 
 //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);
-LedPanel panel(NUMPIXELS,PIN_PIXELS);
 
 void setup() {
   Serial.begin(BAUD_RATE);
-  translateCodetoThreshold(findcolor);
   Wire.begin();
   AFMS.begin();  // create with the default frequency 1.6KHz
   interface.begin(); //communication between ESP and OpenMV
-  panel.beginPanel();
     
-  Setpointx = 160.0; 
-  Setpointy = 120.0; //the values that the PID will try to reach
-  // MeshSetup();
-  PIDSetup();
-  LocalizationSetup();
+  ColorDetect_Setup();
+  Debug_Setup();
+  PID_Setup();
+  LIDAR_Setup();
+  Localization_Setup();
+  #ifdef MESH_CPP
+    Mesh_Setup();
+  #endif
 }
 
 void loop() {  
-  panel.singleLED(DEBUG_BASESPEED, BASE_SPEED, BASE_SPEED, BASE_SPEED);
-  
+  Debug_LoopStart();
   #ifdef MESH_CPP
-  thisNode.update();
+    MeshUpdate();
   #endif
   
-  while(pau == 1){
+  /* --------- PAUSE STATE --------- */
+  while(pause == 1){
+    Serial.println("pause");
     #ifdef MESH_CPP
-      thisNode.update();
+      Mesh_PauseState();
     #endif
-    int zero = 0;
-    moveVertical(zero);
-    moveHorizontal(zero,zero);
-    panel.singleLED(DEBUG_STATE, 10, 10, 0);
-    Serial.println("pause");
+    Debug_PauseState();
+    Propulsion_PauseState();
   }
+  /* ------- END PAUSE STATE ------- */
   
-  int xtemp = 0;
-  int ytemp = 0;
-  int angletemp = 0;
-  int x = 0;
-  int y = 0;
   int id = -1;
-  int tx = 0;
-  int ty = 0;
-  int tz = 0;
-  int rx = 0;
-  int ry = 0;
-  int rz = 0;
+  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};
-  panel.singleLED(DEBUG_STATE, 10, 0, 0); //standby
   
+  /* -------- STANDBY STATE -------- */
+  Debug_StandbyState();
+  /* ------ END STANDBY STATE ------ */
+  
+  /* --------- INTERRUPTS ---------- */
   altitudeBarrier();
   
   if(insideWindyArea){
     // leave windy area
     // flip horizontal inputs
   }
-  
+  /* ------ END OF INTERRUPTS ------ */
+
+  /* ---------- MAIN LOOP ---------- */
   // Have we captured the ball?
-  // ball_capture = lidar_output()
-  
-  if(ball_capture){
-	  if(cam.exe_goalfinder(goal[0], goal[1], goal[2], id, x, y, tz, rx, ry, rz)){
-      if (displayTracking > 0){
-		    displayTrackedObject(x, y, RESOLUTION_W, RESOLUTION_H); //THIS NEEDS WORK
-      }
-      panel.singleLED(DEBUG_STATE, 0, 10, 0);
-      Serial.println("blob detected");
-      Serial.print("x value: ");
-      Serial.println(x);
-      Serial.print("y value: ");
-      Serial.println(y);
-      Inputx = x/1.00;
-      Inputy = y/1.00;
-      PID_x.Compute();
-      PID_y.Compute();
+  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);
   
-      Serial.print("Outputx: ");
-      Serial.println(Outputx);
-      Serial.print("Outputy: ");
-      Serial.println(Outputy);
-      
-	  moveVertical(Outputy);
-      moveHorizontal(Outputx, BASE_SPEED);
-    } else { //seeking algorithm
-      //panel.resetPanel();
-      panel.singleLED(DEBUG_STATE, 10, 10, 10);
-      Serial.println("seeking...");
-      int zero = 0;
-      moveVertical(zero);
-      moveHorizontal(SEEKING_SPEED, zero);
+  if(ballCapture){ // Ball captured
+	  if(goalFind){ // Ball captured and Goal found
+      Debug_GoalFound();
+      PID_GoalFound(tx/1.00, ty/1.00);
+      Localization_GoalFound();
+	    Propulsion_GoalFound();
+    }
+    else { // Ball captured but Goal not found
+      Debug_GoalSeeking();
+      Propulsion_GoalSeeking();
     }
   }
-  else {
-	  ball_detect = cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y);
-    if(ball_detect) {
-      // move towards ball
-      
+  else { // Ball not captured
+	  if(ballDetect) { // Ball not captured but detected
+      Debug_BallDetected();
+      PID_BallDetected(x/1.00, y/1.00);
+      Propulsion_BallDetected();
     }
-    else {
-      // seek the ball
-      // add seeking algorithm - turn around for 360 degrees?
+    else { // Ball not captured and not detected
+      Propulsion_BallSeeking();
     }
   }
+
+  // -------- END MAIN LOOP -------- //
 }
 
-void altitudeBarrier(){
+void altitudeBarrier(int altitude){
   if(altitude < lowHeightBarrier){
     moveVertical((lowHeightBarrier - altitude)*255/lowHeightBarrier);
   }
@@ -158,4 +116,3 @@ void altitudeBarrier(){
     moveVertical(((ceilingHeight - highHeightBarrier) - altitude)*255/highHeightBarrier);
   }	
 }
-

Code/Control/Main_Code/Mesh.cpp → Code/Control/Main_Code/Mesh.cpp.bkp

@@ -16,6 +16,14 @@ void MeshSetup(){
   thisNode.init();
 }
 
+void MeshUpdate(){
+  thisNode.update();
+}
+
+void Mesh_PauseState(){
+  MeshUpdate();
+}
+
 void interpretDashInstructions(String& msg){
   // Deserialize different dash topics as they are received
   String result;

Code/Control/Main_Code/PID.cpp

 #include <PID_v1.h>
 
+double Setpointx, Inputx, Outputx;
+double Setpointy, Inputy, Outputy;
+
 //Specify the links and initial tuning parameters
 double Kpx=2, Kix=0.05, Kdx=0.25;
 double Kpy=2, Kiy=0.1,  Kdy=0.25;
 PID PID_x(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT);
 PID PID_y(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT);
 
-void PIDSetup(){
+void PID_Setup(){
+  Setpointx = 160.0; 
+  Setpointy = 120.0; //the values that the PID will try to reach
+  
   PID_y.SetOutputLimits(-255, 255); //up positive
   PID_x.SetOutputLimits(-255, 255); //left positive
   PID_x.SetMode(AUTOMATIC);
   PID_y.SetMode(AUTOMATIC);
 }
 
+void PID_GoalFound(int InputxVal, int InputyVal){
+  Inputx = InputxVal;
+  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){
+  Inputx = InputxVal;
+  Inputy = InputyVal;
+  PID_x.Compute();
+  PID_y.Compute();
+}
 
 //This function translate a string of message into the constants for a PID controller. Ignoring the 
 //first command character, user can input any one, two, or all three parameter to be changed by identifying

Code/Control/Main_Code/Propulsion.cpp

+#include "Constants.cpp"
 #include <Adafruit_MotorShield.h>
 
+int zero = 0;
+
 // Create the motor shield object with the default I2C address
 Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
 Adafruit_DCMotor *motor_VL = AFMS.getMotor(2); 
@@ -7,6 +10,31 @@ Adafruit_DCMotor *motor_VR = AFMS.getMotor(1);
 Adafruit_DCMotor *motor_HL = AFMS.getMotor(3);
 Adafruit_DCMotor *motor_HR = AFMS.getMotor(4);
 
+void Propulsion_PauseState(){
+  moveVertical(zero);
+  moveHorizontal(zero,zero);
+}
+
+void Propulsion_GoalFound(){
+  moveVertical(Outputy);
+  moveHorizontal(Outputx, BASE_SPEED);
+}
+
+void Propulsion_GoalSeeking(){
+  moveVertical(0);
+  moveHorizontal(SEEKING_SPEED, 0);
+}
+
+void Propulsion_BallDetected(){
+  moveVertical(Outputy);
+  moveHorizontal(Outputx, BASE_SPEED);
+}
+
+void Propulsion_BallSeeking(){
+  moveVertical(0);
+  moveHorizontal(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
@@ -28,7 +56,7 @@ void moveVertical(int vel){
     }
 }
 
-void moveHorizontal(int vel_hori,int base_speed){
+void moveHorizontal(int vel_hori, int base_speed){
   int lspeed = vel_hori + base_speed;
   int rspeed = vel_hori + base_speed;