Skip to content
Snippets Groups Projects
Commit f2c19694 authored by Aaron John Sabu's avatar Aaron John Sabu
Browse files

Removed redundant code

parent 3b619762
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Code/Control/Laptop_Code/main_backup/main.py
+ 27
− 38
View file @ f2c19694
...@@ -4,7 +4,6 @@ import ball_detection.ball_detection as ball_detection ...@@ -4,7 +4,6 @@ import ball_detection.ball_detection as ball_detection
import simple_pid.PID as PID import simple_pid.PID as PID
import timeit import timeit
from constants import * from constants import *
# ========= Serial Port I/O =========== # ========= Serial Port I/O ===========
...@@ -72,7 +71,6 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4) ...@@ -72,7 +71,6 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
Output: Output:
None None
''' '''
output_message = '' output_message = ''
for pwm_itr in [pwm1, pwm2, pwm3, pwm4]: for pwm_itr in [pwm1, pwm2, pwm3, pwm4]:
...@@ -90,7 +88,7 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4) ...@@ -90,7 +88,7 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
serial_port.write(output_message.encode()) serial_port.write(output_message.encode())
# ====== supporting function in main control ==== # ====== Logic-directing Functions ====
def ball_detect(gbx, gby): def ball_detect(gbx, gby):
''' '''
return True if green ball is detected return True if green ball is detected
...@@ -160,7 +158,6 @@ def seeking(): ...@@ -160,7 +158,6 @@ def seeking():
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4 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: Description:
...@@ -179,23 +176,22 @@ def move2ball(gbx, gby, gb_dist): ...@@ -179,23 +176,22 @@ def move2ball(gbx, gby, gb_dist):
inputx = gbx / 1.00 inputx = gbx / 1.00
inputy = gby / 1.00 inputy = gby / 1.00
# ESP-Cam Center
setpoint_x = 400 setpoint_x = 400
setpoint_y = 300 # ESP 32 Cam Center setpoint_y = 300
pid_x = PID(kpx,kix,kdx,setpoint=setpoint_x)
pid_y = PID(kpy,kiy,kdy,setpoint=setpoint_y)
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.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_x.output_limits = (-255,255)
pid_y.output_limits = (-255,255) pid_y.output_limits = (-255,255)
outputx = pid_x(inputx) outputx = pid_x(inputx)
outputy = pid_y(inputy) outputy = pid_y(inputy)
# vertical # Vertical
pwm1 = abs(outputy) pwm1 = abs(outputy)
pwm2 = abs(outputy) pwm2 = abs(outputy)
...@@ -209,25 +205,21 @@ def move2ball(gbx, gby, gb_dist): ...@@ -209,25 +205,21 @@ def move2ball(gbx, gby, gb_dist):
# horizontal # horizontal
lspeed = -1 * outputx + base_speed lspeed = -1 * outputx + base_speed
rspeed = 1 * outputx + base_speed rspeed = 1 * outputx + base_speed
pwm3 = min( abs(lspeed), 255) pwm3 = min(abs(lspeed), 255)
pwm4 = min( abs(rspeed), 255) pwm4 = min(abs(rspeed), 255)
if (lspeed > 0): if (lspeed > 0):
dir3 = '+' dir3 = '+'
else: else:
dir3 = '-' dir3 = '-'
if (rspeed > 0): if (rspeed > 0):
dir4 = '+' dir4 = '+'
else: else:
dir4 = '-' dir4 = '-'
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4 return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
# =========== Main Control ===========
# =========== main control ===========
def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM): def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
''' '''
Description: Description:
...@@ -242,36 +234,33 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM): ...@@ -242,36 +234,33 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
Output: Output:
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
''' '''
# placeholder
# # placeholder
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
dir1, dir2, dir3, dir4 = '+', '-', '+', '-' dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
ballDetect = ball_detect(gbx, gby) ballDetect = ball_detect(gbx, gby)
ballCapture = ball_capture(LIDAR_dist) ballCapture = ball_capture(LIDAR_dist)
goalDetect = goal_detect(tx,ty) goalDetect = goal_detect(tx, ty)
# debug
ballCapture = 0 ballCapture = 0 # debug
if ballCapture: # Ball captured if ballCapture: # Ball captured
if goalDetect: # Goal detected if goalDetect: # Goal detected
stop_all() stop_all() # Debug
#pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty) # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
else: # Goal not detected else: # Goal not detected
stop_all() stop_all() # Debug
#pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking() # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
else: # Ball not captured else: # Ball not captured
if ballDetect: # Ball detected if ballDetect: # Ball detected
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist) pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
else: # Ball not detected else: # Ball not detected
# stop_all() # stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking() pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
# ===== Main Function =====
# ===== Main Functions =====
if __name__ == '__main__': if __name__ == '__main__':
# =========== SET UP ============ # =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O # Defining Variables for ESP 32 Serial I/O
...@@ -280,12 +269,12 @@ if __name__ == '__main__': ...@@ -280,12 +269,12 @@ if __name__ == '__main__':
serial_port.close() serial_port.close()
serial_port.open() serial_port.open()
# Weit Time # Wait Time
waitTime = 0.05 waitTime = 0.05
# Loading the PyTorch ML model for ball detection # Loading the PyTorch ML model for ball detection
model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile) model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
#model = ball_detection.returnModel(modelAction, device, trainLoc, labelSet, modelLoc, modelFile) # model = ball_detection.returnModel(modelAction, device, trainLoc, labelSet, modelLoc, modelFile)
# =========== DECLARE VARIABLES =========== # =========== DECLARE VARIABLES ===========
...@@ -303,12 +292,12 @@ if __name__ == '__main__': ...@@ -303,12 +292,12 @@ if __name__ == '__main__':
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
dir1, dir2, dir3, dir4 = '+', '+', '+', '+' dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
# =========== LOOP FOREVER=========== # ESP32_SLAVE talks first
# ESP32_SLAVE 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) 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) serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
# =========== LOOP FOREVER===========
while True: while True:
# ===== STEP 1: TAKE ALL INPUT ===== # ===== 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)
...@@ -324,4 +313,4 @@ if __name__ == '__main__': ...@@ -324,4 +313,4 @@ if __name__ == '__main__':
# ===== STEP 3: FEED ALL OUTPUT ===== # ===== STEP 3: FEED ALL OUTPUT =====
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4) serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
time.sleep(waitTime) time.sleep(waitTime)
\ No newline at end of file
Code/Control/Laptop_Code/main_backup/main_aaron_move2goal.py deleted 100644 → 0
+ 0
− 362
View file @ 3b619762
import time
import serial
import ball_detection.ball_detection as ball_detection
import simple_pid.PID as PID
from constants import *
# ========= 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)
output_message = output_message + dir1 + dir2 + dir3 + dir4 + '\n'
serial_port.write(output_message.encode())
# DEBUG Verbose
print("serial out ...")
print(output_message)
# ====== 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
# ======= 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:
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 = ty / 1.00
# April Tag Center
setpoint_x1 = 160
setpoint_y1 = 120
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(kdx_b, kix_b, kpx_b, setpoint = setpoint_x)
pid_y = PID(kdx_b, kiy_b, kpy_b, 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 = 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
# =========== 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)
print("ballDetect = ", ballDetect)
print("ballCapture = ", ballCapture)
print("goalDetect = ", goalDetect)
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()
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
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
# ===== Main Functions =====
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.10
# 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 = 0, 0, 0 # 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 = '+', '+', '+', '+'
count = 0
# =========== LOOP FOREVER===========
while True:
# 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)
# ===== 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)
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
Code/Control/Laptop_Code/main_integrated_test.py
+ 18
− 33
View file @ f2c19694
...@@ -72,7 +72,6 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4) ...@@ -72,7 +72,6 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
Output: Output:
None None
''' '''
output_message = '' output_message = ''
for pwm_itr in [pwm1, pwm2, pwm3, pwm4]: for pwm_itr in [pwm1, pwm2, pwm3, pwm4]:
...@@ -90,7 +89,7 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4) ...@@ -90,7 +89,7 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
serial_port.write(output_message.encode()) serial_port.write(output_message.encode())
# ====== supporting function in main control ==== # ====== Logic-directing Functions ====
def ball_detect(gbx, gby): def ball_detect(gbx, gby):
''' '''
return True if green ball is detected return True if green ball is detected
...@@ -123,7 +122,6 @@ def stop_all(): ...@@ -123,7 +122,6 @@ def stop_all():
dir1, dir2, dir3, dir4 = '+', '-', '+', '-' dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
def move2goal(tx, ty): def move2goal(tx, ty):
""" """
Description: Description:
...@@ -184,8 +182,6 @@ def move2goal(tx, ty): ...@@ -184,8 +182,6 @@ def move2goal(tx, ty):
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4 return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
def seeking(): def seeking():
""" """
Description: Description:
...@@ -223,23 +219,22 @@ def move2ball(gbx, gby, gb_dist): ...@@ -223,23 +219,22 @@ def move2ball(gbx, gby, gb_dist):
inputx = gbx / 1.00 inputx = gbx / 1.00
inputy = gby / 1.00 inputy = gby / 1.00
# ESP-Cam Center
setpoint_x = 400 setpoint_x = 400
setpoint_y = 300 # ESP 32 Cam Center setpoint_y = 300
pid_x = PID(kpx,kix,kdx,setpoint=setpoint_x)
pid_y = PID(kpy,kiy,kdy,setpoint=setpoint_y)
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.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_x.output_limits = (-255,255)
pid_y.output_limits = (-255,255) pid_y.output_limits = (-255,255)
outputx = pid_x(inputx) outputx = pid_x(inputx)
outputy = pid_y(inputy) outputy = pid_y(inputy)
# vertical # Vertical
pwm1 = abs(outputy) pwm1 = abs(outputy)
pwm2 = abs(outputy) pwm2 = abs(outputy)
...@@ -250,28 +245,24 @@ def move2ball(gbx, gby, gb_dist): ...@@ -250,28 +245,24 @@ def move2ball(gbx, gby, gb_dist):
dir1 = '-' dir1 = '-'
dir2 = '-' dir2 = '-'
# horizontal # Horizontal
lspeed = -1 * outputx + base_speed lspeed = -1 * outputx + base_speed
rspeed = 1 * outputx + base_speed rspeed = 1 * outputx + base_speed
pwm3 = min( abs(lspeed), 255) pwm3 = min(abs(lspeed), 255)
pwm4 = min( abs(rspeed), 255) pwm4 = min(abs(rspeed), 255)
if (lspeed > 0): if (lspeed > 0):
dir3 = '+' dir3 = '+'
else: else:
dir3 = '-' dir3 = '-'
if (rspeed > 0): if (rspeed > 0):
dir4 = '+' dir4 = '+'
else: else:
dir4 = '-' dir4 = '-'
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4 return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
# =========== Main Control ===========
# =========== main control ===========
def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM): def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
''' '''
Description: Description:
...@@ -286,8 +277,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM): ...@@ -286,8 +277,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
Output: Output:
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
''' '''
# placeholder
# # placeholder
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
dir1, dir2, dir3, dir4 = '+', '-', '+', '-' dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
...@@ -295,27 +285,24 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM): ...@@ -295,27 +285,24 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
ballCapture = ball_capture(LIDAR_dist) ballCapture = ball_capture(LIDAR_dist)
goalDetect = goal_detect(tx,ty) goalDetect = goal_detect(tx,ty)
# debug
if ballCapture: # Ball captured if ballCapture: # Ball captured
if goalDetect: # Goal detected if goalDetect: # Goal detected
#stop_all() # stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty) pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
else: # Goal not detected else: # Goal not detected
#stop_all() # stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking() pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
else: # Ball not captured else: # Ball not captured
if ballDetect: # Ball detected if ballDetect: # Ball detected
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist) pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
else: # Ball not detected else: # Ball not detected
# stop_all() # stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking() pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
# ===== Main Function =====
# ===== Main Functions =====
if __name__ == '__main__': if __name__ == '__main__':
# =========== SET UP ============ # =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O # Defining Variables for ESP 32 Serial I/O
...@@ -346,21 +333,19 @@ if __name__ == '__main__': ...@@ -346,21 +333,19 @@ if __name__ == '__main__':
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
dir1, dir2, dir3, dir4 = '+', '+', '+', '+' dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
# =========== LOOP FOREVER=========== # Trigger the ESP32_SLAVE to talk first
# Trigger the ESP32_SLAVE 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) 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) serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
# =========== LOOP FOREVER===========
while True: while True:
# ===== STEP 1: TAKE ALL INPUT ===== # ===== 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() line = serial_port.readline()
if line == b'SERIAL_IN_START\r\n': if line == b'SERIAL_IN_START\r\n':
tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port) tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
print("gbx,gby:{},{}".format(gbx,gby)) print("gbx,gby:{},{}".format(gbx,gby))
time.sleep(waitTime) time.sleep(waitTime)
...@@ -370,4 +355,4 @@ if __name__ == '__main__': ...@@ -370,4 +355,4 @@ if __name__ == '__main__':
# ===== STEP 3: FEED ALL OUTPUT ===== # ===== STEP 3: FEED ALL OUTPUT =====
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4) serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
time.sleep(waitTime) time.sleep(waitTime)
\ No newline at end of file
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