diff --git a/Code/Control/Laptop_Code/main.py b/Code/Control/Laptop_Code/main.py
index 89fafebbad2c30ba1a66a8e817a9352f50091449..8ce272caf98ce987307a4adfa7ff4d324b3b786b 100644
--- a/Code/Control/Laptop_Code/main.py
+++ b/Code/Control/Laptop_Code/main.py
@@ -2,6 +2,8 @@ import time
import serial
import ball_detection.ball_detection as ball_detection
import simple_pid.PID as PID
+import timeit
+
from constants import *
@@ -70,25 +72,25 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
Output:
None
'''
+
output_message = ''
for pwm_itr in [pwm1, pwm2, pwm3, pwm4]:
- print(pwm_itr)
+ # print(pwm_itr)
if len(str(pwm_itr)) == 2:
output_message += '0'
elif len(str(pwm_itr)) == 1:
output_message += '00'
output_message += str(pwm_itr)
+ print(pwm_itr)
output_message = output_message + dir1 + dir2 + dir3 + dir4 + '\n'
- serial_port.write(output_message.encode())
-
- # DEBUG Verbose
print("serial out ...")
print(output_message)
+ serial_port.write(output_message.encode())
-# ====== Logic-directing Functions ====
+# ====== supporting function in main control ====
def ball_detect(gbx, gby):
'''
return True if green ball is detected
@@ -116,13 +118,11 @@ def ball_capture(LIDAR_dist):
else:
return False
-# ======= Motion-based Functions ======
def stop_all():
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-
def move2goal(tx, ty):
"""
Description:
@@ -136,10 +136,11 @@ def move2goal(tx, ty):
Output:
pwm1, pwm2, pwm3, pwm4
dir1, dir2, dir3, dir4
- """
+ """
pass
- # return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-
+
+ # return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
def seeking():
"""
@@ -159,7 +160,8 @@ def seeking():
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
-def move2ball(gbx,gby,gb_dist):
+
+def move2ball(gbx, gby, gb_dist):
"""
Description:
Given the center of x y dist of green ball detected. Call PID control to
@@ -177,49 +179,53 @@ def move2ball(gbx,gby,gb_dist):
inputx = gbx / 1.00
inputy = gby / 1.00
- # ESP-Cam Center
setpoint_x = 400
- setpoint_y = 300
-
- pid_x = PID(kdx,kix,kpx,setpoint = setpoint_x)
- pid_y = PID(kdy,kiy,kpy,setpoint = setpoint_y)
-
+ setpoint_y = 300 # ESP 32 Cam Center
+
+ pid_x = PID(kpx,kix,kdx,setpoint=setpoint_x)
+ pid_y = PID(kpy,kiy,kdy,setpoint=setpoint_y)
+
+
pid_x.auto_mode = True
- pid_x.set_auto_mode(True, last_output = 8.0)
+ pid_x.set_auto_mode(True, last_output=8.0)
pid_x.output_limits = (-255,255)
pid_y.output_limits = (-255,255)
-
+
+
outputx = pid_x(inputx)
outputy = pid_y(inputy)
-
- # Vertical
+
+ # vertical
pwm1 = abs(outputy)
pwm2 = abs(outputy)
-
+
if(outputy > 0):
dir1 = '+'
dir2 = '+'
else:
dir1 = '-'
dir2 = '-'
-
- # Horizontal
+
+ # horizontal
lspeed = -1 * outputx + base_speed
rspeed = 1 * outputx + base_speed
- pwm3 = abs(lspeed)
- pwm4 = abs(rspeed)
+ pwm3 = min( abs(lspeed), 255)
+ pwm4 = min( abs(rspeed), 255)
if (lspeed > 0):
dir3 = '+'
else:
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):
@@ -236,23 +242,28 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
Output:
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
'''
- # placeholder
+
+ # # placeholder
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
ballDetect = ball_detect(gbx, gby)
ballCapture = ball_capture(LIDAR_dist)
goalDetect = goal_detect(tx,ty)
-
+ # debug
+ ballCapture = 0
if ballCapture: # Ball captured
if goalDetect: # Goal detected
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
- else: # Goal not detectedpwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 =
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
+ stop_all()
+ #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
+ else: # Goal not detected
+ stop_all()
+ #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
else: # Ball not captured
if ballDetect: # Ball detected
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
else: # Ball not detected
+ # stop_all()
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
@@ -264,16 +275,18 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
if __name__ == '__main__':
# =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O
- PORT = "COM5" # for Alienware
+ PORT = "COM6" # for Alienware
serial_port = serial.Serial(PORT, 115200)
serial_port.close()
serial_port.open()
# Weit Time
- waitTime = 0.10
+ waitTime = 0.05
# Loading the PyTorch ML model for ball detection
model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
+ #model = ball_detection.returnModel(modelAction, device, trainLoc, labelSet, modelLoc, modelFile)
+
# =========== DECLARE VARIABLES ===========
# ESP CAM In
@@ -290,27 +303,25 @@ if __name__ == '__main__':
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
- count = 0
# =========== LOOP FOREVER===========
+ # ESP32_SLAVE Talk First
+ gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
while True:
+ # ===== STEP 1: TAKE ALL INPUT =====
gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
- print("gbx, gby: {}, {}".format(gbx, gby))
-
line = serial_port.readline()
+
if line == b'SERIAL_IN_START\r\n':
- # ===== STEP 1: TAKE ALL INPUT =====
tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
+ print("gbx,gby:{},{}".format(gbx,gby))
- # ===== STEP 2: MAIN CONTROL LOOP =====
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+ # ===== STEP 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)
+ # ===== STEP 3: FEED ALL OUTPUT =====
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
- if count == 0:
- # first time calling (call once)
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
- serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
- # count +=1
\ No newline at end of file
+ time.sleep(waitTime)