diff --git a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
index 43434c83c03c63ad0020f1e3a5b5eea74064f01e..b642da5d681b7b1dbe90e8a5790c85d423950f15 100644
--- a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
+++ b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
@@ -316,5 +316,5 @@ void send_message(){
Serial.println("Error sending the data");
}
//-------------------------------------------------------------------------------------
- // delay(50); // delay 50 ms after send out the message
+ delay(50); // delay 50 ms after send out the message
}
diff --git a/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.cpp b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fb27cf808991a0b2083ca36f412ba1aa088527e6
--- /dev/null
+++ b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.cpp
@@ -0,0 +1,91 @@
+#include "Camera.h"
+#include <Wire.h>
+#include <Arduino.h>
+#include <openmvrpc.h>
+
+Camera::Camera(openmv::rpc_i2c_master *intface){
+ interface = intface;
+}
+
+void Camera::exe_face_detection(){
+ struct { uint16_t x, y, w, h; } face_detection_result;
+ if (interface->call_no_args(F("face_detection"), &face_detection_result, sizeof(face_detection_result))) {
+ Serial.print(F("Largest Face Detected [x="));
+ Serial.print(face_detection_result.x);
+ Serial.print(F(", y="));
+ Serial.print(face_detection_result.y);
+ Serial.print(F(", w="));
+ Serial.print(face_detection_result.w);
+ Serial.print(F(", h="));
+ Serial.print(face_detection_result.h);
+ Serial.println(F("]"));
+ }
+}
+
+bool Camera::exe_apriltag_detection(int ID,int *x_temp,int *y_temp,int *angle_temp){
+ struct { uint16_t cx, cy, rot; } result;
+ if (interface->call(F("apriltags"), &ID, sizeof(ID), &result, sizeof(result))) {
+ }
+ if (result.cx == 0 && result.cy == 0 && result.rot == 0){
+ return false;
+ } else {
+ *x_temp = result.cx;
+ *y_temp = result.cy;
+ *angle_temp = result.rot;
+ return true;
+ }
+}
+
+bool Camera::exe_color_detection(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max){
+ int8_t color_thresholds[6] = {l_min, l_max, a_min, a_max, b_min, b_max};
+ short buff[128 + 1] = {};
+ if (interface->call(F("color_detection"), color_thresholds, sizeof(color_thresholds), buff, sizeof(buff)-1)) {
+ int i = 0;
+ while (buff[i] != '\0' && i<100) {
+ Serial.print(buff[i]);
+ i++;
+ }
+ Serial.println("");
+ }
+ if (buff[0] == 0){ //no blob detected
+ return false;
+ } else {
+ return true;
+ }
+}
+
+bool Camera::exe_color_detection_biggestblob(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max, int& x, int&y){
+ int8_t color_thresholds[6] = {l_min, l_max, a_min, a_max, b_min, b_max};
+ struct { uint16_t cx, cy; } color_detection_result;
+ if (interface->call(F("color_detection_single"), color_thresholds, sizeof(color_thresholds), &color_detection_result, sizeof(color_detection_result))) {
+
+ }
+ x = color_detection_result.cx;
+ y = color_detection_result.cy;
+ if (x == 0 && y == 0){
+ return false;
+ } else {
+ return true;
+ }
+}
+
+
+bool Camera::exe_goalfinder(int8_t goal1, int8_t goal2, int8_t goal3, int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz){
+ int8_t goalid[3] = {goal1, goal2, goal3};
+ struct { uint16_t cid, ctx, cty, ctz, crx, cry, crz; } goalfinder_result;
+ if(interface->call(F("goalfinder"), goalid, sizeof(goalid), &goalfinder_result, sizeof(goalfinder_result))){
+
+ }
+ if (goalfinder_result.crx == 0 && goalfinder_result.cry == 0){
+ return false;
+ } else {
+ id = goalfinder_result.cid;
+ tx = goalfinder_result.ctx;
+ ty = goalfinder_result.cty;
+ tz = goalfinder_result.ctz;
+ rx = goalfinder_result.crx;
+ ry = goalfinder_result.cry;
+ rz = goalfinder_result.crz;
+ return true;
+ }
+}
diff --git a/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.h b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.h
new file mode 100644
index 0000000000000000000000000000000000000000..5a4536e6d0967e96fd5864e596b8f8f9f0312289
--- /dev/null
+++ b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/Camera.h
@@ -0,0 +1,19 @@
+#ifndef CAMERA_H
+#define CAMERA_H
+#include <openmvrpc.h>
+
+class Camera
+{
+ private:
+ openmv::rpc_i2c_master *interface;
+
+ public:
+ Camera(openmv::rpc_i2c_master *intface);
+ void exe_face_detection(); // Face should be about 2ft away.
+ bool exe_apriltag_detection(int ID,int *x_temp,int *y_temp,int *angle_temp);
+ bool exe_color_detection(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max);
+ bool exe_color_detection_biggestblob(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max, int& x, int&y);
+ bool exe_goalfinder(int8_t goal1, int8_t goal2, int8_t goal3, int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz); //optional to add tag size as parameter?
+
+};
+#endif
diff --git a/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/ESP32_slave.ino
index 9f133bb5f337b1c4ff34ba590d9409bf3b9cea8f..37e23c4c1bba7cbbfc667484fad80b35460551a4 100644
--- a/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/ESP32_slave.ino
+++ b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/ESP32_slave.ino
@@ -11,7 +11,8 @@
// REPLACE WITH THE MAC Address of your receiver (MASTER)
// Slave: 40:F5:20:44:B6:4C
// uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xCF, 0x00};
-uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
+ uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
+// uint8_t broadcastAddress[] = {0xc4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}; // #3
String success;
// Define variables to store incoming readings
@@ -82,10 +83,10 @@ void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
Serial.print("\r\nLast Packet Send Status:\t");
Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
if (status ==0){
- success = "Delivery Success :)";
+ success = "Delivery Success 🙂";
}
else{
- success = "Delivery Fail :(";
+ success = "Delivery Fail ðŸ™";
}
}
@@ -210,7 +211,7 @@ void loop()
Sent_dist = 0;
}
// ========== lidar state info =========
- if (Sent_dist < 300 && Sent_dist < 0){
+ if (Sent_dist < 300 && Sent_dist > 0){
receivedData.DebugM = "found b";
}else{
receivedData.DebugM = "no b";
@@ -284,7 +285,7 @@ void print_received_Data(){
Serial.println(Rec_dir3);
Serial.print("Rec_dir4:");
Serial.println(Rec_dir4);
-Serial.println("_________________________");
+Serial.println("_______________________");
}
print_count +=1;
}
@@ -316,4 +317,4 @@ void send_message(){
}
//-------------------------------------------------------------------------------------
delay(50); // delay 50 ms after send out the message
-}
+}
diff --git a/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.cpp b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7a90e33a9ea7f73c7ad525b0919f9c69b4928fda
--- /dev/null
+++ b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.cpp
@@ -0,0 +1,83 @@
+#include <math.h>
+
+//Source: http://www.easyrgb.com/index.php?X=MATH&H=01#text1
+void lab2rgb( float l_s, float a_s, float b_s, float& R, float& G, float& B )
+{
+ float var_Y = ( l_s + 16. ) / 116.;
+ float var_X = a_s / 500. + var_Y;
+ float var_Z = var_Y - b_s / 200.;
+
+ if ( pow(var_Y,3) > 0.008856 ) var_Y = pow(var_Y,3);
+ else var_Y = ( var_Y - 16. / 116. ) / 7.787;
+ if ( pow(var_X,3) > 0.008856 ) var_X = pow(var_X,3);
+ else var_X = ( var_X - 16. / 116. ) / 7.787;
+ if ( pow(var_Z,3) > 0.008856 ) var_Z = pow(var_Z,3);
+ else var_Z = ( var_Z - 16. / 116. ) / 7.787;
+
+ float X = 95.047 * var_X ; //ref_X = 95.047 Observer= 2°, Illuminant= D65
+ float Y = 100.000 * var_Y ; //ref_Y = 100.000
+ float Z = 108.883 * var_Z ; //ref_Z = 108.883
+
+
+ var_X = X / 100. ; //X from 0 to 95.047 (Observer = 2°, Illuminant = D65)
+ var_Y = Y / 100. ; //Y from 0 to 100.000
+ var_Z = Z / 100. ; //Z from 0 to 108.883
+
+ float var_R = var_X * 3.2406 + var_Y * -1.5372 + var_Z * -0.4986;
+ float var_G = var_X * -0.9689 + var_Y * 1.8758 + var_Z * 0.0415;
+ float var_B = var_X * 0.0557 + var_Y * -0.2040 + var_Z * 1.0570;
+
+ if ( var_R > 0.0031308 ) var_R = 1.055 * pow(var_R , ( 1 / 2.4 )) - 0.055;
+ else var_R = 12.92 * var_R;
+ if ( var_G > 0.0031308 ) var_G = 1.055 * pow(var_G , ( 1 / 2.4 ) ) - 0.055;
+ else var_G = 12.92 * var_G;
+ if ( var_B > 0.0031308 ) var_B = 1.055 * pow( var_B , ( 1 / 2.4 ) ) - 0.055;
+ else var_B = 12.92 * var_B;
+
+ R = var_R * 255.;
+ G = var_G * 255.;
+ B = var_B * 255.;
+
+}
+
+
+
+
+
+//old codes
+
+// motorVertical->setSpeed(0);
+// motorVertical->run(FORWARD);
+// // turn on motor
+// motorVertical->run(RELEASE);
+// Serial.println(SEEKING_SPEED);
+// motorLeft->setSpeed(SEEKING_SPEED);
+// motorLeft->run(FORWARD);
+// motorRight->setSpeed(SEEKING_SPEED);
+
+// if (abs(Outputx) < 125){
+// motorRight->setSpeed(BASE_SPEED - Outputx);
+// motorRight->run(BACKWARD);
+// motorLeft->setSpeed(BASE_SPEED + Outputx); //this need to be higher
+// motorLeft->run(BACKWARD);
+// } else if (Outputx >= 125) { //propeller push in opposite direction, moving to the right
+// motorRight->setSpeed(Outputx);
+// motorRight->run(FORWARD);
+// motorLeft->setSpeed(255); //this need to be higher
+// motorLeft->run(BACKWARD);
+// } else {
+// motorRight->setSpeed(255);
+// motorRight->run(BACKWARD);
+// motorLeft->setSpeed(Outputx); //this need to be higher
+// motorLeft->run(FORWARD);
+// }
+
+// motorVertical->setSpeed(0);
+// motorVertical->run(FORWARD);
+// // turn on motor
+// motorVertical->run(RELEASE);
+// motorLeft->setSpeed(0);
+// motorLeft->run(FORWARD);
+// motorRight->setSpeed(0);
+// motorRight->run(BACKWARD);
+// motorRight->run(BACKWARD);
diff --git a/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.h b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.h
new file mode 100644
index 0000000000000000000000000000000000000000..a1ad9adbad7124479fd8d5168a7b1b24a915521c
--- /dev/null
+++ b/Code/Control/Laptop_Code/Previous_low_level/ESP32_slave/utilities.h
@@ -0,0 +1,8 @@
+#ifndef UTI_H
+#define UTI_H
+
+void lab2rgb( float l_s, float a_s, float b_s, float& R, float& G, float& B );
+void translateCodetoThreshold(int code);
+
+
+#endif
diff --git a/Code/Control/Laptop_Code/README.md b/Code/Control/Laptop_Code/README.md
index 538cd96562a819330f68bb4744d71b6897837b42..a19c8a625086acc4cbcf3c094426f04b6ba1f620 100644
--- a/Code/Control/Laptop_Code/README.md
+++ b/Code/Control/Laptop_Code/README.md
@@ -1,7 +1,10 @@
-Working on it
+[Progress] The overall logic is clear and tested!
-
+What to do next in python main:
-Main_control()
-
-* to be continued
\ No newline at end of file
+- Have keyboard interruption in the python function
+ - To tweak PID value while running the program
+ - To stop the balloon from being crazy
+- The meter version April tag detection is still problematic so we need to tune that
+- Use field test experiment to test PID
+-
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/Test_keyboard/keyboard.py b/Code/Control/Laptop_Code/Test_keyboard/keyboard.py
new file mode 100644
index 0000000000000000000000000000000000000000..3e3f90dd9f57c9d900feea778072bfba92c54392
--- /dev/null
+++ b/Code/Control/Laptop_Code/Test_keyboard/keyboard.py
@@ -0,0 +1,162 @@
+import cv2
+import pygame
+
+kpx,kix,kdx = 1,0.2,0.5
+
+
+def auto_control():
+ print("auto_control function")
+
+def stop_all():
+ print("stop_all function")
+
+def manual_command(val1,val2,val3,val4,sign1,sign2,sign3,sign4):
+ pwm1 = val1
+ pwm2 = val2
+ pwm3 = val3
+ pwm4 = val4
+ dir1 = sign1
+ dir2 = sign2
+ dir3 = sign3
+ dir4 = sign4
+ return pwm1,pwm2,pwm3,pwm4,dir1,dir2,dir3,dir4
+
+def manual_control(Ctl_com):
+
+ if get_key("w"):
+ val = start_speed
+ Ctl_com = manual_command(val,val, 0, 0, "+","+","+","+")
+
+ elif get_key("s"):
+ val = start_speed
+ Ctl_com = manual_command(val, val, 0, 0, "-", "-", "+", "+")
+
+ if get_key("UP"):
+ val = start_speed
+
+ Ctl_com = manual_command(0,0, val, val, "+","+","+","+")
+ elif get_key("DOWN"):
+ val = start_speed
+ Ctl_com = manual_command(0,0, val, val, "+","+","-","-")
+
+ elif get_key("LEFT"):
+ val = start_speed
+ Ctl_com = manual_command(0,0, val, val, "+","+","-","+")
+
+ elif get_key("RIGHT"):
+ val = start_speed
+ Ctl_com = manual_command(0,0, val, val, "+","+","+","-")
+
+ return Ctl_com
+ # print("manual_control function")
+
+def decode_ctl(Ctl_com):
+ pwm1 = Ctl_com[0]
+ pwm2 = Ctl_com[1]
+ pwm3 = Ctl_com[2]
+ pwm4 = Ctl_com[3]
+ dir1 = Ctl_com[4]
+ dir2 = Ctl_com[5]
+ dir3 = Ctl_com[6]
+ dir4 = Ctl_com[7]
+ return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+def dynamic_variable(str_name_v):
+
+ global kpx,kix,kdx,start_speed
+ if str_name_v == "kpx":
+ kpx = input("Enter your value: ")
+ print("kpx:{}".format(kpx))
+ elif str_name_v == "kix":
+ kix = input("Enter your value: ")
+ print("kix:{}".format(kix))
+ elif str_name_v == "kdx":
+ kdx = input("Enter your value: ")
+ print("kdx:{}".format(kdx))
+ elif str_name_v == "stsp":
+ start_speed = input("Enter your value: ")
+ print("start_speed:{}".format(start_speed))
+
+def variables_change_once():
+
+ str_name = input("Enter your variable: ")
+ dynamic_variable(str_name)
+
+ # print("variables_change function")
+
+def init():
+ pygame.init()
+ win= pygame.display.set_mode((400,400))
+
+def keyboard_stop(flag_s,print_count_s):
+
+ if get_key('q'):
+ flag_s = 0
+ print_count_s = 1
+ return flag_s,print_count_s
+
+def get_key(keyname):
+ ans = False
+ for eve in pygame.event.get(): pass
+ keyInput = pygame.key.get_pressed()
+ myKey = getattr(pygame,'K_{}'.format(keyname))
+
+ if keyInput[myKey]:
+ ans = True
+ pygame.display.update()
+ return ans
+
+if __name__ == '__main__':
+ global start_speed
+ start_speed = 70
+ Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
+ flag = 0
+ print_count = 1
+ init()
+ while True:
+
+ if get_key('a'):
+ flag = 1
+ while (flag == 1):
+ auto_control()
+
+ cap = cv2.VideoCapture(0)
+ ret, frame = cap.read()
+ if not ret:
+ continue
+ cv2.imshow("image", frame)
+
+ flag, print_count = keyboard_stop(flag,print_count)
+ if flag == 0:
+ cap.release()
+ cv2.destroyAllWindows()
+
+ elif get_key('s'):
+ stop_all()
+ print("stop all motors")
+
+ elif get_key('m'):
+ flag = 2
+ while (flag == 2):
+ Ctl_command = manual_control(Ctl_com)
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = decode_ctl(Ctl_command)
+ print("Ctl_com:{},{},{},{},{},{},{},{}".format(pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4))
+ flag, print_count = keyboard_stop(flag,print_count)
+
+ elif get_key('v'):
+ flag = 3
+ while (flag == 3):
+ variables_change_once()
+ flag = 0
+ print_count = 1
+ # flag, print_count = keyboard_stop(flag,print_count)
+
+ elif get_key('k'): # kill the program
+ break
+
+ if print_count is not 0:
+ print("No subsystem is running")
+ print("kpx:{}".format(kpx))
+ print("kix:{}".format(kix))
+ print("kdx:{}".format(kdx))
+ print_count = 0
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/inputs.py b/Code/Control/Laptop_Code/inputs.py
deleted file mode 100644
index 6d4cf404632b89e9702c7b090aa8a9279c42ecbe..0000000000000000000000000000000000000000
--- a/Code/Control/Laptop_Code/inputs.py
+++ /dev/null
@@ -1,492 +0,0 @@
-def formatInput(inp):
- tx, ty, tz, rx, ry, rz, d_lidar, debug = inp
- t = [tx, ty, tz]
- r = [rx, ry, rz]
- return [t, r, d_lidar, debug]
-
-#include <esp_now.h>
-#include <WiFi.h>
-#include <SparkFun_VL53L1X.h>
-#include <Wire.h>
-#include <Adafruit_MotorShield.h>
-#include "LedPanel.h"
-#include <Arduino.h>
-#include <string>
-#include <vector>
-#include "Camera.h"
-#include <PID_v1.h>
-#include "utilities.h"
-
-# LIDAR
-SFEVL53L1X distanceSensor;
-budgetIndex = 4;
-budgetValue[7] = {15, 20, 33, 50, 100, 200, 500};
-LED = LED_BUILTIN;
-
-double Inputx, Outputx;
-double Inputy, Outputy;
-
-
-broadcastAddress = [0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C] #4
-success = ""
-
-sentDebugM = ""
-count = 0
-count_var = 0
-print_count = 0
-change_count = 0
-
-strData = "";
-valData = 0.0;
-queData = "";
-Kpx = 2.0, Kix = 0.1, Kdx = 0.25;
-Kpy = 1.0, Kiy = 0.1, Kdy = 0.25;
-g1 = 0, g2 = 1, g3 = 2;
-goal_id = [g1, g2, g3]
-goal = [0,1,2]
-gbc = 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
-char newPTCL = '1';
-int pau = 0;
-int displayTracking = 0;
-
-// =============================== All the setup ===========================================
-//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);
-// ========================== Motor part ====================================
-// Create the motor shield object with the default I2C address
-Adafruit_MotorShield AFMS = Adafruit_MotorShield();
-// changed
-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
-// ========================== PID part ====================================
-// Setup PID
-PID PID_x(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT);
-PID PID_y(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT);
-
-//-------------------------------------------------------------------------------------
-typedef struct struct_message {
- String StrD;
- double ValD;
- String DebugM;
- String QueM;
- String VarM;
-} struct_message;
-
-struct_message receivedData;
-//-------------------------------------------------------------------------------------
-// Callback when data is sent
-void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
- print("\r\nLast Packet Send Status:\t");
- print(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
-
- if (status ==0){
- success = "Delivery Success :)";
- }
- else{
- success = "Delivery Fail :(";
- }
-}
-
-# Callback when data is received
-def OnDataRecv(*mac, *incomingData, dataLen):
- memcpy(&receivedData, incomingData, sizeof(receivedData));
- print("\ndata:");
- print(receivedData.StrD);
- print(receivedData.ValD);
- print(receivedData.QueM);
-
- if (receivedData.QueM != "?"){
- valData = receivedData.ValD;
- }
-
- strData = receivedData.StrD;
- queData = receivedData.QueM;
-
- count = 0;
- count_var = 0;
- print_count=0;
- change_count = 0;
-
- print("queData:");
- print(queData);
-
-def ball_detect(x, y):
- if x == 0 and y == 0:
- return False
- else:
- return True
-
-void send_message_var_once(){
- if(count_var==0){
- send_message();
- count_var+=1;
- }
-}
-
-
-void send_message_once(){
- if(count==0){
- send_message();
- count+=1;
- receivedData.DebugM = "";
- }
-}
-
-def sendMessage(message):
- esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &receivedData, sizeof(receivedData))
- // esp_now_send(RxMACaddress, (uint8_t *) &sentData, sizeof(sentData));
- if result == ESP_OK:
- print("Sent with success")
- else:
- print("Error sending the data")
- delay(100);
-
-def ChangeVariables():
- # PID
- Kpx = getDoubleVal(strData,"kpx",valData,Kpx);
- Kix = getDoubleVal(strData,"kix",valData,Kix);
- Kdx = getDoubleVal(strData,"kdx",valData,Kdx);
- Kpy = getDoubleVal(strData,"kpy",valData,Kpy);
- Kiy = getDoubleVal(strData,"kiy",valData,Kiy);
- Kdy = getDoubleVal(strData,"kdy",valData,Kdy);
-
- # Goal ID
- g1 = getIntVal(strData,"gda",valData,goal_id[0]);
- g2 = getIntVal(strData,"gdb",valData,goal_id[1]);
- g3 = getIntVal(strData,"gdc",valData,goal_id[2]);
- goal_id[0] = g1;
- goal_id[1] = g2;
- goal_id[2] = g3;
-
- # Color Threshold
- Lmin = getIntVal(strData,"tha",valData,threshold[0]);
- Lmax = getIntVal(strData,"thb",valData,threshold[1]);
- Amin = getIntVal(strData,"thc",valData,threshold[2]);
- Amax = getIntVal(strData,"thd",valData,threshold[3]);
- Bmin = getIntVal(strData,"the",valData,threshold[4]);
- Bmax = getIntVal(strData,"thf",valData,threshold[5]);
- threshold[0] = Lmin;
- threshold[1] = Lmax;
- threshold[2] = Amin;
- threshold[3] = Amax;
- threshold[4] = Bmin;
- threshold[5] = Bmax;
-
- # base_speed, seeking_speed, LIDAR_Thres
- base_speed = abs(getDoubleVal(strData,"bsp",valData,base_speed));
- seeking_speed = abs(getDoubleVal(strData,"ssp",valData,seeking_speed));
- LIDAR_Thres = getDoubleVal(strData,"lth",valData,LIDAR_Thres);
-
- # Green Ball x, y
- gbc = getDoubleVal(strData,"gbc",valData,gbc);
- gbx = gbc /1000000;
- gby = fmod(gbc,1000000)/1000;//(gbc.toInt() % 1000000)/1000;
- gbd = fmod(gbc,1000); // gbc.toInt() % 1000;
-
- # Setpoints
- Setpointx = getDoubleVal(strData,"spx",valData,Setpointx);
- Setpointy = getDoubleVal(strData,"spy",valData,Setpointy);
-
- debug_ball_cap = getDoubleVal(strData,"dbc",valData,debug_ball_cap);
-
-def getDoubleVal(checkData, Ans, val, ori_val):
- if checkData == Ans:
- strData = ""
- valData = 0.0
- return val
- else:
- return ori_val
-
-def getIntVal(checkData, Ans, val, ori_val):
- if checkData == Ans:
- strData = ""
- valData = 0.0
- return int(val)
- else:
- return ori_val
-
-def print_allvariables():
- if print_count <= 1:
- print("---------------")
- print("base speed:")
- print(base_speed)
- print("|")
- print("seeking speed:")
- print(seeking_speed);
- print("|");
- print("lidar thres:");
- print(LIDAR_Thres);
-
- print("threshold:");
- print(threshold[0]);
- print("|");
- print(threshold[1]);
- print("|");
- print(threshold[2]);
- print("|");
- print(threshold[3]);
- print("|");
- print(threshold[4]);
- print("|");
- print(threshold[5]);
-
- print("gid:");
- print(goal_id[0]);
- print(goal_id[1]);
- print(goal_id[2]);
-
- print("Kdx:");
- print(Kdx);
- print("|");
- print("Kix:");
- print(Kix);
- print("|");
- print("Kpx:");
- print(Kpx);
- print("|");
- print("Kdy:");
- print(Kdy);
- print("|");
- print("Kiy:");
- print(Kiy);
- print("|");
- print("Kpy:");
- print(Kpy);
- print("|");
-
- print("gbc:");
- print(gbc);
- print("|");
- print("gbx:");
- print(gbx);
- print("|");
- print("gby:");
- print(gby);
- print("gbd:");
- print(gbd);
-
- print("---------------------------\n");
- print_count +=1 ;
- }
-}
-
-void seeking(){
- moveVertical(0);
- moveHorizontal(seeking_speed, 0);
-}
-
-# vel value should be between -255 to 255 with positive values moving the blimp upward.
-def moveVertical(vel):
- if vel > 0: # up
- panel.singleLED(DEBUG_VERTICALSPEED, abs(vel), 0, 0)
- motorVertical_L->setSpeed(abs((int) vel))
- motorVertical_R->setSpeed(abs((int) vel))
- motorVertical_L->run(BACKWARD)
- motorVertical_R->run(FORWARD)
- elif vel < 0: # down
- panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(vel))
- motorVertical_L->setSpeed(abs((int) vel))
- motorVertical_R->setSpeed(abs((int) vel))
- motorVertical_L->run(FORWARD)
- motorVertical_R->run(BACKWARD)
- else:
- panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0)
- motorVertical_L->setSpeed(0)
- motorVertical_R->setSpeed(0)
-
-def moveHorizontal(vel_rot, vel_trn):
- lspeed = -1*vel_rot + vel_trn
- rspeed = 1*vel_rot + vel_trn
-
- if lspeed > 0:
- motorLeft->run(BACKWARD) # make it move forward
- else:
- motorLeft->run(FORWARD)
- if rspeed > 0:
- motorRight-> run(FORWARD)
- else:
- motorRight-> run(BACKWARD) # make it move backward
- displaySpeed(lspeed, rspeed)
- motorLeft->setSpeed(min(MAX_SPEED, abs(lspeed )))
- motorRight->setSpeed(min(MAX_SPEED, abs(rspeed)))
-
-def displaySpeed(lspeed, rspeed):
- if lspeed < 0:
- panel.singleLED(DEBUG_LSPEED, 0, 0, abs(lspeed))
- else:
- panel.singleLED(DEBUG_LSPEED, abs(lspeed), 0, 0)
-
- if rspeed < 0:
- panel.singleLED(DEBUG_RSPEED, 0, 0, abs(rspeed))
- else:
- panel.singleLED(DEBUG_RSPEED, abs(rspeed), 0, 0)
-
-//When using the camera to detect objects such as colored blobs or april tags. This function is
-//useful when only a single object is the target. The approximate position will be marked with an
-//led turning white in the 8*4 panel of the NeoPixel LED Panel. Therefore, this function can be
-//called assuming that you have created the LED panel object, which in this code is named "panel".
-//If the LED panel is called something else, just edit the code here
-void displayTrackedObject(int cx, int cy, int w_res, int h_res){
- int lednum = 0;
- int vertshift = 0;
- panel.resetPanel();
- lednum = cx/(w_res/8); //because lednum is an int, it will handle flooring the value
- vertshift = cy/(h_res/4);
- lednum = lednum + 8*vertshift;
- panel.singleLED(lednum, 10, 10 , 10);
-}
-
-//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
-//the new parameter with the capital letter of that specific parameter, each separated with a space.
-//Some of the valid msg examples are:
-// - "P0.02"
-// - "D1.23"
-// - "P0.14 D9.5"
-// - "P0.1 I0.1 D0.1"
-//
-//NOTE:
-// - the parameter doesn't have to be in order. You can do DI or IP in whichever order as long as it follows a valid double value
-// - while the function works if you passed a negative double, the PID controller will not and will produce error.
-// - suggested command character: 'T'[uning]
-def setPIDConstants(msg, &p_constant, &i_constant, &d_constant):
- new_p = Kpx
- new_i = Kix
- new_d = Kdx
-
- msgLen = msg.length()
- startpoint = 0
- endpoint = 0
-
- for i in range(0, msgLen):
- if msg[i] == 'P':
- startpoint = i + 1
- for j in range(i + 1, msgLen):
- if msg[j] == ' ':
- endpoint = j
- break
- else:
- endpoint = msgLen
- if endpoint > startpoint # check to see if it is a valid value
- new_p = msg.substring(startpoint, endpoint).toDouble()
-
- if msg[i] == 'I':
- startpoint = i + 1
- for j in range(i + 1, msgLen):
- if msg[j] == ' ':
- endpoint = j
- break
- else:
- endpoint = msgLen
- if endpoint > startpoint # check to see if it is a valid value
- new_i = msg.substring(startpoint, endpoint).toDouble()
-
- if msg[i] == 'D':
- startpoint = i + 1
- for j in range(i + 1, msgLen + 1):
- if msg[j] == ' ' or msg[j] == '\0':
- endpoint = j
- break
- else:
- endpoint = msgLen
- if endpoint > startpoint # check to see if it is a valid value
- new_d = msg.substring(startpoint, endpoint).toDouble()
-
- ## Debugging
- unsigned long t = millis();
-
- debugPIDConstants(DEBUG_KP, p_constant, new_p)
- debugPIDConstants(DEBUG_KI, i_constant, new_i)
- debugPIDConstants(DEBUG_KD, d_constant, new_d)
-
- p_constant = new_p
- i_constant = new_i
- d_constant = new_d
-
- while(millis() < t+2000): # debugging LED will light up for 2 seconds
- pass
-
- panel.singleLED(DEBUG_KP, 0, 0, 0)
- panel.singleLED(DEBUG_KI, 0, 0, 0)
- panel.singleLED(DEBUG_KD, 0, 0, 0)
-}
-
-def debugPIDConstants(lednum, oldval, newval):
- if newval > oldval:
- panel.singleLED(lednum, 0, 10, 0)
- elif newval < oldval:
- panel.singleLED(lednum, 10, 0, 0)
- else:
- panel.singleLED(lednum, 10, 10, 10)
-
-def loop(GB, T, R, d_LIDAR, Debug):
- if change_count == 0:
- if queData == "?":
- QueryVariables()
- else:
- ChangeVariables()
- change_count += 1
-
- panel.singleLED(DEBUG_BASESPEED, base_speed, base_speed, base_speed)
-
- ## Standby Mode
- panel.singleLED(DEBUG_STATE, 10, 0, 0)
-
- ## Goal Finder
- ID = -1;
- x, y = 0, 0
-
- receivedData.DebugM = String(d_LIDAR)
-
- goalDetect = 1
- if R[1] == 0 and R[2] == 0:
- goalDetect = 0
-
- # ballDetect = cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)
- ballDetect = ball_detect(GB[0], GB[1])
-
- ballCapture = lidar.ball_capture()
-
- if ballCapture == 1 or debug_ball_cap == 1: # Ball captured
- if goalDetect: # Goal detected
- panel.singleLED(DEBUG_STATE, 0, 10, 0);
- Inputx = T[0]/1.00;
- Inputy = T[1]/1.00;
- PID_x.Compute();
- PID_y.Compute();
- receivedData.DebugM = "det g mv2g";
- send_message_once();
- moveVertical(Outputy);
- moveHorizontal(Outputx, base_speed);
- else: # Goal not detected
- seeking();
- receivedData.DebugM = "seeking2";
- send_message_once();
- else: # Ball not captured
- if ballDetect: # Ball detected
- receivedData.DebugM = "catching b";
- send_message_once();
- Inputx = GB[0]/1.00
- Inputy = GB[1]/1.00
- PID_x.Compute()
- PID_y.Compute()
- moveVertical(Outputy)
- moveHorizontal(Outputx, base_speed)
- else: # Ball not detected
- seeking()
- receivedData.DebugM = "seeking1"
- send_message_once()
-
- print_allvariables()
-
- sendMessage(PWM, Dir)
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/main.py b/Code/Control/Laptop_Code/main.py
deleted file mode 100644
index 8ce272caf98ce987307a4adfa7ff4d324b3b786b..0000000000000000000000000000000000000000
--- a/Code/Control/Laptop_Code/main.py
+++ /dev/null
@@ -1,327 +0,0 @@
-import time
-import serial
-import ball_detection.ball_detection as ball_detection
-import simple_pid.PID as PID
-import timeit
-
-
-from constants import *
-
-# ========= Serial Port I/O ===========
-
-def serial_port_in(serial_port):
- '''
- Description:
- Take all ESP32_Master serial port's printIn and take all necessary input object
-
- Input:
- serial_port : serial.Serail object
-
- Output:
- tx, ty, tz, rx, ry, rz, LIDAR_dist, DebugM
- '''
-
- # DEBUG Verbose
- print("initiating one round of serial in ...")
-
- for i in range(7):
- line = serial_port.readline()
- val = int(line.decode())
-
- if i == 0:
- tx = val
- elif i == 1:
- ty = val
- elif i == 2:
- tz = val
- elif i == 3:
- rx = val
- elif i == 4:
- ry = val
- elif i == 5:
- rz = val
- elif i == 6:
- LIDAR_dist = val
-
- line = serial_port.readline()
- debugM = line.decode()
-
- # DEBUG Verbose
- print("tx:{}".format(tx))
- print("ty:{}".format(ty))
- print("tz:{}".format(tz))
- print("rx:{}".format(rx))
- print("ry:{}".format(ry))
- print("rz:{}".format(rz))
- print("dist:{}".format(LIDAR_dist))
- print(debugM)
-
- return tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM
-
-
-def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4):
- '''
- Description:
- Feed to ESP32_Master to send ESP32_Slave necessary information
- the format of sending is pwm are 3 digit space
-
- Input:
- serial_port : serial.Serail object
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : variables to send
-
- Output:
- None
- '''
-
- output_message = ''
-
- for pwm_itr in [pwm1, pwm2, pwm3, pwm4]:
- # print(pwm_itr)
- if len(str(pwm_itr)) == 2:
- output_message += '0'
- elif len(str(pwm_itr)) == 1:
- output_message += '00'
- output_message += str(pwm_itr)
- print(pwm_itr)
-
- output_message = output_message + dir1 + dir2 + dir3 + dir4 + '\n'
- print("serial out ...")
- print(output_message)
- serial_port.write(output_message.encode())
-
-
-# ====== supporting function in main control ====
-def ball_detect(gbx, gby):
- '''
- return True if green ball is detected
- '''
- if gbx == -1 and gby == -1:
- return False
- else:
- return True
-
-def goal_detect(tx,ty):
- '''
- return True if April Tag is detected
- '''
- if tx == 0 and ty == 0:
- return False
- else:
- return True
-
-def ball_capture(LIDAR_dist):
- '''
- return True if April Tag is detected
- '''
- if (LIDAR_dist < LIDAR_Thres) and (LIDAR_dist > 0): # Ball captured
- return True
- else:
- return False
-
-def stop_all():
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
- dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
- return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-def move2goal(tx, ty):
- """
- Description:
- Given the center of the AT tx, ty. Call PID control to output the blimp
- motor to manuver to the goal
-
- Input:
- tx : x component, center of April Tag
- ty : y component, center of Aprol Tag
-
- Output:
- pwm1, pwm2, pwm3, pwm4
- dir1, dir2, dir3, dir4
- """
- pass
-
- # return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-
-def seeking():
- """
- Description:
- By default, when there ball is not determined capture, the manuver of the
- motors to have it scan its surronding 360 degrees
-
- Input:
- none
-
- Output:
- pwm1, pwm2, pwm3, pwm4
- dir1, dir2, dir3, dir4
- """
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , seeking_speed , seeking_speed
- dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
-
- return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-
-
-def move2ball(gbx, gby, gb_dist):
- """
- Description:
- Given the center of x y dist of green ball detected. Call PID control to
- output the blimp motor to manuver to the green ball
-
- Input:
- gbx : x component, center of green ball
- gby : y component, center of green ball
- gb_dist : distance to green ball
-
- Output:
- pwm1, pwm2, pwm3, pwm4
- dir1, dir2, dir3, dir4
- """
- inputx = gbx / 1.00
- inputy = gby / 1.00
-
- setpoint_x = 400
- setpoint_y = 300 # ESP 32 Cam Center
-
- pid_x = PID(kpx,kix,kdx,setpoint=setpoint_x)
- pid_y = PID(kpy,kiy,kdy,setpoint=setpoint_y)
-
-
- pid_x.auto_mode = True
- pid_x.set_auto_mode(True, last_output=8.0)
- pid_x.output_limits = (-255,255)
- pid_y.output_limits = (-255,255)
-
-
- outputx = pid_x(inputx)
- outputy = pid_y(inputy)
-
- # vertical
- pwm1 = abs(outputy)
- pwm2 = abs(outputy)
-
- if(outputy > 0):
- dir1 = '+'
- dir2 = '+'
- else:
- dir1 = '-'
- dir2 = '-'
-
- # horizontal
- lspeed = -1 * outputx + base_speed
- rspeed = 1 * outputx + base_speed
- pwm3 = min( abs(lspeed), 255)
- pwm4 = min( abs(rspeed), 255)
- if (lspeed > 0):
- dir3 = '+'
- else:
- dir3 = '-'
-
- if (rspeed > 0):
- dir4 = '+'
- else:
- dir4 = '-'
-
-
- return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-
-
-
-# =========== main control ===========
-
-def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
- '''
- Description:
- Given green ball information and AT information, the main control logic
- to manuver the blimp motors
-
- Input:
- gbx, gby, gb_dist : green ball information
- tx, ty, tz, rx, ry, rz, LIDAR_dist : AirTag information
- debugM : Debug Message
-
- Output:
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
- '''
-
- # # placeholder
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
- dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
-
- ballDetect = ball_detect(gbx, gby)
- ballCapture = ball_capture(LIDAR_dist)
- goalDetect = goal_detect(tx,ty)
- # debug
- ballCapture = 0
- if ballCapture: # Ball captured
- if goalDetect: # Goal detected
- stop_all()
- #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
- else: # Goal not detected
- stop_all()
- #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
- else: # Ball not captured
- if ballDetect: # Ball detected
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
- else: # Ball not detected
- # stop_all()
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
-
- return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-
-
-# ===== Main Functions =====
-
-if __name__ == '__main__':
- # =========== SET UP ============
- # Defining Variables for ESP 32 Serial I/O
- PORT = "COM6" # for Alienware
- serial_port = serial.Serial(PORT, 115200)
- serial_port.close()
- serial_port.open()
-
- # Weit Time
- waitTime = 0.05
-
- # Loading the PyTorch ML model for ball detection
- model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
- #model = ball_detection.returnModel(modelAction, device, trainLoc, labelSet, modelLoc, modelFile)
-
-
- # =========== DECLARE VARIABLES ===========
- # ESP CAM In
- gbx, gby = -1, -1 # by default (-1 means no found green ball)
- gb_dist = -1 # by default (-1 means no found green ball)
-
- # Serial Port In
- tx, ty, tz = 0, 0, 0 # by default (0 means no found AirTag)
- rx, ry, rz = 0, 0, 0
- LIDAR_dist = 0
- debugM = 'Testing'
-
- # Serial Port Out
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
- dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
-
- # =========== LOOP FOREVER===========
- # ESP32_SLAVE Talk First
- gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
-
- while True:
- # ===== STEP 1: TAKE ALL INPUT =====
- gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
- line = serial_port.readline()
-
- if line == b'SERIAL_IN_START\r\n':
- tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
- print("gbx,gby:{},{}".format(gbx,gby))
-
- # ===== STEP 2: MAIN CONTROL LOOP =====
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
-
- # ===== STEP 3: FEED ALL OUTPUT =====
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
-
- time.sleep(waitTime)
diff --git a/Code/Control/Laptop_Code/NEW_main_zhiying_catchBall_test_2in1.py b/Code/Control/Laptop_Code/main_backup/NEW_main_zhiying_catchBall_test_2in1.py
similarity index 95%
rename from Code/Control/Laptop_Code/NEW_main_zhiying_catchBall_test_2in1.py
rename to Code/Control/Laptop_Code/main_backup/NEW_main_zhiying_catchBall_test_2in1.py
index ec638f1ae93576716c31dc69902ae93e097cbf6a..8eda29a48376484ef80c46a181726030e9941acd 100644
--- a/Code/Control/Laptop_Code/NEW_main_zhiying_catchBall_test_2in1.py
+++ b/Code/Control/Laptop_Code/main_backup/NEW_main_zhiying_catchBall_test_2in1.py
@@ -4,7 +4,6 @@ import ball_detection.ball_detection as ball_detection
import simple_pid.PID as PID
import timeit
-
from constants import *
# ========= Serial Port I/O ===========
@@ -72,7 +71,6 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
Output:
None
'''
-
output_message = ''
for pwm_itr in [pwm1, pwm2, pwm3, pwm4]:
@@ -90,7 +88,7 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
serial_port.write(output_message.encode())
-# ====== supporting function in main control ====
+# ====== Logic-directing Functions ====
def ball_detect(gbx, gby):
'''
return True if green ball is detected
@@ -156,6 +154,7 @@ def seeking():
dir1, dir2, dir3, dir4
"""
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , seeking_speed , seeking_speed
+ # pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 70, 70
dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
@@ -176,22 +175,26 @@ def move2ball(gbx, gby, gb_dist):
pwm1, pwm2, pwm3, pwm4
dir1, dir2, dir3, dir4
"""
+ # base_speed = 70
+
+ # kdx,kix,kpx = 2,0.1,0.25
+ # kdy,kiy,kpy = 1,0.1,0.25
+
inputx = gbx / 1.00
inputy = gby / 1.00
+ # ESP-Cam Center
setpoint_x = 400
- setpoint_y = 300 # ESP 32 Cam Center
-
- pid_x = PID(kpx,kix,kdx,setpoint=setpoint_x)
- pid_y = PID(kpy,kiy,kdy,setpoint=setpoint_y)
+ setpoint_y = 300
+ pid_x = PID(kpx, kix, kdx, setpoint = setpoint_x)
+ pid_y = PID(kpy, kiy, kdy, setpoint = setpoint_y)
pid_x.auto_mode = True
- pid_x.set_auto_mode(True, last_output=8.0)
+ pid_x.set_auto_mode(True, last_output = 8.0)
pid_x.output_limits = (-255,255)
pid_y.output_limits = (-255,255)
-
-
+
outputx = pid_x(inputx)
outputy = pid_y(inputy)
@@ -215,19 +218,15 @@ def move2ball(gbx, gby, gb_dist):
dir3 = '+'
else:
dir3 = '-'
-
if (rspeed > 0):
dir4 = '+'
else:
dir4 = '-'
-
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-
# =========== main control ===========
-
def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
'''
Description:
@@ -242,8 +241,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
Output:
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
'''
-
- # # placeholder
+ # placeholder
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
@@ -269,9 +267,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-# ===== Main Functions =====
-
+# ===== Main Function =====
if __name__ == '__main__':
# =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O
@@ -303,12 +299,12 @@ if __name__ == '__main__':
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
- # =========== LOOP FOREVER===========
# ESP32_SLAVE Talk First
gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+ # =========== LOOP FOREVER===========
while True:
# ===== STEP 1: TAKE ALL INPUT =====
gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
diff --git a/Code/Control/Laptop_Code/main_zhiying_catchBall_test_2in1.py b/Code/Control/Laptop_Code/main_backup/main_zhiying_catchBall_test_2in1.py
similarity index 96%
rename from Code/Control/Laptop_Code/main_zhiying_catchBall_test_2in1.py
rename to Code/Control/Laptop_Code/main_backup/main_zhiying_catchBall_test_2in1.py
index 175bdc22392b37dd7555134827dd3622cdcf5b62..1351ed3f4e4ee28a42c883b69fe7eeaa9381081c 100644
--- a/Code/Control/Laptop_Code/main_zhiying_catchBall_test_2in1.py
+++ b/Code/Control/Laptop_Code/main_backup/main_zhiying_catchBall_test_2in1.py
@@ -1,371 +1,371 @@
-import time
-import serial
-import ball_detection.ball_detection as ball_detection
-import simple_pid.PID as PID
-import timeit
-
-
-from constants import *
-
-# ========= Serial Port I/O ===========
-
-def serial_port_in(serial_port):
- '''
- Description:
- Take all ESP32_Master serial port's printIn and take all necessary input object
-
- Input:
- serial_port : serial.Serail object
-
- Output:
- tx, ty, tz, rx, ry, rz, LIDAR_dist, DebugM
- '''
-
- # DEBUG Verbose
- print("initiating one round of serial in ...")
-
- for i in range(7):
- line = serial_port.readline()
- val = int(line.decode())
-
- if i == 0:
- tx = val
- elif i == 1:
- ty = val
- elif i == 2:
- tz = val
- elif i == 3:
- rx = val
- elif i == 4:
- ry = val
- elif i == 5:
- rz = val
- elif i == 6:
- LIDAR_dist = val
-
- line = serial_port.readline()
- debugM = line.decode()
-
- # DEBUG Verbose
- print("tx:{}".format(tx))
- print("ty:{}".format(ty))
- print("tz:{}".format(tz))
- print("rx:{}".format(rx))
- print("ry:{}".format(ry))
- print("rz:{}".format(rz))
- print("dist:{}".format(LIDAR_dist))
- print(debugM)
-
- return tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM
-
-
-def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4):
- '''
- Description:
- Feed to ESP32_Master to send ESP32_Slave necessary information
- the format of sending is pwm are 3 digit space
-
- Input:
- serial_port : serial.Serail object
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : variables to send
-
- Output:
- None
- '''
-
- output_message = ''
-
- for pwm_itr in [pwm1, pwm2, pwm3, pwm4]:
- # print(pwm_itr)
- if len(str(pwm_itr)) == 2:
- output_message += '0'
- elif len(str(pwm_itr)) == 1:
- output_message += '00'
- output_message += str(pwm_itr)
- print(pwm_itr)
-
- output_message = output_message + dir1 + dir2 + dir3 + dir4 + '\n'
- print("serial out ...")
- print(output_message)
- serial_port.write(output_message.encode())
-
- # DEBUG Verbose
-
-
-
-
-
-# ====== supporting function in main control ====
-def ball_detect(gbx, gby):
- '''
- return True if green ball is detected
- '''
- if gbx == -1 and gby == -1:
- return False
- else:
- return True
-
-def goal_detect(tx,ty):
- '''
- return True if April Tag is detected
- '''
- if tx == 0 and ty == 0:
- return False
- else:
- return True
-
-def ball_capture(LIDAR_dist):
- '''
- return True if April Tag is detected
- '''
- if (LIDAR_dist < LIDAR_Thres) and (LIDAR_dist > 0): # Ball captured
- return True
- else:
- return False
-
-def stop_all():
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
- dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
- return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-def move2goal(tx, ty):
- """
- Description:
- Given the center of the AT tx, ty. Call PID control to output the blimp
- motor to manuver to the goal
-
- Input:
- tx : x component, center of April Tag
- ty : y component, center of Aprol Tag
-
- Output:
- pwm1, pwm2, pwm3, pwm4
- dir1, dir2, dir3, dir4
- """
- pass
-
- # return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-
-def seeking():
- """
- Description:
- By default, when there ball is not determined capture, the manuver of the
- motors to have it scan its surronding 360 degrees
-
- Input:
- none
-
- Output:
- pwm1, pwm2, pwm3, pwm4
- dir1, dir2, dir3, dir4
- """
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , seeking_speed , seeking_speed
- dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
-
- return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-
-
-def move2ball(gbx, gby, gb_dist):
- """
- Description:
- Given the center of x y dist of green ball detected. Call PID control to
- output the blimp motor to manuver to the green ball
-
- Input:
- gbx : x component, center of green ball
- gby : y component, center of green ball
- gb_dist : distance to green ball
-
- Output:
- pwm1, pwm2, pwm3, pwm4
- dir1, dir2, dir3, dir4
- """
- inputx = gbx / 1.00
- inputy = gby / 1.00
-
- setpoint_x = 400
- setpoint_y = 300 # ESP 32 Cam Center
-
- pid_x = PID(kpx,kix,kdx,setpoint=setpoint_x)
- pid_y = PID(kpy,kiy,kdy,setpoint=setpoint_y)
-
-
- pid_x.auto_mode = True
- pid_x.set_auto_mode(True, last_output=8.0)
- pid_x.output_limits = (-255,255)
- pid_y.output_limits = (-255,255)
-
-
- outputx = pid_x(inputx)
- outputy = pid_y(inputy)
-
- # vertical
- pwm1 = abs(outputy)
- pwm2 = abs(outputy)
-
- if(outputy > 0):
- dir1 = '+'
- dir2 = '+'
- else:
- dir1 = '-'
- dir2 = '-'
-
- # horizontal
- lspeed = -1 * outputx + base_speed
- rspeed = 1 * outputx + base_speed
- pwm3 = min( abs(lspeed), 255)
- pwm4 = min( abs(rspeed), 255)
- if (lspeed > 0):
- dir3 = '+'
- else:
- dir3 = '-'
-
- if (rspeed > 0):
- dir4 = '+'
- else:
- dir4 = '-'
-
-
- return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-
-
-
-# =========== main control ===========
-
-def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
- '''
- Description:
- Given green ball information and AT information, the main control logic
- to manuver the blimp motors
-
- Input:
- gbx, gby, gb_dist : green ball information
- tx, ty, tz, rx, ry, rz, LIDAR_dist : AirTag information
- debugM : Debug Message
-
- Output:
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
- '''
-
- # placeholder
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
- dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
-
- ballDetect = ball_detect(gbx, gby)
- ballCapture = ball_capture(LIDAR_dist)
- goalDetect = goal_detect(tx,ty)
- # debug
- ballCapture = 0
- if ballCapture: # Ball captured
- if goalDetect: # Goal detected
- stop_all()
- #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
- else: # Goal not detected
- stop_all()
- #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
- else: # Ball not captured
- if ballDetect: # Ball detected
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
- else: # Ball not detected
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
-
- return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-
-
-# ===== Main Functions =====
-
-if __name__ == '__main__':
- # =========== SET UP ============
- # Defining Variables for ESP 32 Serial I/O
- PORT = "COM6" # for Alienware
- serial_port = serial.Serial(PORT, 115200)
- serial_port.close()
- serial_port.open()
-
- # Weit Time
- waitTime = 0.1
-
- # Loading the PyTorch ML model for ball detection
- model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
- #model = ball_detection.returnModel(modelAction, device, trainLoc, labelSet, modelLoc, modelFile)
-
-
- # =========== DECLARE VARIABLES ===========
- # ESP CAM In
- gbx, gby = -1, -1 # by default (-1 means no found green ball)
- gb_dist = -1 # by default (-1 means no found green ball)
-
- # Serial Port In
- tx, ty, tz = 0, 0, 0 # by default (0 means no found AirTag)
- rx, ry, rz = 0, 0, 0
- LIDAR_dist = 0
- debugM = 'Testing'
-
- # Serial Port Out
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
- dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
-
- count = 0
- flg = 0
- # =========== LOOP FOREVER===========
- while True:
-
- gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
- # print("gbx,gby:{},{}".format(gbx,gby))
- # tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
-
- # ===== STEP 2: MAIN CONTROL LOOP =====
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
-
- # ===== STEP 3: FEED ALL OUTPUT =====
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
-
- time.sleep(0.1)
-
- """
- line = serial_port.readline()
- if line == b'SERIAL_IN_START\r\n':
- # start = time.time()
- # print('I have been there')
- # ===== STEP 1: TAKE ALL INPUT =====
- gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
- # print("gbx,gby:{},{}".format(gbx,gby))
- tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
-
- # ===== STEP 2: MAIN CONTROL LOOP =====
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
-
- # ===== STEP 3: FEED ALL OUTPUT =====
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
-
-
- # end = time.time()
- # print("time:")
- # print(end - start)
- if count == 0:
- # first time calling (call once)
- print("bbb")
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
- count +=1
- """
-
- """
- if flg == 1:
- print("1")
- output_message = '100100000000+-++' + '\n'
- serial_port.write(output_message.encode())
-
- flg = 0
- time.sleep(0.3)
- elif flg == 0:
- print("2")
- output_message = '000000000000+-++' + '\n'
- serial_port.write(output_message.encode())
- flg = 1
- time.sleep(0.3)
- """
-
- # time.sleep(waitTime)
+import time
+import serial
+import ball_detection.ball_detection as ball_detection
+import simple_pid.PID as PID
+import timeit
+
+
+from constants import *
+
+# ========= Serial Port I/O ===========
+
+def serial_port_in(serial_port):
+ '''
+ Description:
+ Take all ESP32_Master serial port's printIn and take all necessary input object
+
+ Input:
+ serial_port : serial.Serail object
+
+ Output:
+ tx, ty, tz, rx, ry, rz, LIDAR_dist, DebugM
+ '''
+
+ # DEBUG Verbose
+ print("initiating one round of serial in ...")
+
+ for i in range(7):
+ line = serial_port.readline()
+ val = int(line.decode())
+
+ if i == 0:
+ tx = val
+ elif i == 1:
+ ty = val
+ elif i == 2:
+ tz = val
+ elif i == 3:
+ rx = val
+ elif i == 4:
+ ry = val
+ elif i == 5:
+ rz = val
+ elif i == 6:
+ LIDAR_dist = val
+
+ line = serial_port.readline()
+ debugM = line.decode()
+
+ # DEBUG Verbose
+ print("tx:{}".format(tx))
+ print("ty:{}".format(ty))
+ print("tz:{}".format(tz))
+ print("rx:{}".format(rx))
+ print("ry:{}".format(ry))
+ print("rz:{}".format(rz))
+ print("dist:{}".format(LIDAR_dist))
+ print(debugM)
+
+ return tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM
+
+
+def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4):
+ '''
+ Description:
+ Feed to ESP32_Master to send ESP32_Slave necessary information
+ the format of sending is pwm are 3 digit space
+
+ Input:
+ serial_port : serial.Serail object
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : variables to send
+
+ Output:
+ None
+ '''
+
+ output_message = ''
+
+ for pwm_itr in [pwm1, pwm2, pwm3, pwm4]:
+ # print(pwm_itr)
+ if len(str(pwm_itr)) == 2:
+ output_message += '0'
+ elif len(str(pwm_itr)) == 1:
+ output_message += '00'
+ output_message += str(pwm_itr)
+ print(pwm_itr)
+
+ output_message = output_message + dir1 + dir2 + dir3 + dir4 + '\n'
+ print("serial out ...")
+ print(output_message)
+ serial_port.write(output_message.encode())
+
+ # DEBUG Verbose
+
+
+
+
+
+# ====== supporting function in main control ====
+def ball_detect(gbx, gby):
+ '''
+ return True if green ball is detected
+ '''
+ if gbx == -1 and gby == -1:
+ return False
+ else:
+ return True
+
+def goal_detect(tx,ty):
+ '''
+ return True if April Tag is detected
+ '''
+ if tx == 0 and ty == 0:
+ return False
+ else:
+ return True
+
+def ball_capture(LIDAR_dist):
+ '''
+ return True if April Tag is detected
+ '''
+ if (LIDAR_dist < LIDAR_Thres) and (LIDAR_dist > 0): # Ball captured
+ return True
+ else:
+ return False
+
+def stop_all():
+ pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
+ dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
+ return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+def move2goal(tx, ty):
+ """
+ Description:
+ Given the center of the AT tx, ty. Call PID control to output the blimp
+ motor to manuver to the goal
+
+ Input:
+ tx : x component, center of April Tag
+ ty : y component, center of Aprol Tag
+
+ Output:
+ pwm1, pwm2, pwm3, pwm4
+ dir1, dir2, dir3, dir4
+ """
+ pass
+
+ # return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+
+def seeking():
+ """
+ Description:
+ By default, when there ball is not determined capture, the manuver of the
+ motors to have it scan its surronding 360 degrees
+
+ Input:
+ none
+
+ Output:
+ pwm1, pwm2, pwm3, pwm4
+ dir1, dir2, dir3, dir4
+ """
+ pwm1, pwm2, pwm3, pwm4 = 0 , 0 , seeking_speed , seeking_speed
+ dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
+
+ return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
+
+def move2ball(gbx, gby, gb_dist):
+ """
+ Description:
+ Given the center of x y dist of green ball detected. Call PID control to
+ output the blimp motor to manuver to the green ball
+
+ Input:
+ gbx : x component, center of green ball
+ gby : y component, center of green ball
+ gb_dist : distance to green ball
+
+ Output:
+ pwm1, pwm2, pwm3, pwm4
+ dir1, dir2, dir3, dir4
+ """
+ inputx = gbx / 1.00
+ inputy = gby / 1.00
+
+ setpoint_x = 400
+ setpoint_y = 300 # ESP 32 Cam Center
+
+ pid_x = PID(kpx,kix,kdx,setpoint=setpoint_x)
+ pid_y = PID(kpy,kiy,kdy,setpoint=setpoint_y)
+
+
+ pid_x.auto_mode = True
+ pid_x.set_auto_mode(True, last_output=8.0)
+ pid_x.output_limits = (-255,255)
+ pid_y.output_limits = (-255,255)
+
+
+ outputx = pid_x(inputx)
+ outputy = pid_y(inputy)
+
+ # vertical
+ pwm1 = abs(outputy)
+ pwm2 = abs(outputy)
+
+ if(outputy > 0):
+ dir1 = '+'
+ dir2 = '+'
+ else:
+ dir1 = '-'
+ dir2 = '-'
+
+ # horizontal
+ lspeed = -1 * outputx + base_speed
+ rspeed = 1 * outputx + base_speed
+ pwm3 = min( abs(lspeed), 255)
+ pwm4 = min( abs(rspeed), 255)
+ if (lspeed > 0):
+ dir3 = '+'
+ else:
+ dir3 = '-'
+
+ if (rspeed > 0):
+ dir4 = '+'
+ else:
+ dir4 = '-'
+
+
+ return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
+
+
+# =========== main control ===========
+
+def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
+ '''
+ Description:
+ Given green ball information and AT information, the main control logic
+ to manuver the blimp motors
+
+ Input:
+ gbx, gby, gb_dist : green ball information
+ tx, ty, tz, rx, ry, rz, LIDAR_dist : AirTag information
+ debugM : Debug Message
+
+ Output:
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
+ '''
+
+ # placeholder
+ pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
+ dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
+
+ ballDetect = ball_detect(gbx, gby)
+ ballCapture = ball_capture(LIDAR_dist)
+ goalDetect = goal_detect(tx,ty)
+ # debug
+ ballCapture = 0
+ if ballCapture: # Ball captured
+ if goalDetect: # Goal detected
+ stop_all()
+ #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
+ else: # Goal not detected
+ stop_all()
+ #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
+ else: # Ball not captured
+ if ballDetect: # Ball detected
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
+ else: # Ball not detected
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
+
+ return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+
+
+# ===== Main Functions =====
+
+if __name__ == '__main__':
+ # =========== SET UP ============
+ # Defining Variables for ESP 32 Serial I/O
+ PORT = "COM6" # for Alienware
+ serial_port = serial.Serial(PORT, 115200)
+ serial_port.close()
+ serial_port.open()
+
+ # Weit Time
+ waitTime = 0.1
+
+ # Loading the PyTorch ML model for ball detection
+ model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
+ #model = ball_detection.returnModel(modelAction, device, trainLoc, labelSet, modelLoc, modelFile)
+
+
+ # =========== DECLARE VARIABLES ===========
+ # ESP CAM In
+ gbx, gby = -1, -1 # by default (-1 means no found green ball)
+ gb_dist = -1 # by default (-1 means no found green ball)
+
+ # Serial Port In
+ tx, ty, tz = 0, 0, 0 # by default (0 means no found AirTag)
+ rx, ry, rz = 0, 0, 0
+ LIDAR_dist = 0
+ debugM = 'Testing'
+
+ # Serial Port Out
+ pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
+ dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
+
+ count = 0
+ flg = 0
+ # =========== LOOP FOREVER===========
+ while True:
+
+ gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ # print("gbx,gby:{},{}".format(gbx,gby))
+ # tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
+
+ # ===== STEP 2: MAIN CONTROL LOOP =====
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+
+ # ===== STEP 3: FEED ALL OUTPUT =====
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
+ time.sleep(0.1)
+
+ """
+ line = serial_port.readline()
+ if line == b'SERIAL_IN_START\r\n':
+ # start = time.time()
+ # print('I have been there')
+ # ===== STEP 1: TAKE ALL INPUT =====
+ gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ # print("gbx,gby:{},{}".format(gbx,gby))
+ tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
+
+ # ===== STEP 2: MAIN CONTROL LOOP =====
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+
+ # ===== STEP 3: FEED ALL OUTPUT =====
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
+
+ # end = time.time()
+ # print("time:")
+ # print(end - start)
+ if count == 0:
+ # first time calling (call once)
+ print("bbb")
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+ count +=1
+ """
+
+ """
+ if flg == 1:
+ print("1")
+ output_message = '100100000000+-++' + '\n'
+ serial_port.write(output_message.encode())
+
+ flg = 0
+ time.sleep(0.3)
+ elif flg == 0:
+ print("2")
+ output_message = '000000000000+-++' + '\n'
+ serial_port.write(output_message.encode())
+ flg = 1
+ time.sleep(0.3)
+ """
+
+ # time.sleep(waitTime)
diff --git a/Code/Control/Laptop_Code/main_zhiying_seeking_indv_test.py b/Code/Control/Laptop_Code/main_backup/main_zhiying_seeking_indv_test.py
similarity index 87%
rename from Code/Control/Laptop_Code/main_zhiying_seeking_indv_test.py
rename to Code/Control/Laptop_Code/main_backup/main_zhiying_seeking_indv_test.py
index cfd5faa11965e62d3cba89a51e6b9330c40220de..f415d104b90fd622e712cf5861c29f2a12d57929 100644
--- a/Code/Control/Laptop_Code/main_zhiying_seeking_indv_test.py
+++ b/Code/Control/Laptop_Code/main_backup/main_zhiying_seeking_indv_test.py
@@ -1,336 +1,325 @@
-import time
-import serial
-import ball_detection.ball_detection as ball_detection
-import simple_pid.PID as PID
-
-from constants import *
-
-# ========= Serial Port I/O ===========
-
-def serial_port_in(serial_port):
- '''
- Description:
- Take all ESP32_Master serial port's printIn and take all necessary input object
-
- Input:
- serial_port : serial.Serail object
-
- Output:
- tx, ty, tz, rx, ry, rz, LIDAR_dist, DebugM
- '''
-
- # DEBUG Verbose
- print("initiating one round of serial in ...")
-
- for i in range(7):
- line = serial_port.readline()
- val = int(line.decode())
-
- if i == 0:
- tx = val
- elif i == 1:
- ty = val
- elif i == 2:
- tz = val
- elif i == 3:
- rx = val
- elif i == 4:
- ry = val
- elif i == 5:
- rz = val
- elif i == 6:
- LIDAR_dist = val
-
- line = serial_port.readline()
- debugM = line.decode()
-
- # DEBUG Verbose
- print("tx:{}".format(tx))
- print("ty:{}".format(ty))
- print("tz:{}".format(tz))
- print("rx:{}".format(rx))
- print("ry:{}".format(ry))
- print("rz:{}".format(rz))
- print("dist:{}".format(LIDAR_dist))
- print(debugM)
-
- return tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM
-
-
-def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4):
- '''
- Description:
- Feed to ESP32_Master to send ESP32_Slave necessary information
- the format of sending is pwm are 3 digit space
-
- Input:
- serial_port : serial.Serail object
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : variables to send
-
- Output:
- None
- '''
-
- output_message = ''
-
- for pwm_itr in [pwm1, pwm2, pwm3, pwm4]:
- # print(pwm_itr)
- if len(str(pwm_itr)) == 2:
- output_message += '0'
- elif len(str(pwm_itr)) == 1:
- output_message += '00'
- output_message += str(pwm_itr)
- print(pwm_itr)
-
- output_message = output_message + dir1 + dir2 + dir3 + dir4 + '\n'
- serial_port.write(output_message.encode())
-
- # DEBUG Verbose
- print("serial out ...")
- print(output_message)
-
-
-
-
-# ====== supporting function in main control ====
-def ball_detect(gbx, gby):
- '''
- return True if green ball is detected
- '''
- if gbx == -1 and gby == -1:
- return False
- else:
- return True
-
-def goal_detect(tx,ty):
- '''
- return True if April Tag is detected
- '''
- if tx == 0 and ty == 0:
- return False
- else:
- return True
-
-def ball_capture(LIDAR_dist):
- '''
- return True if April Tag is detected
- '''
- if (LIDAR_dist < LIDAR_Thres) and (LIDAR_dist > 0): # Ball captured
- return True
- else:
- return False
-
-def stop_all():
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
- dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
- return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-def move2goal(tx, ty):
- """
- Description:
- Given the center of the AT tx, ty. Call PID control to output the blimp
- motor to manuver to the goal
-
- Input:
- tx : x component, center of April Tag
- ty : y component, center of Aprol Tag
-
- Output:
- pwm1, pwm2, pwm3, pwm4
- dir1, dir2, dir3, dir4
- """
- pass
-
- # return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-
-def seeking():
- """
- Description:
- By default, when there ball is not determined capture, the manuver of the
- motors to have it scan its surronding 360 degrees
-
- Input:
- none
-
- Output:
- pwm1, pwm2, pwm3, pwm4
- dir1, dir2, dir3, dir4
- """
- #pwm1, pwm2, pwm3, pwm4 = 0 , 0 , seeking_speed , seeking_speed
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 70, 70
- dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
-
- return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-
-
-def move2ball(gbx, gby, gb_dist):
- """
- Description:
- Given the center of x y dist of green ball detected. Call PID control to
- output the blimp motor to manuver to the green ball
-
- Input:
- gbx : x component, center of green ball
- gby : y component, center of green ball
- gb_dist : distance to green ball
-
- Output:
- pwm1, pwm2, pwm3, pwm4
- dir1, dir2, dir3, dir4
- """
- base_speed = 70
-
- kdx,kix,kpx = 2,0.1,0.25
- kdy,kiy,kpy = 1,0.1,0.25
-
- inputx = gbx / 1.00
- inputy = gby / 1.00
-
- setpoint_x = 400
- setpoint_y = 300 # ESP 32 Cam Center
-
- pid_x = PID(kdx,kix,kpx,setpoint=setpoint_x)
- pid_y = PID(kdy,kiy,kpy,setpoint=setpoint_y)
-
-
- pid_x.auto_mode = True
- pid_x.set_auto_mode(True, last_output=8.0)
- pid_x.output_limits = (-255,255)
- pid_y.output_limits = (-255,255)
-
-
- outputx = pid_x(inputx)
- outputy = pid_y(inputy)
-
- # vertical
- pwm1 = abs(outputy)
- pwm2 = abs(outputy)
-
- if(outputy > 0):
- dir1 = '+'
- dir2 = '+'
- else:
- dir1 = '-'
- dir2 = '-'
-
- # horizontal
- lspeed = -1 * outputx + base_speed
- rspeed = 1 * outputx + base_speed
- pwm3 = abs(lspeed)
- pwm4 = abs(rspeed)
- if (lspeed > 0):
- dir3 = '+'
- else:
- dir3 = '-'
-
- if (rspeed > 0):
- dir4 = '+'
- else:
- dir4 = '-'
-
-
- return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-
-
-
-# =========== main control ===========
-
-def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
- '''
- Description:
- Given green ball information and AT information, the main control logic
- to manuver the blimp motors
-
- Input:
- gbx, gby, gb_dist : green ball information
- tx, ty, tz, rx, ry, rz, LIDAR_dist : AirTag information
- debugM : Debug Message
-
- Output:
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
- '''
-
- # placeholder
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
- dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
-
- ballDetect = ball_detect(gbx, gby)
- ballCapture = ball_capture(LIDAR_dist)
- goalDetect = goal_detect(tx,ty)
- # debug
- ballCapture = 0
- if ballCapture: # Ball captured
- if goalDetect: # Goal detected
- stop_all()
- #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
- else: # Goal not detected
- stop_all()
- #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
- else: # Ball not captured
- if ballDetect: # Ball detected
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
- else: # Ball not detected
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
-
- return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-
-
-# ===== Main Functions =====
-
-if __name__ == '__main__':
- # =========== SET UP ============
- # Defining Variables for ESP 32 Serial I/O
- PORT = "COM6" # for Alienware
- serial_port = serial.Serial(PORT, 115200)
- serial_port.close()
- serial_port.open()
-
- # Weit Time
- waitTime = 0.10
-
- # Loading the PyTorch ML model for ball detection
- model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
- #model = ball_detection.returnModel(modelAction, device, trainLoc, labelSet, modelLoc, modelFile)
-
-
- # =========== DECLARE VARIABLES ===========
- # ESP CAM In
- gbx, gby = -1, -1 # by default (-1 means no found green ball)
- gb_dist = -1 # by default (-1 means no found green ball)
-
- # Serial Port In
- tx, ty, tz = 0, 0, 0 # by default (0 means no found AirTag)
- rx, ry, rz = 0, 0, 0
- LIDAR_dist = 0
- debugM = 'Testing'
-
- # Serial Port Out
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
- dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
-
- count = 0
- # =========== LOOP FOREVER===========
- while True:
- #gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
- print("gbx,gby:{},{}".format(gbx,gby))
-
- line = serial_port.readline()
- if line == b'SERIAL_IN_START\r\n':
- # ===== STEP 1: TAKE ALL INPUT =====
- tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
-
- # ===== STEP 2: MAIN CONTROL LOOP =====
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
-
- # ===== STEP 3: FEED ALL OUTPUT =====
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
-
- if count == 0:
- # first time calling (call once)
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
- # count +=1
-
- time.sleep(waitTime)
+import time
+import serial
+import ball_detection.ball_detection as ball_detection
+import simple_pid.PID as PID
+import timeit
+
+from constants import *
+
+# ========= Serial Port I/O ===========
+
+def serial_port_in(serial_port):
+ '''
+ Description:
+ Take all ESP32_Master serial port's printIn and take all necessary input object
+
+ Input:
+ serial_port : serial.Serail object
+
+ Output:
+ tx, ty, tz, rx, ry, rz, LIDAR_dist, DebugM
+ '''
+
+ # DEBUG Verbose
+ print("initiating one round of serial in ...")
+
+ for i in range(7):
+ line = serial_port.readline()
+ val = int(line.decode())
+
+ if i == 0:
+ tx = val
+ elif i == 1:
+ ty = val
+ elif i == 2:
+ tz = val
+ elif i == 3:
+ rx = val
+ elif i == 4:
+ ry = val
+ elif i == 5:
+ rz = val
+ elif i == 6:
+ LIDAR_dist = val
+
+ line = serial_port.readline()
+ debugM = line.decode()
+
+ # DEBUG Verbose
+ print("tx:{}".format(tx))
+ print("ty:{}".format(ty))
+ print("tz:{}".format(tz))
+ print("rx:{}".format(rx))
+ print("ry:{}".format(ry))
+ print("rz:{}".format(rz))
+ print("dist:{}".format(LIDAR_dist))
+ print(debugM)
+
+ return tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM
+
+
+def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4):
+ '''
+ Description:
+ Feed to ESP32_Master to send ESP32_Slave necessary information
+ the format of sending is pwm are 3 digit space
+
+ Input:
+ serial_port : serial.Serail object
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : variables to send
+
+ Output:
+ None
+ '''
+ output_message = ''
+
+ for pwm_itr in [pwm1, pwm2, pwm3, pwm4]:
+ # print(pwm_itr)
+ if len(str(pwm_itr)) == 2:
+ output_message += '0'
+ elif len(str(pwm_itr)) == 1:
+ output_message += '00'
+ output_message += str(pwm_itr)
+ print(pwm_itr)
+
+ output_message = output_message + dir1 + dir2 + dir3 + dir4 + '\n'
+ print("serial out ...")
+ print(output_message)
+ serial_port.write(output_message.encode())
+
+
+# ====== Logic-directing Functions ====
+def ball_detect(gbx, gby):
+ '''
+ return True if green ball is detected
+ '''
+ if gbx == -1 and gby == -1:
+ return False
+ else:
+ return True
+
+def goal_detect(tx,ty):
+ '''
+ return True if April Tag is detected
+ '''
+ if tx == 0 and ty == 0:
+ return False
+ else:
+ return True
+
+def ball_capture(LIDAR_dist):
+ '''
+ return True if April Tag is detected
+ '''
+ if (LIDAR_dist < LIDAR_Thres) and (LIDAR_dist > 0): # Ball captured
+ return True
+ else:
+ return False
+
+def stop_all():
+ pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
+ dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
+ return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+def move2goal(tx, ty):
+ """
+ Description:
+ Given the center of the AT tx, ty. Call PID control to output the blimp
+ motor to manuver to the goal
+
+ Input:
+ tx : x component, center of April Tag
+ ty : y component, center of Aprol Tag
+
+ Output:
+ pwm1, pwm2, pwm3, pwm4
+ dir1, dir2, dir3, dir4
+ """
+ pass
+
+ # return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+
+def seeking():
+ """
+ Description:
+ By default, when there ball is not determined capture, the manuver of the
+ motors to have it scan its surronding 360 degrees
+
+ Input:
+ none
+
+ Output:
+ pwm1, pwm2, pwm3, pwm4
+ dir1, dir2, dir3, dir4
+ """
+ # pwm1, pwm2, pwm3, pwm4 = 0 , 0 , seeking_speed , seeking_speed
+ pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 70, 70
+ dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
+
+ return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
+
+def move2ball(gbx, gby, gb_dist):
+ """
+ Description:
+ Given the center of x y dist of green ball detected. Call PID control to
+ output the blimp motor to manuver to the green ball
+
+ Input:
+ gbx : x component, center of green ball
+ gby : y component, center of green ball
+ gb_dist : distance to green ball
+
+ Output:
+ pwm1, pwm2, pwm3, pwm4
+ dir1, dir2, dir3, dir4
+ """
+ base_speed = 70
+
+ kdx,kix,kpx = 2,0.1,0.25
+ kdy,kiy,kpy = 1,0.1,0.25
+
+ inputx = gbx / 1.00
+ inputy = gby / 1.00
+
+ # ESP-Cam Center
+ setpoint_x = 400
+ setpoint_y = 300
+
+ pid_x = PID(kpx, kix, kdx, setpoint = setpoint_x)
+ pid_y = PID(kpy, kiy, kdy, setpoint = setpoint_y)
+
+ pid_x.auto_mode = True
+ pid_x.set_auto_mode(True, last_output = 8.0)
+ pid_x.output_limits = (-255,255)
+ pid_y.output_limits = (-255,255)
+
+ outputx = pid_x(inputx)
+ outputy = pid_y(inputy)
+
+ # Vertical
+ pwm1 = abs(outputy)
+ pwm2 = abs(outputy)
+
+ if(outputy > 0):
+ dir1 = '+'
+ dir2 = '+'
+ else:
+ dir1 = '-'
+ dir2 = '-'
+
+ # horizontal
+ lspeed = -1 * outputx + base_speed
+ rspeed = 1 * outputx + base_speed
+ pwm3 = abs(lspeed)
+ pwm4 = abs(rspeed)
+ if (lspeed > 0):
+ dir3 = '+'
+ else:
+ dir3 = '-'
+ if (rspeed > 0):
+ dir4 = '+'
+ else:
+ dir4 = '-'
+
+ return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
+
+# =========== Main Control ===========
+def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
+ '''
+ Description:
+ Given green ball information and AT information, the main control logic
+ to manuver the blimp motors
+
+ Input:
+ gbx, gby, gb_dist : green ball information
+ tx, ty, tz, rx, ry, rz, LIDAR_dist : AirTag information
+ debugM : Debug Message
+
+ Output:
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
+ '''
+ # placeholder
+ pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
+ dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
+
+ ballDetect = ball_detect(gbx, gby)
+ ballCapture = ball_capture(LIDAR_dist)
+ goalDetect = goal_detect(tx, ty)
+
+ ballCapture = 0 # debug
+ if ballCapture: # Ball captured
+ if goalDetect: # Goal detected
+ stop_all() # Debug
+ # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
+ else: # Goal not detected
+ stop_all() # Debug
+ # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
+ else: # Ball not captured
+ if ballDetect: # Ball detected
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
+ else: # Ball not detected
+ # stop_all() # Debug
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
+
+ return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+
+# ===== Main Function =====
+if __name__ == '__main__':
+ # =========== SET UP ============
+ # Defining Variables for ESP 32 Serial I/O
+ PORT = "COM6" # for Alienware
+ serial_port = serial.Serial(PORT, 115200)
+ serial_port.close()
+ serial_port.open()
+
+ # Wait Time
+ waitTime = 0.10
+
+ # Loading the PyTorch ML model for ball detection
+ model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
+ # model = ball_detection.returnModel(modelAction, device, trainLoc, labelSet, modelLoc, modelFile)
+
+
+ # =========== DECLARE VARIABLES ===========
+ # ESP CAM In
+ gbx, gby = -1, -1 # by default (-1 means no found green ball)
+ gb_dist = -1 # by default (-1 means no found green ball)
+
+ # Serial Port In
+ tx, ty, tz = 0, 0, 0 # by default (0 means no found AirTag)
+ rx, ry, rz = 0, 0, 0
+ LIDAR_dist = 0
+ debugM = 'Testing'
+
+ # Serial Port Out
+ pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
+ dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
+
+ count = 0
+ # =========== LOOP FOREVER===========
+ while True:
+ #gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ print("gbx,gby:{},{}".format(gbx,gby))
+
+ line = serial_port.readline()
+ if line == b'SERIAL_IN_START\r\n':
+ # ===== STEP 1: TAKE ALL INPUT =====
+ tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
+
+ # ===== STEP 2: MAIN CONTROL LOOP =====
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+
+ # ===== STEP 3: FEED ALL OUTPUT =====
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
+ if count == 0:
+ # first time calling (call once)
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+ # count +=1
+
+ time.sleep(waitTime)
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/main_integrated_test.py b/Code/Control/Laptop_Code/main_integrated_test.py
index 1f1efaec3e203ba0e8f06debd4dda15db9cd2a30..41dad51d6a4eb16cef6c6e9336366e9c0256ecd9 100644
--- a/Code/Control/Laptop_Code/main_integrated_test.py
+++ b/Code/Control/Laptop_Code/main_integrated_test.py
@@ -4,7 +4,6 @@ import ball_detection.ball_detection as ball_detection
import simple_pid.PID as PID
import timeit
-
from constants import *
# ========= Serial Port I/O ===========
@@ -72,7 +71,6 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
Output:
None
'''
-
output_message = ''
for pwm_itr in [pwm1, pwm2, pwm3, pwm4]:
@@ -90,7 +88,7 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
serial_port.write(output_message.encode())
-# ====== supporting function in main control ====
+# ====== Logic-directing Functions ====
def ball_detect(gbx, gby):
'''
return True if green ball is detected
@@ -104,7 +102,7 @@ def goal_detect(tx,ty):
'''
return True if April Tag is detected
'''
- if tx == 0 and ty == 0:
+ if tx == 100000 and ty == 100000:
return False
else:
return True
@@ -123,7 +121,6 @@ def stop_all():
dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
def move2goal(tx, ty):
"""
Description:
@@ -161,11 +158,11 @@ def move2goal(tx, ty):
pwm2 = abs(outputy)
if(outputy > 0):
- dir1 = '+'
- dir2 = '+'
- else:
dir1 = '-'
dir2 = '-'
+ else:
+ dir1 = '+'
+ dir2 = '+'
# Horizontal
lspeed = -1 * outputx + base_speed
@@ -184,8 +181,6 @@ def move2goal(tx, ty):
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-
-
def seeking():
"""
Description:
@@ -223,23 +218,22 @@ def move2ball(gbx, gby, gb_dist):
inputx = gbx / 1.00
inputy = gby / 1.00
+ # ESP-Cam Center
setpoint_x = 400
- setpoint_y = 300 # ESP 32 Cam Center
-
- pid_x = PID(kpx,kix,kdx,setpoint=setpoint_x)
- pid_y = PID(kpy,kiy,kdy,setpoint=setpoint_y)
+ setpoint_y = 300
+ pid_x = PID(kpx, kix, kdx, setpoint = setpoint_x)
+ pid_y = PID(kpy, kiy, kdy, setpoint = setpoint_y)
pid_x.auto_mode = True
- pid_x.set_auto_mode(True, last_output=8.0)
+ pid_x.set_auto_mode(True, last_output = 8.0)
pid_x.output_limits = (-255,255)
pid_y.output_limits = (-255,255)
-
outputx = pid_x(inputx)
outputy = pid_y(inputy)
- # vertical
+ # Vertical
pwm1 = abs(outputy)
pwm2 = abs(outputy)
@@ -250,28 +244,24 @@ def move2ball(gbx, gby, gb_dist):
dir1 = '-'
dir2 = '-'
- # horizontal
+ # Horizontal
lspeed = -1 * outputx + base_speed
rspeed = 1 * outputx + base_speed
- pwm3 = min( abs(lspeed), 255)
- pwm4 = min( abs(rspeed), 255)
+ pwm3 = min(abs(lspeed), 255)
+ pwm4 = min(abs(rspeed), 255)
if (lspeed > 0):
dir3 = '+'
else:
dir3 = '-'
-
if (rspeed > 0):
dir4 = '+'
else:
dir4 = '-'
-
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-
-# =========== main control ===========
-
+# =========== Main Control ===========
def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
'''
Description:
@@ -286,40 +276,36 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
Output:
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
'''
-
- # # placeholder
+ # placeholder
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
ballDetect = ball_detect(gbx, gby)
ballCapture = ball_capture(LIDAR_dist)
- goalDetect = goal_detect(tx,ty)
+ goalDetect = goal_detect(tx, ty)
- # debug
if ballCapture: # Ball captured
if goalDetect: # Goal detected
- #stop_all()
+ # stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
else: # Goal not detected
- #stop_all()
+ # stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
else: # Ball not captured
if ballDetect: # Ball detected
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
else: # Ball not detected
- # stop_all()
+ # stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-# ===== Main Functions =====
-
+# ===== Main Function =====
if __name__ == '__main__':
# =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O
- PORT = "COM20" # for Alienware
+ PORT = "COM6" # for Alienware
serial_port = serial.Serial(PORT, 115200)
serial_port.close()
serial_port.open()
@@ -330,14 +316,13 @@ if __name__ == '__main__':
# Loading the PyTorch ML model for ball detection
model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
-
# =========== DECLARE VARIABLES ===========
# ESP CAM In
gbx, gby = -1, -1 # by default (-1 means no found green ball)
gb_dist = -1 # by default (-1 means no found green ball)
# Serial Port In
- tx, ty, tz = 0, 0, 0 # by default (0 means no found AirTag)
+ tx, ty, tz = 100000, 100000, 100000 # by default (0 means no found AirTag)
rx, ry, rz = 0, 0, 0
LIDAR_dist = 0
debugM = 'Testing'
@@ -346,21 +331,19 @@ if __name__ == '__main__':
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
- # =========== LOOP FOREVER===========
- # Trigger the ESP32_SLAVE Talk First
+ # Trigger the ESP32_SLAVE to talk first
gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+ # =========== LOOP FOREVER===========
while True:
# ===== STEP 1: TAKE ALL INPUT =====
gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
line = serial_port.readline()
if line == b'SERIAL_IN_START\r\n':
-
tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
-
print("gbx,gby:{},{}".format(gbx,gby))
time.sleep(waitTime)
@@ -370,4 +353,4 @@ if __name__ == '__main__':
# ===== STEP 3: FEED ALL OUTPUT =====
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
- time.sleep(waitTime)
+ time.sleep(waitTime)
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/main_aaron_move2goal.py b/Code/Control/Laptop_Code/main_keyboard.py
similarity index 63%
rename from Code/Control/Laptop_Code/main_aaron_move2goal.py
rename to Code/Control/Laptop_Code/main_keyboard.py
index d92a1f5652a9827f6e495519ec13d66a65dd5a76..a7dd3dbbcfee30f6945160082bf6bf9d17d24bf1 100644
--- a/Code/Control/Laptop_Code/main_aaron_move2goal.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -2,6 +2,7 @@ import time
import serial
import ball_detection.ball_detection as ball_detection
import simple_pid.PID as PID
+import cv2
from constants import *
@@ -73,19 +74,18 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
output_message = ''
for pwm_itr in [pwm1, pwm2, pwm3, pwm4]:
- print(pwm_itr)
+ # print(pwm_itr)
if len(str(pwm_itr)) == 2:
output_message += '0'
elif len(str(pwm_itr)) == 1:
output_message += '00'
output_message += str(pwm_itr)
+ print(pwm_itr)
output_message = output_message + dir1 + dir2 + dir3 + dir4 + '\n'
- serial_port.write(output_message.encode())
-
- # DEBUG Verbose
print("serial out ...")
print(output_message)
+ serial_port.write(output_message.encode())
# ====== Logic-directing Functions ====
@@ -102,7 +102,7 @@ def goal_detect(tx,ty):
'''
return True if April Tag is detected
'''
- if tx == 0 and ty == 0:
+ if tx == 100000 and ty == 100000:
return False
else:
return True
@@ -116,13 +116,11 @@ def ball_capture(LIDAR_dist):
else:
return False
-# ======= Motion-based Functions ======
def stop_all():
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
def move2goal(tx, ty):
"""
Description:
@@ -138,11 +136,11 @@ def move2goal(tx, ty):
dir1, dir2, dir3, dir4
"""
inputx = tx / 1.00
- inputy = ty / 1.00
+ inputy = -1.00 * (ty + AT_goal_Delta) / 1.00 #
# April Tag Center
- setpoint_x1 = 160
- setpoint_y1 = 120
+ setpoint_x1 = 0.0
+ setpoint_y1 = 0.0
pid_x = PID(kdx_g, kix_g, kpx_g, setpoint = setpoint_x1)
pid_y = PID(kdx_g, kiy_g, kpy_g, setpoint = setpoint_y1)
@@ -160,11 +158,11 @@ def move2goal(tx, ty):
pwm2 = abs(outputy)
if(outputy > 0):
- dir1 = '+'
- dir2 = '+'
- else:
dir1 = '-'
dir2 = '-'
+ else:
+ dir1 = '+'
+ dir2 = '+'
# Horizontal
lspeed = -1 * outputx + base_speed
@@ -201,7 +199,8 @@ def seeking():
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-def move2ball(gbx,gby,gb_dist):
+
+def move2ball(gbx, gby, gb_dist):
"""
Description:
Given the center of x y dist of green ball detected. Call PID control to
@@ -223,8 +222,8 @@ def move2ball(gbx,gby,gb_dist):
setpoint_x = 400
setpoint_y = 300
- pid_x = PID(kdx_b, kix_b, kpx_b, setpoint = setpoint_x)
- pid_y = PID(kdx_b, kiy_b, kpy_b, setpoint = setpoint_y)
+ pid_x = PID(kpx, kix, kdx, setpoint = setpoint_x)
+ pid_y = PID(kpy, kiy, kdy, setpoint = setpoint_y)
pid_x.auto_mode = True
pid_x.set_auto_mode(True, last_output = 8.0)
@@ -248,8 +247,8 @@ def move2ball(gbx,gby,gb_dist):
# Horizontal
lspeed = -1 * outputx + base_speed
rspeed = 1 * outputx + base_speed
- pwm3 = abs(lspeed)
- pwm4 = abs(rspeed)
+ pwm3 = min(abs(lspeed), 255)
+ pwm4 = min(abs(rspeed), 255)
if (lspeed > 0):
dir3 = '+'
else:
@@ -262,8 +261,7 @@ def move2ball(gbx,gby,gb_dist):
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-# =========== main control ===========
-
+# =========== Main Control ===========
def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
'''
Description:
@@ -286,27 +284,118 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
ballCapture = ball_capture(LIDAR_dist)
goalDetect = goal_detect(tx, ty)
- print("ballDetect = ", ballDetect)
- print("ballCapture = ", ballCapture)
- print("goalDetect = ", goalDetect)
-
if ballCapture: # Ball captured
if goalDetect: # Goal detected
+ # stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
- else: # Goal not detectedpwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 =
+ else: # Goal not detected
+ # stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
else: # Ball not captured
if ballDetect: # Ball detected
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
else: # Ball not detected
+ # stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+# ============= keyboard interruption ===================
+
+def init():
+ pygame.init()
+ win= pygame.display.set_mode((200,200))
+
+def keyboard_stop(flag_s,print_count_s):
+ if get_key('q'):
+ flag_s = 0
+ print_count_s = 1
+ return flag_s,print_count_s
-# ===== Main Functions =====
+def get_key(keyname):
+ ans = False
+ for eve in pygame.event.get(): pass
+ keyInput = pygame.key.get_pressed()
+ myKey = getattr(pygame,'K_{}'.format(keyname))
+ if keyInput[myKey]:
+ ans = True
+ pygame.display.update()
+ return ans
+
+def auto_control(serial_port):
+ # =================================== tested autonomous control ======================================================
+ # ===== STEP 1: TAKE ALL INPUT =====
+ waitTime = 0.05
+ gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight=True)
+ line = serial_port.readline()
+
+ if line == b'SERIAL_IN_START\r\n':
+ tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
+ print("gbx,gby:{},{}".format(gbx, gby))
+ time.sleep(waitTime)
+
+ # ===== STEP 2: MAIN CONTROL LOOP =====
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist,
+ debugM)
+
+ # ===== STEP 3: FEED ALL OUTPUT =====
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
+ time.sleep(waitTime) # second
+ # =========================================================================================
+
+def decode_ctl(Ctl_com):
+ pwm1 = Ctl_com[0]
+ pwm2 = Ctl_com[1]
+ pwm3 = Ctl_com[2]
+ pwm4 = Ctl_com[3]
+ dir1 = Ctl_com[4]
+ dir2 = Ctl_com[5]
+ dir3 = Ctl_com[6]
+ dir4 = Ctl_com[7]
+ return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+def manual_control(Ctl_com,serial_port):
+
+ if get_key("w"):
+ val = start_speed
+ Ctl_com = manual_command(val, val, 0, 0, "+", "+", "+", "+")
+
+ elif get_key("s"):
+ val = start_speed
+ Ctl_com = manual_command(val, val, 0, 0, "-", "-", "+", "+")
+
+ if get_key("UP"):
+ val = start_speed
+
+ Ctl_com = manual_command(0, 0, val, val, "+", "+", "+", "+")
+ elif get_key("DOWN"):
+ val = start_speed
+ Ctl_com = manual_command(0, 0, val, val, "+", "+", "-", "-")
+
+ elif get_key("LEFT"):
+ val = start_speed
+ Ctl_com = manual_command(0, 0, val, val, "+", "+", "-", "+")
+
+ elif get_key("RIGHT"):
+ val = start_speed
+ Ctl_com = manual_command(0, 0, val, val, "+", "+", "+", "-")
+
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = decode_ctl(Ctl_com)
+
+ waitTime = 0.05
+
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
+ time.sleep(waitTime)
+
+
+def variables_change():
+ pass
+
+# ===== Main Function =====
if __name__ == '__main__':
# =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O
@@ -316,7 +405,7 @@ if __name__ == '__main__':
serial_port.open()
# Weit Time
- waitTime = 0.10
+ waitTime = 0.05
# Loading the PyTorch ML model for ball detection
model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
@@ -327,7 +416,7 @@ if __name__ == '__main__':
gb_dist = -1 # by default (-1 means no found green ball)
# Serial Port In
- tx, ty, tz = 0, 0, 0 # by default (0 means no found AirTag)
+ tx, ty, tz = 100000, 100000, 100000 # by default (0 means no found AirTag)
rx, ry, rz = 0, 0, 0
LIDAR_dist = 0
debugM = 'Testing'
@@ -336,27 +425,45 @@ if __name__ == '__main__':
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
- count = 0
+ # Trigger the ESP32_SLAVE to talk first
+ gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
+ Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
+ flag = 0
+ print_count = 1
+ init()
# =========== LOOP FOREVER===========
while True:
- # gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
- # print("gbx, gby: {}, {}".format(gbx, gby))
- line = serial_port.readline()
- if line == b'SERIAL_IN_START\r\n':
- # ===== STEP 1: TAKE ALL INPUT =====
- tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
+ if get_key('a'):
+ flag = 1
+ while (flag == 1):
+ auto_control(serial_port)
+ flag, print_count = keyboard_stop(flag,print_count)
+
+ elif get_key('s'):
+ stop_all()
+ print("stop all motors")
+
+ elif get_key('m'):
+ flag = 2
+ while (flag == 2):
+ manual_control(Ctl_com,serial_port)
+ flag, print_count = keyboard_stop(flag,print_count)
+
+ elif get_key('v'):
+ flag = 3
+ while (flag == 3):
+ variables_change()
+ flag, print_count = keyboard_stop(flag,print_count)
+ elif get_key('k'):
+ break
- # ===== STEP 2: MAIN CONTROL LOOP =====
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+ if print_count is not 0:
+ print("No subsystem is running")
+ print_count = 0
- # ===== STEP 3: FEED ALL OUTPUT =====
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
- time.sleep(waitTime)
- if count == 0:
- # first time calling (call once)
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
- count +=1
diff --git a/Code/Control/Laptop_Code/main_zhiying_move2ball_indv_test.py b/Code/Control/Laptop_Code/main_zhiying_move2ball_indv_test.py
deleted file mode 100644
index da8bf923f35a56d2f1721593d70164aaf462d203..0000000000000000000000000000000000000000
--- a/Code/Control/Laptop_Code/main_zhiying_move2ball_indv_test.py
+++ /dev/null
@@ -1,334 +0,0 @@
-import time
-import serial
-import ball_detection.ball_detection as ball_detection
-import simple_pid.PID as PID
-
-from constants import *
-
-# ========= Serial Port I/O ===========
-
-def serial_port_in(serial_port):
- '''
- Description:
- Take all ESP32_Master serial port's printIn and take all necessary input object
-
- Input:
- serial_port : serial.Serail object
-
- Output:
- tx, ty, tz, rx, ry, rz, LIDAR_dist, DebugM
- '''
-
- # DEBUG Verbose
- print("initiating one round of serial in ...")
-
- for i in range(7):
- line = serial_port.readline()
- val = int(line.decode())
-
- if i == 0:
- tx = val
- elif i == 1:
- ty = val
- elif i == 2:
- tz = val
- elif i == 3:
- rx = val
- elif i == 4:
- ry = val
- elif i == 5:
- rz = val
- elif i == 6:
- LIDAR_dist = val
-
- line = serial_port.readline()
- debugM = line.decode()
-
- # DEBUG Verbose
- print("tx:{}".format(tx))
- print("ty:{}".format(ty))
- print("tz:{}".format(tz))
- print("rx:{}".format(rx))
- print("ry:{}".format(ry))
- print("rz:{}".format(rz))
- print("dist:{}".format(LIDAR_dist))
- print(debugM)
-
- return tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM
-
-
-def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4):
- '''
- Description:
- Feed to ESP32_Master to send ESP32_Slave necessary information
- the format of sending is pwm are 3 digit space
-
- Input:
- serial_port : serial.Serail object
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : variables to send
-
- Output:
- None
- '''
-
- output_message = ''
-
- for pwm_itr in [pwm1, pwm2, pwm3, pwm4]:
- # print(pwm_itr)
- if len(str(pwm_itr)) == 2:
- output_message += '0'
- elif len(str(pwm_itr)) == 1:
- output_message += '00'
- output_message += str(pwm_itr)
- print(pwm_itr)
-
- output_message = output_message + dir1 + dir2 + dir3 + dir4 + '\n'
- serial_port.write(output_message.encode())
-
- # DEBUG Verbose
- print("serial out ...")
- print(output_message)
-
-
-
-
-# ====== supporting function in main control ====
-def ball_detect(gbx, gby):
- '''
- return True if green ball is detected
- '''
- if gbx == -1 and gby == -1:
- return False
- else:
- return True
-
-def goal_detect(tx,ty):
- '''
- return True if April Tag is detected
- '''
- if tx == 0 and ty == 0:
- return False
- else:
- return True
-
-def ball_capture(LIDAR_dist):
- '''
- return True if April Tag is detected
- '''
- if (LIDAR_dist < LIDAR_Thres) and (LIDAR_dist > 0): # Ball captured
- return True
- else:
- return False
-
-def stop_all():
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
- dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
- return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-def move2goal(tx, ty):
- """
- Description:
- Given the center of the AT tx, ty. Call PID control to output the blimp
- motor to manuver to the goal
-
- Input:
- tx : x component, center of April Tag
- ty : y component, center of Aprol Tag
-
- Output:
- pwm1, pwm2, pwm3, pwm4
- dir1, dir2, dir3, dir4
- """
- pass
-
- # return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-
-def seeking():
- """
- Description:
- By default, when there ball is not determined capture, the manuver of the
- motors to have it scan its surronding 360 degrees
-
- Input:
- none
-
- Output:
- pwm1, pwm2, pwm3, pwm4
- dir1, dir2, dir3, dir4
- """
- pass
- # return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-
-def move2ball(gbx, gby, gb_dist):
- """
- Description:
- Given the center of x y dist of green ball detected. Call PID control to
- output the blimp motor to manuver to the green ball
-
- Input:
- gbx : x component, center of green ball
- gby : y component, center of green ball
- gb_dist : distance to green ball
-
- Output:
- pwm1, pwm2, pwm3, pwm4
- dir1, dir2, dir3, dir4
- """
- base_speed = 70
-
- kdx,kix,kpx = 2,0.1,0.25
- kdy,kiy,kpy = 1,0.1,0.25
-
- inputx = gbx / 1.00
- inputy = gby / 1.00
-
- setpoint_x = 400
- setpoint_y = 300 # ESP 32 Cam Center
-
- pid_x = PID(kdx,kix,kpx,setpoint=setpoint_x)
- pid_y = PID(kdy,kiy,kpy,setpoint=setpoint_y)
-
-
- pid_x.auto_mode = True
- pid_x.set_auto_mode(True, last_output=8.0)
- pid_x.output_limits = (-255,255)
- pid_y.output_limits = (-255,255)
-
-
- outputx = pid_x(inputx)
- outputy = pid_y(inputy)
-
- # vertical
- pwm1 = abs(outputy)
- pwm2 = abs(outputy)
-
- if(outputy > 0):
- dir1 = '+'
- dir2 = '+'
- else:
- dir1 = '-'
- dir2 = '-'
-
- # horizontal
- lspeed = -1 * outputx + base_speed
- rspeed = 1 * outputx + base_speed
- pwm3 = abs(lspeed)
- pwm4 = abs(rspeed)
- if (lspeed > 0):
- dir3 = '+'
- else:
- dir3 = '-'
-
- if (rspeed > 0):
- dir4 = '+'
- else:
- dir4 = '-'
-
-
- return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-
-
-
-# =========== main control ===========
-
-def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
- '''
- Description:
- Given green ball information and AT information, the main control logic
- to manuver the blimp motors
-
- Input:
- gbx, gby, gb_dist : green ball information
- tx, ty, tz, rx, ry, rz, LIDAR_dist : AirTag information
- debugM : Debug Message
-
- Output:
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
- '''
-
- # placeholder
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
- dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
-
- ballDetect = ball_detect(gbx, gby)
- ballCapture = ball_capture(LIDAR_dist)
- goalDetect = goal_detect(tx,ty)
- # debug
- ballCapture = 0
- if ballCapture: # Ball captured
- if goalDetect: # Goal detected
- stop_all()
- #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
- else: # Goal not detected
- stop_all()
- #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
- else: # Ball not captured
- if ballDetect: # Ball detected
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
- else: # Ball not detected
- stop_all()
- #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
-
- return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
-
-
-# ===== Main Functions =====
-
-if __name__ == '__main__':
- # =========== SET UP ============
- # Defining Variables for ESP 32 Serial I/O
- PORT = "COM6" # for Alienware
- serial_port = serial.Serial(PORT, 115200)
- serial_port.close()
- serial_port.open()
-
- # Weit Time
- waitTime = 0.10
-
- # Loading the PyTorch ML model for ball detection
- model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
- #model = ball_detection.returnModel(modelAction, device, trainLoc, labelSet, modelLoc, modelFile)
-
-
- # =========== DECLARE VARIABLES ===========
- # ESP CAM In
- gbx, gby = -1, -1 # by default (-1 means no found green ball)
- gb_dist = -1 # by default (-1 means no found green ball)
-
- # Serial Port In
- tx, ty, tz = 0, 0, 0 # by default (0 means no found AirTag)
- rx, ry, rz = 0, 0, 0
- LIDAR_dist = 0
- debugM = 'Testing'
-
- # Serial Port Out
- pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
- dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
-
- count = 0
- # =========== LOOP FOREVER===========
- while True:
- gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
- print("gbx,gby:{},{}".format(gbx,gby))
-
- line = serial_port.readline()
- if line == b'SERIAL_IN_START\r\n':
- # ===== STEP 1: TAKE ALL INPUT =====
- tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
-
- # ===== STEP 2: MAIN CONTROL LOOP =====
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
-
- # ===== STEP 3: FEED ALL OUTPUT =====
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
-
- if count == 0:
- # first time calling (call once)
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
- # count +=1
-
- time.sleep(waitTime)