From e90d42fbcafd6c27137758179555aa3d43b55298 Mon Sep 17 00:00:00 2001
From: Aaron John Sabu <aaronjohnsabu1999@gmail.com>
Date: Thu, 14 Oct 2021 14:45:27 -0700
Subject: [PATCH] Progress with code modularization
---
Code/Control/Main_Code/ColorDetect.cpp | 6 +
Code/Control/Main_Code/Constants.cpp | 9 +
Code/Control/Main_Code/Debug.cpp | 45 +++++
Code/Control/Main_Code/LIDAR.cpp | 24 +++
Code/Control/Main_Code/Localization.cpp | 13 +-
Code/Control/Main_Code/Main_Code.ino | 155 +++++++-----------
.../Main_Code/{Mesh.cpp => Mesh.cpp.bkp} | 8 +
Code/Control/Main_Code/PID.cpp | 26 ++-
Code/Control/Main_Code/Propulsion.cpp | 30 +++-
9 files changed, 210 insertions(+), 106 deletions(-)
create mode 100644 Code/Control/Main_Code/Constants.cpp
rename Code/Control/Main_Code/{Mesh.cpp => Mesh.cpp.bkp} (97%)
diff --git a/Code/Control/Main_Code/ColorDetect.cpp b/Code/Control/Main_Code/ColorDetect.cpp
index 4d3d4ef..8805715 100644
--- a/Code/Control/Main_Code/ColorDetect.cpp
+++ b/Code/Control/Main_Code/ColorDetect.cpp
@@ -1,3 +1,9 @@
+#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){
diff --git a/Code/Control/Main_Code/Constants.cpp b/Code/Control/Main_Code/Constants.cpp
new file mode 100644
index 0000000..f332f03
--- /dev/null
+++ b/Code/Control/Main_Code/Constants.cpp
@@ -0,0 +1,9 @@
+// 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
diff --git a/Code/Control/Main_Code/Debug.cpp b/Code/Control/Main_Code/Debug.cpp
index 368ac56..c29f469 100644
--- a/Code/Control/Main_Code/Debug.cpp
+++ b/Code/Control/Main_Code/Debug.cpp
@@ -1,3 +1,48 @@
+#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");
diff --git a/Code/Control/Main_Code/LIDAR.cpp b/Code/Control/Main_Code/LIDAR.cpp
index dc78879..4c08a21 100644
--- a/Code/Control/Main_Code/LIDAR.cpp
+++ b/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");
+ }
+}
diff --git a/Code/Control/Main_Code/Localization.cpp b/Code/Control/Main_Code/Localization.cpp
index 948f04e..ee447c4 100644
--- a/Code/Control/Main_Code/Localization.cpp
+++ b/Code/Control/Main_Code/Localization.cpp
@@ -1,3 +1,4 @@
+#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
+}
diff --git a/Code/Control/Main_Code/Main_Code.ino b/Code/Control/Main_Code/Main_Code.ino
index 782e8e9..bb99f2f 100644
--- a/Code/Control/Main_Code/Main_Code.ino
+++ b/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);
}
}
-
diff --git a/Code/Control/Main_Code/Mesh.cpp b/Code/Control/Main_Code/Mesh.cpp.bkp
similarity index 97%
rename from Code/Control/Main_Code/Mesh.cpp
rename to Code/Control/Main_Code/Mesh.cpp.bkp
index ba0cba9..f13d583 100644
--- a/Code/Control/Main_Code/Mesh.cpp
+++ b/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;
diff --git a/Code/Control/Main_Code/PID.cpp b/Code/Control/Main_Code/PID.cpp
index 15388e9..1cd5a69 100644
--- a/Code/Control/Main_Code/PID.cpp
+++ b/Code/Control/Main_Code/PID.cpp
@@ -1,18 +1,42 @@
#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
diff --git a/Code/Control/Main_Code/Propulsion.cpp b/Code/Control/Main_Code/Propulsion.cpp
index ee694d0..0854722 100644
--- a/Code/Control/Main_Code/Propulsion.cpp
+++ b/Code/Control/Main_Code/Propulsion.cpp
@@ -1,5 +1,8 @@
+#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;
--
GitLab