fixed some lagging bugs

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
+        time.sleep(waitTime)
\ No newline at end of file