diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py index a7fc2b9cfc4a8602e06d875df5254fc610c8abec..8bc5633946e62bb5d1b6ca90092cc686f11abd16 100644 --- a/Code/Control/Laptop_Code/main_keyboard.py +++ b/Code/Control/Laptop_Code/main_keyboard.py @@ -104,7 +104,7 @@ def goal_detect(tx,ty): ''' return True if April Tag is detected ''' - if tx == 0 and ty == 0: + if tx == 100000 and ty == 100000: return False else: return True @@ -144,8 +144,8 @@ def move2goal(tx, ty): 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 = PID(kpx_g, kix_g, kdx_g, setpoint = setpoint_x1) + pid_y = PID(kpy_g, kiy_g, kdy_g, setpoint = setpoint_y1) pid_x.auto_mode = True pid_x.set_auto_mode(True, last_output = 8.0) @@ -286,6 +286,8 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM): ballCapture = ball_capture(LIDAR_dist) goalDetect = goal_detect(tx, ty) + ballCapture = 1 + # debug # ballCapture = 1 # goalDetect = 0 @@ -307,7 +309,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM): 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) + # gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True) line = serial_port.readline() if line == b'SERIAL_IN_START\r\n': @@ -326,7 +328,7 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di 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) + # 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 @@ -437,12 +439,15 @@ def manual_control(Ctl_com,serial_port): elif get_key("RIGHT"): val = start_speed Ctl_com = manual_command(0, 0, val, val, "+", "+", "+", "-") + elif get_key("r"): + val = start_speed + Ctl_com = manual_command( val, val,0,0, "+", "-", "+", "+") 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) + # gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True) serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)