From c9d1b3b134039c2925fc8519410a7b094591058d Mon Sep 17 00:00:00 2001
From: Shahrul Kamil bin Hassan <shahrulkamil98@g.ucla.edu>
Date: Sat, 9 Oct 2021 00:38:04 +0000
Subject: [PATCH] Replace feather_main_test_PID_April_Tag.ino
---
.../feather_main_test_PID_April_Tag.ino | 66 +++++++------------
1 file changed, 23 insertions(+), 43 deletions(-)
diff --git a/Code/Feather_Code_Test/feather_main_test_PID_April_Tag/feather_main_test_PID_April_Tag.ino b/Code/Feather_Code_Test/feather_main_test_PID_April_Tag/feather_main_test_PID_April_Tag.ino
index 1987131..ae4dc1b 100644
--- a/Code/Feather_Code_Test/feather_main_test_PID_April_Tag/feather_main_test_PID_April_Tag.ino
+++ b/Code/Feather_Code_Test/feather_main_test_PID_April_Tag/feather_main_test_PID_April_Tag.ino
@@ -31,7 +31,7 @@ char newPTCL = '1';
int pau = 0;
int displayTracking = 0;
int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
-int BASE_SPEED = 100; //125;
+int BASE_SPEED = 70; //125;
//The debug LEDs that we will be using. Description at:
const int DEBUG_STATE = 31;
@@ -52,8 +52,8 @@ openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, cons
// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
// changed
-Adafruit_DCMotor *motorVertical_L = AFMS.getMotor(2);
-Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(1);
+Adafruit_DCMotor *motorVertical_L = AFMS.getMotor(1);
+Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(2);
Adafruit_DCMotor *motorLeft = AFMS.getMotor(3);
Adafruit_DCMotor *motorRight = AFMS.getMotor(4);
// changed
@@ -106,51 +106,30 @@ int goal_id[3] = {0, 1, 2};
void loop() {
panel.singleLED(DEBUG_BASESPEED, BASE_SPEED, BASE_SPEED, BASE_SPEED);
-
- //Check to see if we want to end the demo
- 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
-
- int goal_id[3] = {0, 1, 2};
- int id = 0;
+ 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(exe_goalfinder(int goal_id[0], int goal_id[1],int goal_id[2], int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz))
- if(exe_goalfinder(goal_id[0],goal_id[1],goal_id[2],&id,&tx, &ty, &tz, &rx, &ry, &rz)){
- if (displayTracking > 0){
- displayTrackedObject(tx, ty, RESOLUTION_W, RESOLUTION_H); //THIS NEEDS WORK
- }
+ if(cam.exe_goalfinder(goal[0],goal[1],goal[2], id, tx, ty, tz, rx, ry, rz)){
panel.singleLED(DEBUG_STATE, 0, 10, 0);
Inputx = tx/1.00;
Inputy = ty/1.00;
PID_x.Compute();
PID_y.Compute();
- Serial.println(Outputy);
+ Serial.print("x: ");
Serial.println(Outputx);
-
+ Serial.print("y: ");
+ Serial.println(Outputy);
moveVertical(Outputy);
moveHorizontal(Outputx, BASE_SPEED);
}else { // seeking
Serial.println("seeking...");
- moveVertical(zero);
+ moveVertical(0);
moveHorizontal(SEEKING_SPEED, 0);
}
@@ -192,15 +171,17 @@ void moveVertical(int vel){
motorVertical_L->setSpeed(abs((int) vel));
motorVertical_R->setSpeed(abs((int) vel));
motorVertical_L->run(BACKWARD);
- motorVertical_R->run(BACKWARD);
+ motorVertical_R->run(FORWARD);
// changed
} else if(vel < 0) { //down
panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(vel));
// changed
- motorVertical_L->setSpeed(abs((int) Outputy));
- motorVertical_R->setSpeed(abs((int) Outputy));
- motorVertical_L->run(FORWARD);
- motorVertical_R->run(FORWARD);
+ motorVertical_L->setSpeed(abs((int) vel));
+ motorVertical_R->setSpeed(abs((int) vel));
+ motorVertical_L->run(FORWARD);
+ motorVertical_R->run(BACKWARD);
+
+
// changed
} else {
panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0);
@@ -217,19 +198,18 @@ void moveHorizontal(int vel_hori,int base_speed){
int rspeed = vel_hori + base_speed;
if (lspeed > 0){
- motorRight->run(FORWARD);
+ motorLeft->run(BACKWARD);
} else {
- motorRight->run(BACKWARD);
+ motorLeft->run(FORWARD);
}
-
if (rspeed > 0){
- motorLeft-> run(BACKWARD);
+ motorRight-> run(FORWARD);
} else {
- motorLeft-> run(FORWARD);
+ motorRight-> run(BACKWARD);
}
displaySpeed(lspeed, rspeed);
- motorLeft->setSpeed(min(MAX_SPEED, abs(rspeed )));
- motorRight->setSpeed(min(MAX_SPEED, abs(lspeed)));
+ motorLeft->setSpeed(min(MAX_SPEED, abs(lspeed )));
+ motorRight->setSpeed(min(MAX_SPEED, abs(rspeed)));
}
void displaySpeed(int lspeed, int rspeed){
--
GitLab