Change the typo bug of pidy

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)