From 189f1f7f6e692755c244958a4dae7daa3750f92b Mon Sep 17 00:00:00 2001
From: Zhiying Li <zhiyingli@g.ucla.edu>
Date: Wed, 27 Oct 2021 04:05:59 -0700
Subject: [PATCH] delete old code
---
.../Laptop_Code/ESP32_slave/ESP32_slave.ino | 321 ----------
Code/Control/Laptop_Code/main_keyboard.py | 547 ------------------
2 files changed, 868 deletions(-)
delete mode 100644 Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
delete mode 100644 Code/Control/Laptop_Code/main_keyboard.py
diff --git a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
deleted file mode 100644
index bca3ae2..0000000
--- a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
+++ /dev/null
@@ -1,321 +0,0 @@
-#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
-// uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xCF, 0x00};
-// uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
-// uint8_t broadcastAddress[] = {0xc4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}; // #3
-uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}; // #1
-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/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
deleted file mode 100644
index a7fc2b9..0000000
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ /dev/null
@@ -1,547 +0,0 @@
-import time
-import serial
-import ball_detection.ball_detection as ball_detection
-import simple_pid.PID as PID
-import timeit
-import pygame
-from constants import *
-
-global ml
-ml = 1
-# ========= 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)
-
- # debug
- # ballCapture = 1
- # goalDetect = 0
- 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
-
-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 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
-
-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 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):
-
- 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 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 = "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)
-
- # =========== 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 = 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 = '+', '+', '+', '+'
- Ctl_com = [0, 0, 0, 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)
- """
- flag = 0
- 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)
- 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 != 0:
- print("No subsystem is running")
- print_count = 0
--
GitLab