diff --git a/Code/Control/Laptop_Code/ESP32_slave/Camera.cpp b/Code/Control/Laptop_Code/ESP32_slave/Camera.cpp
index fb27cf808991a0b2083ca36f412ba1aa088527e6..56f42d65285a6f3129d8bf37368f094f86682d65 100644
--- a/Code/Control/Laptop_Code/ESP32_slave/Camera.cpp
+++ b/Code/Control/Laptop_Code/ESP32_slave/Camera.cpp
@@ -72,7 +72,7 @@ bool Camera::exe_color_detection_biggestblob(int8_t l_min, int8_t l_max, int8_t
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;
+ struct { int16_t cid, ctx, cty, ctz, crx, cry, crz; } goalfinder_result;
if(interface->call(F("goalfinder"), goalid, sizeof(goalid), &goalfinder_result, sizeof(goalfinder_result))){
}
diff --git a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
index 63ee80881f2415244e465f067349e601bc144361..c32e3b41b30ed8d5dc0e58491546bf190020a22c 100644
--- a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
+++ b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
@@ -1,327 +1,326 @@
-#include <esp_now.h>
-#include <WiFi.h>
-#include <Wire.h>
-#include <SparkFun_VL53L1X.h>
-#include <Arduino.h>
-
-#include "Camera.h"
-#include "utilities.h"
-#include <Adafruit_MotorShield.h>
-
-// MAC Address of the receiver (MASTER)
-//uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}; // #1
-//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xBD, 0xF8}; // #2
-//uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}; // #3
-//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
-//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xC0, 0xD8}; // #5
-//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xDE, 0xE0}; // #6
-//uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xD3, 0x3C}; // #7
-uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x9B, 0x04, 0x98}; // #8
-
-String success;
-// Define variables to store incoming readings
-String sentDebugM = "";
-int Rec_pwm1;
-int Rec_pwm2;
-int Rec_pwm3;
-int Rec_pwm4;
-String Rec_dir1;
-String Rec_dir2;
-String Rec_dir3;
-String Rec_dir4;
-
-int count = 0;
-int count_var = 0;
-int print_count = 0;
-int change_count =0;
-int Lidar_flag = 0;
-
-// Define variables to be sent;
-int Sent_tx = 0;
-int Sent_ty = 0;
-int Sent_tz = 0;
-int Sent_rx = 0;
-int Sent_ry = 0;
-int Sent_rz = 0;
-int Sent_dist = 0;
-const int omv_def = 100000;
-int8_t g1 = 0,g2=1,g3=2;
-int8_t goal_id[3] = {g1, g2, g3};
-
-// Define Lidar variables
-SFEVL53L1X distanceSensor;
-int budgetIndex = 4;
-int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500};
-
-// Define LED variable
-int LED = LED_BUILTIN;
-
-// ==================================== data structure =================================================
-//Structure the sending data
-//Must match the receiver structure
-typedef struct struct_message {
- int Rtx;
- int Rty;
- int Rtz;
- int Rrx;
- int Rry;
- int Rrz;
- int Rdist;
- String DebugM;
- int Spwm1;
- int Spwm2;
- int Spwm3;
- int Spwm4;
- String Sdir1;
- String Sdir2;
- String Sdir3;
- String Sdir4;
-} struct_message;
-// Create a struct_message to hold incoming sensor readings
-// struct_message incomingReadings;
-struct_message receivedData;
-
-// =================================== send and received function =====================================
-// Callback when data is sent
-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 :)";
- }
- else{
- success = "Delivery Fail :(";
- }
-}
-
-// Callback when data is received
-void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
- memcpy(&receivedData, incomingData, sizeof(receivedData));
- Serial.println("OnDataRecv");
- // the data format: 225 225 225 225 + + - -
- // + up / forward
- // - down / backward
- Rec_pwm1 = receivedData.Spwm1;
- Rec_pwm2 = receivedData.Spwm2;
- Rec_pwm3 = receivedData.Spwm3;
- Rec_pwm4 = receivedData.Spwm4;
- Rec_dir1 = receivedData.Sdir1;
- Rec_dir2 = receivedData.Sdir2;
- Rec_dir3 = receivedData.Sdir3;
- Rec_dir4 = receivedData.Sdir4;
- count_var = 0;
- count = 0;
- print_count=0;
- change_count = 0;
- control_motion();
-}
-
-// =============================== 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);
-// ========================== 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); // pwm1
-Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(2); // pwm2
-Adafruit_DCMotor *motorLeft = AFMS.getMotor(3); // pwm3
-Adafruit_DCMotor *motorRight = AFMS.getMotor(4); // pwm4
-// ==================================== Set up =================================================
-void setup() {
- // Init Serial Monitor
- Serial.begin(115200);
- Wire.begin();
- interface.begin(); //communication between ESP and OpenMV
- AFMS.begin(); // create with the default frequency 1.6KHz
- // -------------- LED part --------------------
- pinMode(LED, OUTPUT);
- digitalWrite(LED, HIGH);
- digitalWrite(LED, LOW);
- Serial.println("Preliminary Setup done");
- // -------------- lidar part --------------------
- if (distanceSensor.begin() == 0){
- Serial.println("Sensor online!");
- Lidar_flag = 1;
- }else {
- Lidar_flag = 0;
- }
-
- distanceSensor.startRanging();
- distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
-
- // --------------------------- esp now ---------------------------
- // Set device as a Wi-Fi Station
- WiFi.mode(WIFI_STA);
-
- // Init ESP-NOW
- if (esp_now_init() != ESP_OK) {
- Serial.println("Error initializing ESP-NOW");
- return;
- }
-
- // Once ESPNow is successfully Init, we will register for Send CB to
- // get the status of Trasnmitted packet
- esp_now_register_send_cb(OnDataSent);
-
- // Register peer
- esp_now_peer_info_t peerInfo;
- memcpy(peerInfo.peer_addr, broadcastAddress, 6);
- peerInfo.channel = 0;
- peerInfo.encrypt = false;
-
- // Add peer
- if (esp_now_add_peer(&peerInfo) != ESP_OK){
- Serial.println("Failed to add peer");
- return;
- }
- // Register for a callback function that will be called when data is received
- esp_now_register_recv_cb(OnDataRecv);
-
-}
-
-// ================================== Main loop ===================================================
-void loop()
-{
- //-------------------------------------------------------------------------------------
- // ========== goal finder =========
- int id = -1;
- int tx = 0; int ty = 0; int tz = 0;
- int rx = 0; int ry = 0; int rz = 0;
- int x = 0;
- int y = 0;
- bool goalfind_flag = 0;
-
- goalfind_flag = cam.exe_goalfinder(goal_id[0],goal_id[1],goal_id[2], id, tx, ty, tz, rx, ry, rz);
- if (goalfind_flag){
- Sent_tx = tx;
- Sent_ty = ty;
- Sent_tz = tz; // cm
- Sent_rx = rx;
- Sent_ry = ry;
- Sent_rz = rz;
- }else{
- Sent_tx = omv_def;
- Sent_ty = omv_def;
- Sent_tz = omv_def; // cm
- Sent_rx = 0;
- Sent_ry = 0;
- Sent_rz = 0;
- }
- if (Lidar_flag == 1){
- Sent_dist = distanceSensor.getDistance(); // Lidar distance (use the real Lidar data)
- }else {
- Sent_dist = 0;
- }
- // ========== lidar state info =========
- if (Sent_dist < 300 && Sent_dist > 0){
- receivedData.DebugM = "found b";
- }else{
- receivedData.DebugM = "no b";
- }
- // ========== send info =========
- receivedData.Rtx = Sent_tx;
- receivedData.Rty = Sent_ty;
- receivedData.Rtz = Sent_tz;
- receivedData.Rrx = Sent_rx;
- receivedData.Rry = Sent_ry;
- receivedData.Rrz = Sent_rz;
- receivedData.Rdist = Sent_dist;
- send_var_once();
- print_received_Data();
-
- //-------------------------------------------------------------------------------------
- // control_motion();
- //-------------------------------------------------------------------------------------
-}
-// ================================== ^ Main loop ^ ===================================================
-void control_motion(){
- // vertical motor
- Serial.println("control_motion");
- Serial.println(abs(Rec_pwm1));
- motorVertical_L->setSpeed(abs(Rec_pwm1));
- motorVertical_R->setSpeed(abs(Rec_pwm2));
-
- if (Rec_dir1 == "+"){
- motorVertical_L->run(BACKWARD); // up
- }else if (Rec_dir1 == "-"){
- motorVertical_L->run(FORWARD); // down
- }
-
- if (Rec_dir2 == "+"){
- motorVertical_R->run(FORWARD);
- }else if (Rec_dir2 == "-"){
- motorVertical_R->run(BACKWARD);
- }
-
- // horizontal motor
- motorLeft->setSpeed(abs(Rec_pwm3));
- motorRight->setSpeed(abs(Rec_pwm4));
-
- if (Rec_dir3 == "+"){
- motorLeft->run(BACKWARD); // make it move forward
- }else if (Rec_dir3 == "-"){
- motorLeft->run(FORWARD); // make it move backward
- }
-
- if (Rec_dir4 == "+"){
- motorRight-> run(FORWARD); // make it move forward
- }else if (Rec_dir4 == "-"){
- motorRight-> run(BACKWARD); // make it move backward
- }
-
-}
-
-void print_received_Data(){
- if (print_count == 0){
- Serial.print("Rec_pwm1:");
- Serial.println(Rec_pwm1);
- Serial.print("Rec_pwm2:");
- Serial.println(Rec_pwm2);
- Serial.print("Rec_pwm3:");
- Serial.println(Rec_pwm3);
- Serial.print("Rec_pwm4:");
- Serial.println(Rec_pwm4);
- Serial.print("Rec_dir1:");
- Serial.println(Rec_dir1);
- Serial.print("Rec_dir2:");
- Serial.println(Rec_dir2);
- Serial.print("Rec_dir3:");
- Serial.println(Rec_dir3);
- Serial.print("Rec_dir4:");
- Serial.println(Rec_dir4);
- Serial.println("_________________________");
- }
- print_count +=1;
-}
-
-void send_var_once(){
- if(count_var==0){
- send_message();
- count_var+=1;
- }
-}
-
-void send_message_once(){
- if(count==0){
- send_message();
- count+=1;
- receivedData.DebugM = "";
- }
-}
-
-void send_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) {
- Serial.println("Sent with success");
- }
- else {
- Serial.println("Error sending the data");
- }
- //-------------------------------------------------------------------------------------
- delay(50); // delay 50 ms after send out the message
-}
+#include <esp_now.h>
+#include <WiFi.h>
+#include <Wire.h>
+#include <SparkFun_VL53L1X.h>
+#include <Arduino.h>
+
+#include "Camera.h"
+#include "utilities.h"
+#include <Adafruit_MotorShield.h>
+
+// REPLACE WITH THE MAC Address of your receiver (MASTER)
+// Slave: 40:F5:20:44:B6:4C
+// MAC Address of the receiver (MASTER)
+uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}; // #1
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xBD, 0xF8}; // #2
+//uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}; // #3
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xC0, 0xD8}; // #5
+//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xDE, 0xE0}; // #6
+//uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xD3, 0x3C}; // #7
+// uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x9B, 0x04, 0x98}; // #8
+String success;
+
+// Define variables to store incoming readings
+String sentDebugM = "";
+int Rec_pwm1;
+int Rec_pwm2;
+int Rec_pwm3;
+int Rec_pwm4;
+String Rec_dir1;
+String Rec_dir2;
+String Rec_dir3;
+String Rec_dir4;
+
+int count = 0;
+int count_var = 0;
+int print_count = 0;
+int change_count =0;
+int Lidar_flag = 0;
+
+// Define variables to be sent;
+int Sent_tx = 0;
+int Sent_ty = 0;
+int Sent_tz = 0;
+int Sent_rx = 0;
+int Sent_ry = 0;
+int Sent_rz = 0;
+int Sent_dist = 0;
+
+int8_t g1 = 0,g2=1,g3=2;
+int8_t goal_id[3] = {g1, g2, g3};
+
+// Define Lidar variables
+SFEVL53L1X distanceSensor;
+int budgetIndex = 4;
+int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500};
+
+// Define LED variable
+int LED = LED_BUILTIN;
+
+// ==================================== data structure =================================================
+//Structure the sending data
+//Must match the receiver structure
+typedef struct struct_message {
+ int Rtx;
+ int Rty;
+ int Rtz;
+ int Rrx;
+ int Rry;
+ int Rrz;
+ int Rdist;
+ String DebugM;
+ int Spwm1;
+ int Spwm2;
+ int Spwm3;
+ int Spwm4;
+ String Sdir1;
+ String Sdir2;
+ String Sdir3;
+ String Sdir4;
+} struct_message;
+// Create a struct_message to hold incoming sensor readings
+// struct_message incomingReadings;
+struct_message receivedData;
+
+// =================================== send and received function =====================================
+// Callback when data is sent
+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 🙂";
+ }
+ else{
+ success = "Delivery Fail ðŸ™";
+ }
+}
+
+// Callback when data is received
+void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
+ memcpy(&receivedData, incomingData, sizeof(receivedData));
+ // the data format: 225 225 225 225 + + - -
+ // + up / forward
+ // - down / backward
+ Rec_pwm1 = receivedData.Spwm1;
+ Rec_pwm2 = receivedData.Spwm2;
+ Rec_pwm3 = receivedData.Spwm3;
+ Rec_pwm4 = receivedData.Spwm4;
+ Rec_dir1 = receivedData.Sdir1;
+ Rec_dir2 = receivedData.Sdir2;
+ Rec_dir3 = receivedData.Sdir3;
+ Rec_dir4 = receivedData.Sdir4;
+ count_var = 0;
+ count = 0;
+ print_count=0;
+ change_count = 0;
+ change_count = 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);
+// ========================== 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); // pwm1
+Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(2); // pwm2
+Adafruit_DCMotor *motorLeft = AFMS.getMotor(3); // pwm3
+Adafruit_DCMotor *motorRight = AFMS.getMotor(4); // pwm4
+// ==================================== Set up =================================================
+void setup() {
+ // Init Serial Monitor
+ Serial.begin(115200);
+ Wire.begin();
+ interface.begin(); //communication between ESP and OpenMV
+ AFMS.begin(); // create with the default frequency 1.6KHz
+ // -------------- LED part --------------------
+ pinMode(LED, OUTPUT);
+ digitalWrite(LED, HIGH);
+ digitalWrite(LED, LOW);
+
+ // -------------- lidar part --------------------
+ if (distanceSensor.begin() == 0){
+ Serial.println("Sensor online!");
+ Lidar_flag = 1;
+ }else {
+ Lidar_flag = 0;
+ }
+
+ distanceSensor.startRanging();
+ distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
+
+ // --------------------------- esp now ---------------------------
+ // Set device as a Wi-Fi Station
+ WiFi.mode(WIFI_STA);
+
+ // Init ESP-NOW
+ if (esp_now_init() != ESP_OK) {
+ Serial.println("Error initializing ESP-NOW");
+ return;
+ }
+
+ // Once ESPNow is successfully Init, we will register for Send CB to
+ // get the status of Trasnmitted packet
+ esp_now_register_send_cb(OnDataSent);
+
+ // Register peer
+ esp_now_peer_info_t peerInfo;
+ memcpy(peerInfo.peer_addr, broadcastAddress, 6);
+ peerInfo.channel = 0;
+ peerInfo.encrypt = false;
+
+ // Add peer
+ if (esp_now_add_peer(&peerInfo) != ESP_OK){
+ Serial.println("Failed to add peer");
+ return;
+ }
+ // Register for a callback function that will be called when data is received
+ esp_now_register_recv_cb(OnDataRecv);
+
+}
+
+// ================================== Main loop ===================================================
+void loop()
+{
+ //-------------------------------------------------------------------------------------
+ // ========== goal finder =========
+ int id = -1;
+ int tx = 0; int ty = 0; int tz = 0;
+ int rx = 0; int ry = 0; int rz = 0;
+ int x = 0;
+ int y = 0;
+ bool goalfind_flag = 0;
+
+ goalfind_flag = cam.exe_goalfinder(goal_id[0],goal_id[1],goal_id[2], id, tx, ty, tz, rx, ry, rz);
+ if (goalfind_flag){
+ Sent_tx = tx;
+ Sent_ty = ty;
+ Sent_tz = tz; // cm
+ Sent_rx = rx;
+ Sent_ry = ry;
+ Sent_rz = rz;
+ }else{
+ Sent_tx = 0;
+ Sent_ty = 0;
+ Sent_tz = 0; // cm
+ Sent_rx = 0;
+ Sent_ry = 0;
+ Sent_rz = 0;
+ }
+ if (Lidar_flag == 1){
+ Sent_dist = distanceSensor.getDistance(); // Lidar distance (use the real Lidar data)
+ }else {
+ Sent_dist = 0;
+ }
+ // ========== lidar state info =========
+ if (Sent_dist < 300 && Sent_dist > 0){
+ receivedData.DebugM = "found b";
+ }else{
+ receivedData.DebugM = "no b";
+ }
+ // ========== send info =========
+ receivedData.Rtx = Sent_tx;
+ receivedData.Rty = Sent_ty;
+ receivedData.Rtz = Sent_tz;
+ receivedData.Rrx = Sent_rx;
+ receivedData.Rry = Sent_ry;
+ receivedData.Rrz = Sent_rz;
+ receivedData.Rdist = Sent_dist;
+ send_var_once();
+ print_received_Data();
+
+ //-------------------------------------------------------------------------------------
+ control_motion();
+ //-------------------------------------------------------------------------------------
+}
+// ================================== ^ Main loop ^ ===================================================
+void control_motion(){
+ // vertical motor
+ motorVertical_L->setSpeed(abs(Rec_pwm1));
+ motorVertical_R->setSpeed(abs(Rec_pwm2));
+
+ if (Rec_dir1 == "+"){
+ motorVertical_L->run(BACKWARD); // up
+ }else if (Rec_dir1 == "-"){
+ motorVertical_L->run(FORWARD); // down
+ }
+
+ if (Rec_dir2 == "+"){
+ motorVertical_R->run(FORWARD);
+ }else if (Rec_dir2 == "-"){
+ motorVertical_R->run(BACKWARD);
+ }
+
+ // horizontal motor
+ motorLeft->setSpeed(abs(Rec_pwm3));
+ motorRight->setSpeed(abs(Rec_pwm4));
+
+ if (Rec_dir3 == "+"){
+ motorLeft->run(BACKWARD); // make it move forward
+ }else if (Rec_dir3 == "-"){
+ motorLeft->run(FORWARD); // make it move backward
+ }
+
+ if (Rec_dir4 == "+"){
+ motorRight-> run(FORWARD); // make it move forward
+ }else if (Rec_dir4 == "-"){
+ motorRight-> run(BACKWARD); // make it move backward
+ }
+
+}
+
+void print_received_Data(){
+ if (print_count == 0){
+ Serial.print("Rec_pwm1:");
+ Serial.println(Rec_pwm1);
+ Serial.print("Rec_pwm2:");
+ Serial.println(Rec_pwm2);
+ Serial.print("Rec_pwm3:");
+ Serial.println(Rec_pwm3);
+ Serial.print("Rec_pwm4:");
+ Serial.println(Rec_pwm4);
+ Serial.print("Rec_dir1:");
+ Serial.println(Rec_dir1);
+ Serial.print("Rec_dir2:");
+ Serial.println(Rec_dir2);
+ Serial.print("Rec_dir3:");
+ Serial.println(Rec_dir3);
+ Serial.print("Rec_dir4:");
+ Serial.println(Rec_dir4);
+Serial.println("_______________________");
+ }
+ print_count +=1;
+}
+
+void send_var_once(){
+ if(count_var==0){
+ send_message();
+ count_var+=1;
+ }
+}
+
+void send_message_once(){
+ if(count==0){
+ send_message();
+ count+=1;
+ receivedData.DebugM = "";
+ }
+}
+
+void send_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) {
+ Serial.println("Sent with success");
+ }
+ else {
+ Serial.println("Error sending the data");
+ }
+ //-------------------------------------------------------------------------------------
+ delay(50); // delay 50 ms after send out the message
+}
diff --git a/Code/Control/Laptop_Code/ESP32_slave/LedPanel.cpp b/Code/Control/Laptop_Code/ESP32_slave/LedPanel.cpp
deleted file mode 100644
index eb4ee53b1e31f36bf48e1ea4cf8ecff909fb74c3..0000000000000000000000000000000000000000
--- a/Code/Control/Laptop_Code/ESP32_slave/LedPanel.cpp
+++ /dev/null
@@ -1,65 +0,0 @@
-#include "LedPanel.h"
-
-LedPanel::LedPanel(int pixelnum, int pixelpin){
- led_num = pixelnum;
- //pixels = Adafruit_NeoPixel(pixelnum, pixelpin, NEO_GRB + NEO_KHZ800);
-}
-
-void LedPanel::fullPanelLight(int r, int g, int b){
- for (int i=0 ; i<led_num ; i++){
- pixels.setPixelColor(i, r,g,b);
- }
- pixels.show();
-}
-
-void LedPanel::beginPanel(){
- pixels.begin();
-}
-
-void LedPanel::resetPanel(){
- for (int i=0 ; i<led_num ; i++){
- pixels.setPixelColor(i, 0,0,0);
- }
- pixels.show();
-}
-
-void LedPanel::topLeftQuadrant(int r, int g, int b){
- for (int i = 0; i<HORIZONTAL_SIZE/2; i++){
- for (int j=0; j<VERTICAL_SIZE/2;j++){
- pixels.setPixelColor(i+j*HORIZONTAL_SIZE, r, g, b);
- }
- }
- pixels.show();
-}
-
-void LedPanel::bottomLeftQuadrant(int r, int g, int b){
- for (int i = 0; i<HORIZONTAL_SIZE/2; i++){
- for (int j=VERTICAL_SIZE/2; j<VERTICAL_SIZE;j++){
- pixels.setPixelColor(i+j*HORIZONTAL_SIZE, r, g, b);
- }
- }
- pixels.show();
-}
-
-void LedPanel::topRightQuadrant(int r, int g, int b){
- for (int i = HORIZONTAL_SIZE/2; i<HORIZONTAL_SIZE; i++){
- for (int j=0; j<VERTICAL_SIZE/2;j++){
- pixels.setPixelColor(i+j*HORIZONTAL_SIZE, r, g, b);
- }
- }
- pixels.show();
-}
-
-void LedPanel::bottomRightQuadrant(int r, int g, int b){
- for (int i = HORIZONTAL_SIZE/2; i<HORIZONTAL_SIZE; i++){
- for (int j=VERTICAL_SIZE/2; j<VERTICAL_SIZE;j++){
- pixels.setPixelColor(i+j*HORIZONTAL_SIZE, r, g, b);
- }
- }
- pixels.show();
-}
-
-void LedPanel::singleLED(int num, int r, int g, int b) {
- pixels.setPixelColor(num, r, g, b);
- pixels.show();
-}
diff --git a/Code/Control/Laptop_Code/ESP32_slave/LedPanel.h b/Code/Control/Laptop_Code/ESP32_slave/LedPanel.h
deleted file mode 100644
index 41721cc9dd7750951e5345169c2eb604d549f704..0000000000000000000000000000000000000000
--- a/Code/Control/Laptop_Code/ESP32_slave/LedPanel.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#ifndef LEDPANEL_H
-#define LEDPANEL_H
-
-#include <Adafruit_NeoPixel.h>
-#define NUMPIXELS 32
-#define PIN_PIXELS 32 // Use pin 32 for NeoPixels
-const int HORIZONTAL_SIZE = 8;
-const int VERTICAL_SIZE = 4;
-
-class LedPanel {
- private:
- int led_num;
- Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, PIN_PIXELS, NEO_GRB + NEO_KHZ800);
-
- public:
- LedPanel(int pixelnum, int pixelpin);
- void fullPanelLight(int r, int b, int g);
- void resetPanel();
- void topLeftQuadrant(int r, int g, int b);
- void bottomLeftQuadrant(int r, int g, int b);
- void topRightQuadrant(int r, int g, int b);
- void bottomRightQuadrant(int r, int g, int b);
- void beginPanel();
- void singleLED(int num, int r, int g, int b);
-
-};
-
-#endif
diff --git a/Code/Control/Laptop_Code/ESP32_slave/TestInputs.txt b/Code/Control/Laptop_Code/ESP32_slave/TestInputs.txt
deleted file mode 100644
index 00fff876680ffb6421e8cccd06318078017cc3b0..0000000000000000000000000000000000000000
--- a/Code/Control/Laptop_Code/ESP32_slave/TestInputs.txt
+++ /dev/null
@@ -1,2 +0,0 @@
-100100000000+-+-
-000000000000++++
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/constants.py b/Code/Control/Laptop_Code/constants.py
index c066038c674aa909906b86a3de378dc56ae6f765..7d4416bf3702150f02d81d81f1b34eb217c43408 100644
--- a/Code/Control/Laptop_Code/constants.py
+++ b/Code/Control/Laptop_Code/constants.py
@@ -30,19 +30,19 @@ Bmin = 31
Bmax = 127
threshold = [Lmin, Lmax, Amin, Amax, Bmin, Bmax]
-base_speed = 30
-seeking_speed = 70
+base_speed = 70
+seeking_speed = 100
LIDAR_Thres = 300 # mm
# PID Control in move2ball
-kpx,kix,kdx = 1.0, 0.01, 0.25
-kpy,kiy,kdy = 0.8, 0.01, 0.25
+kpx,kix,kdx = 1.5, 0.01, 0.5
+kpy,kiy,kdy = 1.2, 0.01, 0.5
# PID Control in move2goal
-kpx_g,kix_g,kdx_g = 2.8, 0.04, 1.00
-kpy_g,kiy_g,kdy_g = 2.0, 0.04, 1.00
+kpx_g,kix_g,kdx_g = 1.5, 0.01, 0.5
+kpy_g,kiy_g,kdy_g = 1.2, 0.01, 0.5
# difference between center of AT and center of goal
AT_goal_Delta = 110 #cm
diff --git a/Code/Control/Laptop_Code/main_integrated_test.py b/Code/Control/Laptop_Code/main_integrated_test.py
index 41dad51d6a4eb16cef6c6e9336366e9c0256ecd9..6fa51abe6cfd199810c3d39fc23c796cfd1a4724 100644
--- a/Code/Control/Laptop_Code/main_integrated_test.py
+++ b/Code/Control/Laptop_Code/main_integrated_test.py
@@ -1,11 +1,14 @@
import time
import serial
-import ball_detection.ball_detection as ball_detection
+
import simple_pid.PID as PID
import timeit
from constants import *
-
+global ml
+ml = 1
+if ml ==1:
+ import ball_detection.ball_detection as ball_detection
# ========= Serial Port I/O ===========
def serial_port_in(serial_port):
@@ -102,7 +105,7 @@ def goal_detect(tx,ty):
'''
return True if April Tag is detected
'''
- if tx == 100000 and ty == 100000:
+ if tx == 0 and ty == 0:
return False
else:
return True
@@ -167,15 +170,21 @@ def move2goal(tx, ty):
# Horizontal
lspeed = -1 * outputx + base_speed
rspeed = 1 * outputx + base_speed
- pwm3 = abs(lspeed)
- pwm4 = abs(rspeed)
+ # pwm3 = abs(lspeed)
+ # pwm4 = abs(rspeed)
+ pwm3 = min(abs(lspeed), 255)
+ pwm4 = min(abs(rspeed), 255)
if (lspeed > 0):
dir3 = '+'
+ # dir3 = '-'
else:
dir3 = '-'
+ # dir3 = '+'
if (rspeed > 0):
+ #dir4 = '+'
dir4 = '+'
else:
+ # dir4 = '-'
dir4 = '-'
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
@@ -283,7 +292,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
ballDetect = ball_detect(gbx, gby)
ballCapture = ball_capture(LIDAR_dist)
goalDetect = goal_detect(tx, ty)
-
+ ballCapture = 1
if ballCapture: # Ball captured
if goalDetect: # Goal detected
# stop_all() # Debug
@@ -314,7 +323,8 @@ if __name__ == '__main__':
waitTime = 0.05
# Loading the PyTorch ML model for ball detection
- model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
+ if ml == 1:
+ model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
# =========== DECLARE VARIABLES ===========
# ESP CAM In
@@ -332,14 +342,16 @@ if __name__ == '__main__':
dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
# Trigger the ESP32_SLAVE to talk first
- gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ if ml == 1:
+ 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)
+ if ml == 1:
+ gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
line = serial_port.readline()
if line == b'SERIAL_IN_START\r\n':
@@ -353,4 +365,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)
\ No newline at end of file
+ time.sleep(waitTime)
diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index d2efd7764c9904535574bf9c3b511226fd612d5d..a7fc2b9cfc4a8602e06d875df5254fc610c8abec 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -1,16 +1,13 @@
import time
import serial
-import pygame
+import ball_detection.ball_detection as ball_detection
import simple_pid.PID as PID
-import cv2
-
+import timeit
+import pygame
from constants import *
global ml
-ml = 0
-if ml == 1:
- import ball_detection.ball_detection as ball_detection
-
+ml = 1
# ========= Serial Port I/O ===========
def serial_port_in(serial_port):
@@ -107,7 +104,7 @@ def goal_detect(tx,ty):
'''
return True if April Tag is detected
'''
- if tx == 100000 and ty == 100000:
+ if tx == 0 and ty == 0:
return False
else:
return True
@@ -288,7 +285,10 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
ballDetect = ball_detect(gbx, gby)
ballCapture = ball_capture(LIDAR_dist)
goalDetect = goal_detect(tx, ty)
- ballCapture = 1
+
+ # debug
+ # ballCapture = 1
+ # goalDetect = 0
if ballCapture: # Ball captured
if goalDetect: # Goal detected
# stop_all() # Debug
@@ -305,33 +305,9 @@ 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
-# ============= 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
-
-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,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
# ===== STEP 1: TAKE ALL INPUT =====
- # gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
line = serial_port.readline()
if line == b'SERIAL_IN_START\r\n':
@@ -348,62 +324,40 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di
time.sleep(waitTime)
-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_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,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
+def auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
- 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, "+", "+", "-", "+")
+ 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)
+ init_count += 1
+ return init_count
- elif get_key("RIGHT"):
- val = start_speed
- Ctl_com = manual_command(0, 0, val, val, "+", "+", "+", "-")
+def init():
+ pygame.init()
+ win= pygame.display.set_mode((200,200))
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = decode_ctl(Ctl_com)
+def keyboard_stop(flag_s,print_count_s):
- waitTime = 0.05
+ if get_key('q'):
+ flag_s = 0
+ print_count_s = 1
+ return flag_s,print_count_s
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+def get_key(keyname):
+ ans = False
+ for eve in pygame.event.get(): pass
+ keyInput = pygame.key.get_pressed()
+ myKey = getattr(pygame,'K_{}'.format(keyname))
- time.sleep(waitTime)
+ if keyInput[myKey]:
+ ans = True
+ pygame.display.update()
+ return ans
+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 dynamic_variable(str_name_v):
@@ -458,11 +412,68 @@ def variables_change_once():
str_name = input("Enter your variable: ")
dynamic_variable(str_name)
+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
+ # changed
+ gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
+ time.sleep(waitTime)
+
+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 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
# ===== Main Function =====
if __name__ == '__main__':
# =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O
- PORT = "COM3" # Based on your own serial port number
+ PORT = "COM6" # for Alienware
serial_port = serial.Serial(PORT, 115200)
serial_port.close()
serial_port.open()
@@ -471,12 +482,10 @@ if __name__ == '__main__':
waitTime = 0.05
# Loading the PyTorch ML model for ball detection
- if ml == 1:
- model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
+ model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
# =========== DECLARE VARIABLES ===========
# ESP CAM In
- global gbx,gby,gb_dist
gbx, gby = -1, -1 # by default (-1 means no found green ball)
gb_dist = -1 # by default (-1 means no found green ball)
@@ -489,29 +498,29 @@ if __name__ == '__main__':
# Serial Port Out
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
-
+ Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
# Trigger the ESP32_SLAVE to talk first
- if ml == 1:
- gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ """
+ 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_count = 0
+ print_count = 0
global start_speed
start_speed = 70
init()
# =========== LOOP FOREVER===========
while True:
-
if get_key('a'):
flag = 1
while (flag == 1):
+ if init_count == 0:
+ init_count = auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
flag, print_count = keyboard_stop(flag,print_count)
-
elif get_key('s'):
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = stop_all()
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
@@ -533,6 +542,6 @@ if __name__ == '__main__':
elif get_key('k'):
break
- if print_count is not 0:
+ if print_count != 0:
print("No subsystem is running")
print_count = 0
diff --git a/Code/Control/Laptop_Code/main_zzl.py b/Code/Control/Laptop_Code/main_zzl.py
index 951b0a07297e35839435eb8dc65da28540bdacb5..3c0ba85d36341bc48ac6697958b9d0170fc3fd05 100644
--- a/Code/Control/Laptop_Code/main_zzl.py
+++ b/Code/Control/Laptop_Code/main_zzl.py
@@ -1,6 +1,6 @@
import time
import serial
-# import ball_detection.ball_detection as ball_detection
+import ball_detection.ball_detection as ball_detection
import simple_pid.PID as PID
import timeit
@@ -102,7 +102,7 @@ def goal_detect(tx,ty):
'''
return True if April Tag is detected
'''
- if tx == 100000 and ty == 100000:
+ if tx == 0 and ty == 0:
return False
else:
return True
@@ -285,8 +285,8 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
goalDetect = goal_detect(tx, ty)
# debug
- ballCapture = 1
- goalDetect = 0
+ # ballCapture = 1
+ # goalDetect = 0
if ballCapture: # Ball captured
if goalDetect: # Goal detected
# stop_all() # Debug
@@ -305,7 +305,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
# ===== STEP 1: TAKE ALL INPUT =====
- # gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
line = serial_port.readline()
if line == b'SERIAL_IN_START\r\n':
@@ -322,11 +322,19 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di
time.sleep(waitTime)
+def auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
+
+ 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)
+ init_count += 1
+ return init_count
+
# ===== Main Function =====
if __name__ == '__main__':
# =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O
- PORT = "COM22" # for Alienware
+ PORT = "COM6" # for Alienware
serial_port = serial.Serial(PORT, 115200)
serial_port.close()
serial_port.open()
@@ -335,7 +343,7 @@ if __name__ == '__main__':
waitTime = 0.05
# Loading the PyTorch ML model for ball detection
- # model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
+ model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
# =========== DECLARE VARIABLES ===========
# ESP CAM In
@@ -353,11 +361,14 @@ if __name__ == '__main__':
dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
# Trigger the ESP32_SLAVE to talk first
- # gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ """
+ 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)
-
+ """
+ init_count = 0
# =========== LOOP FOREVER===========
while True:
+ if init_count == 0:
+ init_count = auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
-
diff --git a/Code/Control/Laptop_Code/previous_high_level/main_keyboard.py b/Code/Control/Laptop_Code/previous_high_level/main_keyboard.py
new file mode 100644
index 0000000000000000000000000000000000000000..ce27fa6b8eae25e1b900a181115529b711b5b544
--- /dev/null
+++ b/Code/Control/Laptop_Code/previous_high_level/main_keyboard.py
@@ -0,0 +1,548 @@
+import time
+import serial
+import pygame
+import simple_pid.PID as PID
+import cv2
+
+from constants import *
+
+global ml
+ml = 1
+if ml == 1:
+ import ball_detection.ball_detection as ball_detection
+
+# ========= 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
+ """
+ inputx = tx / 1.00
+ inputy = -1.00 * (ty + AT_goal_Delta) / 1.00 #
+
+ # April Tag Center
+ 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)
+
+ 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
+
+
+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
+
+ # 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 = 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)
+ ballCapture = 1
+ 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
+
+# ============= 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
+
+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,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
+ # ===== 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)
+
+ # ===== 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)
+
+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_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,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
+ # changed
+ gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
+ time.sleep(waitTime)
+
+
+def dynamic_variable(str_name_v):
+
+ global start_speed
+ global kpx,kix,kdx
+ global kpy,kiy,kdy
+ global kpx_g,kix_g,kdx_g
+ global kpy_g,kiy_g,kdy_g
+
+ 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))
+ elif str_name_v == "kpy":
+ kpx = input("Enter your value: ")
+ print("kpy:{}".format(kpy))
+ elif str_name_v == "kiy":
+ kix = input("Enter your value: ")
+ print("kiy:{}".format(kiy))
+ elif str_name_v == "kdy":
+ kdx = input("Enter your value: ")
+ print("kdy:{}".format(kdy))
+ if str_name_v == "kpx":
+ kpx = input("Enter your value: ")
+ print("kpx:{}".format(kpx))
+ elif str_name_v == "kix_g":
+ kix = input("Enter your value: ")
+ print("kix_g:{}".format(kix_g))
+ elif str_name_v == "kdx":
+ kdx = input("Enter your value: ")
+ print("kdx_g:{}".format(kdx_g))
+ elif str_name_v == "kpy_g":
+ kpx = input("Enter your value: ")
+ print("kpy_g:{}".format(kpy_g))
+ elif str_name_v == "kiy_g":
+ kix = input("Enter your value: ")
+ print("kiy_g:{}".format(kiy_g))
+ elif str_name_v == "kdy_g":
+ kdx = input("Enter your value: ")
+ print("kdy_g:{}".format(kdy_g))
+
+def variables_change_once():
+
+ str_name = input("Enter your variable: ")
+ dynamic_variable(str_name)
+
+def auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
+ if ml == 1:
+ 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)
+ init_count += 1
+ return init_count
+# ===== Main Function =====
+if __name__ == '__main__':
+ # =========== SET UP ============
+ # Defining Variables for ESP 32 Serial I/O
+ PORT = "COM6" # Based on your own serial port number
+ 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
+ if ml == 1:
+ model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
+
+ # =========== DECLARE VARIABLES ===========
+ # ESP CAM In
+ global gbx,gby,gb_dist
+ 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 = 100000, 100000, 100000 # 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 = '+', '+', '+', '+'
+
+ # Trigger the ESP32_SLAVE to talk first
+
+
+ Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
+ flag = 0
+ print_count = 1
+ init_count = 0
+ global start_speed
+ start_speed = 70
+
+ init()
+ # =========== LOOP FOREVER===========
+ while True:
+
+ if get_key('a'):
+ flag = 1
+ while (flag == 1):
+ if init_count == 0:
+ init_count = auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+
+ auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+ flag, print_count = keyboard_stop(flag,print_count)
+
+ elif get_key('s'):
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = stop_all()
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+ 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_once()
+ flag = 0
+ print_count = 1
+
+ elif get_key('k'):
+ break
+
+ if print_count is not 0:
+ print("No subsystem is running")
+ print_count = 0
diff --git a/Code/OpenMV Code/openMV_main.py b/Code/OpenMV Code/openMV_main.py
index a804a02c20cee598d8d138d42c75ab47a230166b..0507b79ffc7f0a7a09908347e581c313c68f6c5c 100644
--- a/Code/OpenMV Code/openMV_main.py
+++ b/Code/OpenMV Code/openMV_main.py
@@ -26,7 +26,7 @@ TAG_SIZE = 138 #length of inner black border of tag in mm (if printing to fill n
MAX_TAGS = 2
XRES = 320
YRES = 240
-SIZE = 16.3 #Set this to the size of the black square of apriltag in cm
+SIZE = 71 #Set this to the size of the black square of apriltag in cm
f_x = (2.8 / 3.673) * XRES # find_apriltags defaults to this if not set 3.984
f_y = (2.8 / 2.738) * YRES # find_apriltags defaults to this if not set
c_x = XRES * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)
@@ -298,4 +298,3 @@ interface.register_callback(goalfinder)
# processing remote events. interface.loop() does not return.
interface.loop()
-