Merge remote-tracking branch 'origin/main' into main

Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino

+#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 
+}

Code/Control/Laptop_Code/basic_comm_test/ESP32_master/ESP32_master.ino

@@ -5,11 +5,13 @@
 // ==================================== broadcast address ============================================
 // MAC Address of the receiver (SLAVE)
 //uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}; // #1
-// uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xBD, 0xF8}; // #2
+//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[] = {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
 // ==================================== global data =================================================
 String success;
 // Define variables to store BME280 readings to be sent

Code/Control/Laptop_Code/main_keyboard.py

+import time
+import serial
+import pygame
+import simple_pid.PID as PID
+import cv2
+
+from constants import *
+
+global ml
+ml = 0
+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 == 100000 and ty == 100000:
+        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
+
+    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)
+
+# ===== Main Function =====
+if __name__ == '__main__':
+    # =========== SET UP ============
+    # Defining Variables for ESP 32 Serial I/O
+    PORT = "COM3" # 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
+    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)
+
+    Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
+    flag = 0
+    print_count = 1
+    global start_speed
+    start_speed = 70
+
+    init()
+    # =========== LOOP FOREVER===========
+    while True:
+
+        if get_key('a'):
+            flag = 1
+            while (flag == 1):
+                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

Code/IMU_Code/IMU_Code.ino

+// Basic demo for magnetometer readings from Adafruit LIS3MDL
+
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_LIS3MDL.h>
+#include <Adafruit_LSM6DSOX.h>
+
+Adafruit_LIS3MDL lis3mdl;
+Adafruit_LSM6DSOX sox;
+
+#define LSM_CS 10
+#define LSM_MOSI 11
+#define LSM_MISO 12
+#define LSM_SCK 13
+
+#define LIS3MDL_CS 10
+#define LIS3MDL_MOSI 11
+#define LIS3MDL_MISO 12
+#define LIS3MDL_CLK 13
+
+double posX     =  0.00;
+double posY     =  0.00;
+double posZ     =  0.00;
+double velX     =  0.00;
+double velY     =  0.00;
+double velZ     =  0.00;
+double accX     =  0.00;
+double accY     =  0.00;
+double accZ     =  0.00;
+double gX       =  0.00;
+double gY       =  0.00;
+double gZ       = 10.00;
+double prevTime =  0.00;
+
+void setup(void) {
+  Serial.begin(115200);
+  while (!Serial) delay(10);     // will pause Zero, Leonardo, etc until serial console opens
+
+  
+  // Try to initialize!
+  if (!sox.begin_I2C()) {
+    // if (!sox.begin_SPI(LSM_CS)) {
+    // if (!sox.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) {
+    Serial.println("Failed to find LSM6DSOX chip");
+    while (1) {
+      delay(10);
+    }
+  }
+  if (!lis3mdl.begin_I2C()) {          // hardware I2C mode, can pass in address & alt Wire
+  //if (!lis3mdl.begin_SPI(LIS3MDL_CS)) {  // hardware SPI mode
+  //if (!lis3mdl.begin_SPI(LIS3MDL_CS, LIS3MDL_CLK, LIS3MDL_MISO, LIS3MDL_MOSI)) { // soft SPI
+    Serial.println("Failed to find LIS3MDL chip");
+    while (1) { delay(10); }
+  }
+  Serial.println("LIS3MDL Found!");
+
+  // sox.setAccelRange(LSM6DS_ACCEL_RANGE_2_G);
+  Serial.print("Accelerometer range set to: ");
+  switch (sox.getAccelRange()) {
+  case LSM6DS_ACCEL_RANGE_2_G:
+    Serial.println("+-2G");
+    break;
+  case LSM6DS_ACCEL_RANGE_4_G:
+    Serial.println("+-4G");
+    break;
+  case LSM6DS_ACCEL_RANGE_8_G:
+    Serial.println("+-8G");
+    break;
+  case LSM6DS_ACCEL_RANGE_16_G:
+    Serial.println("+-16G");
+    break;
+  }
+
+  // sox.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS );
+  Serial.print("Gyro range set to: ");
+  switch (sox.getGyroRange()) {
+  case LSM6DS_GYRO_RANGE_125_DPS:
+    Serial.println("125 degrees/s");
+    break;
+  case LSM6DS_GYRO_RANGE_250_DPS:
+    Serial.println("250 degrees/s");
+    break;
+  case LSM6DS_GYRO_RANGE_500_DPS:
+    Serial.println("500 degrees/s");
+    break;
+  case LSM6DS_GYRO_RANGE_1000_DPS:
+    Serial.println("1000 degrees/s");
+    break;
+  case LSM6DS_GYRO_RANGE_2000_DPS:
+    Serial.println("2000 degrees/s");
+    break;
+  case ISM330DHCX_GYRO_RANGE_4000_DPS:
+    break; // unsupported range for the DSOX
+  }
+
+  // sox.setAccelDataRate(LSM6DS_RATE_12_5_HZ);
+  Serial.print("Accelerometer data rate set to: ");
+  switch (sox.getAccelDataRate()) {
+  case LSM6DS_RATE_SHUTDOWN:
+    Serial.println("0 Hz");
+    break;
+  case LSM6DS_RATE_12_5_HZ:
+    Serial.println("12.5 Hz");
+    break;
+  case LSM6DS_RATE_26_HZ:
+    Serial.println("26 Hz");
+    break;
+  case LSM6DS_RATE_52_HZ:
+    Serial.println("52 Hz");
+    break;
+  case LSM6DS_RATE_104_HZ:
+    Serial.println("104 Hz");
+    break;
+  case LSM6DS_RATE_208_HZ:
+    Serial.println("208 Hz");
+    break;
+  case LSM6DS_RATE_416_HZ:
+    Serial.println("416 Hz");
+    break;
+  case LSM6DS_RATE_833_HZ:
+    Serial.println("833 Hz");
+    break;
+  case LSM6DS_RATE_1_66K_HZ:
+    Serial.println("1.66 KHz");
+    break;
+  case LSM6DS_RATE_3_33K_HZ:
+    Serial.println("3.33 KHz");
+    break;
+  case LSM6DS_RATE_6_66K_HZ:
+    Serial.println("6.66 KHz");
+    break;
+  }
+
+  // sox.setGyroDataRate(LSM6DS_RATE_12_5_HZ);
+  Serial.print("Gyro data rate set to: ");
+  switch (sox.getGyroDataRate()) {
+  case LSM6DS_RATE_SHUTDOWN:
+    Serial.println("0 Hz");
+    break;
+  case LSM6DS_RATE_12_5_HZ:
+    Serial.println("12.5 Hz");
+    break;
+  case LSM6DS_RATE_26_HZ:
+    Serial.println("26 Hz");
+    break;
+  case LSM6DS_RATE_52_HZ:
+    Serial.println("52 Hz");
+    break;
+  case LSM6DS_RATE_104_HZ:
+    Serial.println("104 Hz");
+    break;
+  case LSM6DS_RATE_208_HZ:
+    Serial.println("208 Hz");
+    break;
+  case LSM6DS_RATE_416_HZ:
+    Serial.println("416 Hz");
+    break;
+  case LSM6DS_RATE_833_HZ:
+    Serial.println("833 Hz");
+    break;
+  case LSM6DS_RATE_1_66K_HZ:
+    Serial.println("1.66 KHz");
+    break;
+  case LSM6DS_RATE_3_33K_HZ:
+    Serial.println("3.33 KHz");
+    break;
+  case LSM6DS_RATE_6_66K_HZ:
+    Serial.println("6.66 KHz");
+    break;
+  }
+  
+  lis3mdl.setPerformanceMode(LIS3MDL_MEDIUMMODE);
+  Serial.print("Performance mode set to: ");
+  switch (lis3mdl.getPerformanceMode()) {
+    case LIS3MDL_LOWPOWERMODE: Serial.println("Low"); break;
+    case LIS3MDL_MEDIUMMODE: Serial.println("Medium"); break;
+    case LIS3MDL_HIGHMODE: Serial.println("High"); break;
+    case LIS3MDL_ULTRAHIGHMODE: Serial.println("Ultra-High"); break;
+  }
+
+  lis3mdl.setOperationMode(LIS3MDL_CONTINUOUSMODE);
+  Serial.print("Operation mode set to: ");
+  // Single shot mode will complete conversion and go into power down
+  switch (lis3mdl.getOperationMode()) {
+    case LIS3MDL_CONTINUOUSMODE: Serial.println("Continuous"); break;
+    case LIS3MDL_SINGLEMODE: Serial.println("Single mode"); break;
+    case LIS3MDL_POWERDOWNMODE: Serial.println("Power-down"); break;
+  }
+
+  lis3mdl.setDataRate(LIS3MDL_DATARATE_155_HZ);
+  // You can check the datarate by looking at the frequency of the DRDY pin
+  Serial.print("Data rate set to: ");
+  switch (lis3mdl.getDataRate()) {
+    case LIS3MDL_DATARATE_0_625_HZ: Serial.println("0.625 Hz"); break;
+    case LIS3MDL_DATARATE_1_25_HZ: Serial.println("1.25 Hz"); break;
+    case LIS3MDL_DATARATE_2_5_HZ: Serial.println("2.5 Hz"); break;
+    case LIS3MDL_DATARATE_5_HZ: Serial.println("5 Hz"); break;
+    case LIS3MDL_DATARATE_10_HZ: Serial.println("10 Hz"); break;
+    case LIS3MDL_DATARATE_20_HZ: Serial.println("20 Hz"); break;
+    case LIS3MDL_DATARATE_40_HZ: Serial.println("40 Hz"); break;
+    case LIS3MDL_DATARATE_80_HZ: Serial.println("80 Hz"); break;
+    case LIS3MDL_DATARATE_155_HZ: Serial.println("155 Hz"); break;
+    case LIS3MDL_DATARATE_300_HZ: Serial.println("300 Hz"); break;
+    case LIS3MDL_DATARATE_560_HZ: Serial.println("560 Hz"); break;
+    case LIS3MDL_DATARATE_1000_HZ: Serial.println("1000 Hz"); break;
+  }
+  
+  lis3mdl.setRange(LIS3MDL_RANGE_4_GAUSS);
+  Serial.print("Range set to: ");
+  switch (lis3mdl.getRange()) {
+    case LIS3MDL_RANGE_4_GAUSS: Serial.println("+-4 gauss"); break;
+    case LIS3MDL_RANGE_8_GAUSS: Serial.println("+-8 gauss"); break;
+    case LIS3MDL_RANGE_12_GAUSS: Serial.println("+-12 gauss"); break;
+    case LIS3MDL_RANGE_16_GAUSS: Serial.println("+-16 gauss"); break;
+  }
+
+  lis3mdl.setIntThreshold(500);
+  lis3mdl.configInterrupt(false, false, true, // enable z axis
+                          true, // polarity
+                          false, // don't latch
+                          true); // enabled!
+
+  prevTime = millis();
+  Serial.print("prevTime = ");
+  Serial.println(prevTime);
+}
+
+void loop() {
+  //  /* Get a new normalized sensor event */
+  sensors_event_t accel;
+  sensors_event_t gyro;
+  sensors_event_t temp;
+  sox.getEvent(&accel, &gyro, &temp);
+
+//  Serial.print("\t\tTemperature ");
+//  Serial.print(temp.temperature);
+//  Serial.println(" deg C");
+
+  /* Display the results (distance is measured in m) */
+  double currTime = millis();
+  double diffTime = (currTime - prevTime)/1000.0;
+  prevTime = currTime;
+
+  Serial.println();
+  Serial.println(currTime);
+  Serial.println(diffTime);
+  
+  accX = accel.acceleration.x - gX;
+  accY = accel.acceleration.y - gY;
+  accZ = accel.acceleration.z - gZ;
+  velX = velX + accX*diffTime;
+  velY = velY + accY*diffTime;
+  velZ = velZ + accZ*diffTime;
+  posX = posX + velX*diffTime - 0.5*accX*diffTime*diffTime;
+  posY = posY + velY*diffTime - 0.5*accY*diffTime*diffTime;
+  posZ = posZ + velZ*diffTime - 0.5*accZ*diffTime*diffTime;
+
+  Serial.print(" \tX: ");
+  Serial.print(posX);
+  Serial.print(" \tY: ");
+  Serial.print(posY);
+  Serial.print(" \tZ: ");
+  Serial.print(posZ);
+//  Serial.println(" m/s^2 ");
+
+  /* Display the results (rotation is measured in rad/s) */
+//  Serial.print("\t\tGyro X: ");
+//  Serial.print(gyro.gyro.x);
+//  Serial.print(" \tY: ");
+//  Serial.print(gyro.gyro.y);
+//  Serial.print(" \tZ: ");
+//  Serial.print(gyro.gyro.z);
+//  Serial.println(" radians/s ");
+//  Serial.println();
+
+//  delay(100);
+
+  lis3mdl.read();      // get X Y and Z data at once
+  // Then print out the raw data
+//  Serial.print("\nX: "); Serial.print(lis3mdl.x); 
+//  Serial.print("  \tY: "); Serial.print(lis3mdl.y); 
+//  Serial.print("  \tZ: "); Serial.println(lis3mdl.z); 
+
+  /* Or....get a new sensor event, normalized to uTesla */
+  sensors_event_t event; 
+  lis3mdl.getEvent(&event);
+  /* Display the results (magnetic field is measured in uTesla) */
+//  Serial.print("\tX: "); Serial.print(event.magnetic.x);
+//  Serial.print(" \tY: "); Serial.print(event.magnetic.y); 
+//  Serial.print(" \tZ: "); Serial.print(event.magnetic.z); 
+//  Serial.println(" uTesla ");
+
+//  delay(100); 
+  Serial.println();
+}