From 0d960762ca523936009007827d86c35a4a6cac97 Mon Sep 17 00:00:00 2001
From: Aaron John Sabu <aaronjohnsabu1999@gmail.com>
Date: Sun, 17 Oct 2021 00:11:39 -0700
Subject: [PATCH] Updated modularized code
---
Code/Control/Main_Code/CameraDefn.h | 24 +++++++
Code/Control/Main_Code/Constants.h | 1 +
Code/Control/Main_Code/LIDAR.h | 23 ++++---
Code/Control/Main_Code/Localization.h | 15 +++--
Code/Control/Main_Code/Main_Code.ino | 85 +++++++++++++------------
Code/Control/Main_Code/Mesh.h.bkp | 6 +-
Code/Control/Main_Code/PID.h | 5 --
Code/Control/Main_Code/Propulsion.h | 92 ++++++++++++++++-----------
8 files changed, 146 insertions(+), 105 deletions(-)
create mode 100644 Code/Control/Main_Code/CameraDefn.h
diff --git a/Code/Control/Main_Code/CameraDefn.h b/Code/Control/Main_Code/CameraDefn.h
new file mode 100644
index 0000000..767cd9e
--- /dev/null
+++ b/Code/Control/Main_Code/CameraDefn.h
@@ -0,0 +1,24 @@
+#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
diff --git a/Code/Control/Main_Code/Constants.h b/Code/Control/Main_Code/Constants.h
index f43c1dc..232fa65 100644
--- a/Code/Control/Main_Code/Constants.h
+++ b/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;
diff --git a/Code/Control/Main_Code/LIDAR.h b/Code/Control/Main_Code/LIDAR.h
index c5cab0b..554e392 100644
--- a/Code/Control/Main_Code/LIDAR.h
+++ b/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
diff --git a/Code/Control/Main_Code/Localization.h b/Code/Control/Main_Code/Localization.h
index 54be7c9..590799e 100644
--- a/Code/Control/Main_Code/Localization.h
+++ b/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
diff --git a/Code/Control/Main_Code/Main_Code.ino b/Code/Control/Main_Code/Main_Code.ino
index a359a37..49f46f1 100644
--- a/Code/Control/Main_Code/Main_Code.ino
+++ b/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;
}
diff --git a/Code/Control/Main_Code/Mesh.h.bkp b/Code/Control/Main_Code/Mesh.h.bkp
index f13d583..ad6d4cf 100644
--- a/Code/Control/Main_Code/Mesh.h.bkp
+++ b/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();
}
diff --git a/Code/Control/Main_Code/PID.h b/Code/Control/Main_Code/PID.h
index fa548ce..2aa0478 100644
--- a/Code/Control/Main_Code/PID.h
+++ b/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){
diff --git a/Code/Control/Main_Code/Propulsion.h b/Code/Control/Main_Code/Propulsion.h
index 663c317..e37903f 100644
--- a/Code/Control/Main_Code/Propulsion.h
+++ b/Code/Control/Main_Code/Propulsion.h
@@ -1,14 +1,15 @@
#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
--
GitLab