main_zhiying is for debugging purpose

Code/Control/Laptop_Code/main_zhiying.py

@@ -5,7 +5,7 @@ import simple_pid.PID as PID
 
 from constants import *
 
-# ========= Serial Port I/) ===========
+# ========= Serial Port I/O ===========
 
 def serial_port_in(serial_port):
     '''
@@ -74,7 +74,7 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
     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:
@@ -83,7 +83,7 @@ def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
         print(pwm_itr)
 
     output_message = output_message + dir1 + dir2 +  dir3 +  dir4 + '\n'
-    # serial_port.write(output_message.encode())
+    serial_port.write(output_message.encode())
 
     # DEBUG Verbose
     print("serial out ...")
@@ -181,8 +181,8 @@ def move2ball(gbx, gby, gb_dist):
     kdx,kix,kpx = 2,0.1,0.25
     kdy,kiy,kpy = 1,0.1,0.25
 
-    Inputx = gbx / 1.00
-    Inputy = gby / 1.00
+    inputx = gbx / 1.00
+    inputy = gby / 1.00
 
     setpoint_x = 400
     setpoint_y = 300  # ESP 32 Cam Center
@@ -249,25 +249,26 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
     '''
 
     # placeholder
-    pwm1, pwm2, pwm3, pwm4 = 255 , 255 , 255 , 255
+    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
-            stop_all():
+            stop_all()
             #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
         else:  # Goal not detected
-            stop_all():
+            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():
+            stop_all()
             #pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
 
 
@@ -281,7 +282,7 @@ 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()
@@ -328,7 +329,8 @@ if __name__ == '__main__':
 
         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
+            # count +=1
 
         time.sleep(waitTime)