Skip to content
Snippets Groups Projects
Commit 189f1f7f authored by Zhiying Li's avatar Zhiying Li
Browse files

delete old code

parent 40c610d9
Branches
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino deleted 100644 → 0
+ 0
− 321
View file @ 40c610d9
#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
}
Code/Control/Laptop_Code/main_keyboard.py deleted 100644 → 0
+ 0
− 547
View file @ 40c610d9
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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment