From eadb2fe57c9262c6a0ede8bda0a9cc594cdfda59 Mon Sep 17 00:00:00 2001
From: Aaron John Sabu <aaronjohnsabu1999@gmail.com>
Date: Tue, 12 Oct 2021 15:27:50 -0700
Subject: [PATCH] added control strategy as if-else's
---
.../PID_Test/PID_Test.ino | 90 ++++++++++---------
1 file changed, 48 insertions(+), 42 deletions(-)
diff --git a/Code/Control/Feather Code - Aaron/PID_Test/PID_Test.ino b/Code/Control/Feather Code - Aaron/PID_Test/PID_Test.ino
index 87cd602..2e12cb8 100644
--- a/Code/Control/Feather Code - Aaron/PID_Test/PID_Test.ino
+++ b/Code/Control/Feather Code - Aaron/PID_Test/PID_Test.ino
@@ -29,6 +29,7 @@ int pau = 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;
@@ -96,21 +97,9 @@ void loop() {
int xtemp = 0;
int ytemp = 0;
int angletemp = 0;
-// if(cam.exe_apriltag_detection(ENDDEMO_TAG, &xtemp, &ytemp, &angletemp)){
-// int zero = 0;
-// moveVertical(zero);
-// moveHorizontal(zero,zero);
-// panel.singleLED(DEBUG_STATE, 0, 0, 10);
-// while(1){
-// Serial.println("end of demo");
-// }
-// }
-
- //if the demo is still ongoing, check to see if there is a desired-color blob
int x = 0;
int y = 0;
panel.singleLED(DEBUG_STATE, 10, 0, 0); //standby
- //if(cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)){
int id = -1;
int tx = 0;
int ty = 0;
@@ -119,41 +108,58 @@ void loop() {
int ry = 0;
int rz = 0;
int8_t goal[3] = {0,1,2};
- 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
+
+ ball_capture = cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)
+
+ if(ball_capture){
+ if(cam.exe_apriltag_detection(ENDDEMO_TAG, &xtemp, &ytemp, &angletemp)){
+ int zero = 0;
+ moveVertical(zero);
+ moveHorizontal(zero,zero);
+ panel.singleLED(DEBUG_STATE, 0, 0, 10);
+ while(1){
+ Serial.println("end of demo");
+ }
}
- 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();
-
- Serial.println(Outputy);
- Serial.println(Outputx);
- Serial.print("Outputx: ");
- Serial.println(Outputx);
- //actuate the vertical motor
- moveVertical(Outputy);
- moveHorizontal(Outputx, BASE_SPEED);
+ 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();
+
+ 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);
+ } else { //seeking algorithm
+ //panel.resetPanel();
+ panel.singleLED(DEBUG_STATE, 10, 10, 10);
+ Serial.println("seeking...");
+ int zero = 0;
+ moveVertical(zero);
+ moveHorizontal(SEEKING_SPEED, zero);
+ }
+ }
+ else {
+ // seek the ball
+
}
}
-//vel value should be between -255 to 255 with positive values moving the blimp
-//upward.
+//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);
--
GitLab