From 9d34f0ca5e041e828b393fb683c88f75f9f6db39 Mon Sep 17 00:00:00 2001
From: Shahrul Kamil bin Hassan <shahrulkamil98@g.ucla.edu>
Date: Thu, 7 Oct 2021 22:47:46 +0000
Subject: [PATCH] Replace feather_main_personaldemo_PID.ino
---
.../feather_main_personaldemo_PID.ino | 57 +++++++++++++------
1 file changed, 40 insertions(+), 17 deletions(-)
diff --git a/Code/Kamil's Feather Code/feather_main_personaldemo_PID.ino b/Code/Kamil's Feather Code/feather_main_personaldemo_PID.ino
index 089e84a..1e73416 100644
--- a/Code/Kamil's Feather Code/feather_main_personaldemo_PID.ino
+++ b/Code/Kamil's Feather Code/feather_main_personaldemo_PID.ino
@@ -1,5 +1,6 @@
#include <Wire.h>
#include <Adafruit_MotorShield.h>
+//#include "Adafruit_VL53L0X.h"
#include "ArnholdMesh.h"
#include "LedPanel.h"
#include <Arduino.h>
@@ -33,7 +34,7 @@ char newPTCL = '1';
int pau = 0;
int displayTracking = 0;
int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
-int BASE_SPEED = 20; //125;
+int BASE_SPEED = 50; //125;
//The debug LEDs that we will be using. Description at:
const int DEBUG_STATE = 31;
@@ -118,27 +119,30 @@ 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(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
-// Serial.print("current threshold: ");
-// for (int j = 0; j < 6; j++){
-// Serial.print(threshold[j]);
-// Serial.print(" ");
-// }
-// Serial.println("");
- if(cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)){
+ //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;
+ int tz = 0;
+ int rx = 0;
+ 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
}
@@ -412,6 +416,25 @@ void debugPIDConstants(int lednum, double oldval, double newval){
+//int goal_id[3] = {0, 1, 2};
+//////goal seeking algorithm
+//if (lidar detect green ball){
+// int id = -1; int tx = 0; int ty = 0; int tz = 0;
+// int rx = 0; int ry = 0; int rz = 0;
+// if(cam.exe_goalfinder(goal_id[0], goal_id[1], goal_id[2], int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz)){
+// //apply the same PID control when seeking green ball
+// //if we are at a specific position near the goal (slightly above), move straight forward for 5 second
+// } else {
+// //seeking mechanism to find the goal
+// //move up until we reach the desired altitude
+// //if already at the desired altitude, rotate around
+// }
+//} else { //no ball in basket
+// //do whatever we have been doing before to find a green balloon
+//}
+
+
+
////MESH COMMUNICATION UTILITY FUNCTIONS////
void interpretDashInstructions(String& msg){
--
GitLab