add move2ball() in main.py -- move2ball() pass individual test, no obvious bugs

Code/Control/Laptop_Code/constants.py

@@ -33,4 +33,10 @@ threshold = [Lmin, Lmax, Amin, Amax, Bmin, Bmax]
 base_speed    = 70
 seeking_speed = 70
 LIDAR_Thres   = 300 # mm
+
+
 base_speed = 70
+
+# PID Control
+kdx,kix,kpx = 2,0.1,0.25
+kdy,kiy,kpy = 1,0.1,0.25

Code/Control/Laptop_Code/main.py

 import time
 import serial
 import ball_detection.ball_detection as ball_detection
+import simple_pid.PID as PID
 
 from constants import *
 
-# ========= Serial Port I/) ===========
+# ========= Serial Port I/O ===========
 
 def serial_port_in(serial_port):
     '''
@@ -119,6 +120,10 @@ def ball_capture(LIDAR_dist):
         return False
 
 
+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):
@@ -136,7 +141,8 @@ def move2goal(tx, ty):
         dir1, dir2, dir3, dir4
     """
     pass
-    # return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+    # return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
 
 
 def seeking():
@@ -153,7 +159,7 @@ def seeking():
         dir1, dir2, dir3, dir4
     """
     pass
-    # return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+    # return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
 
 
 def move2ball(gbx,gby,gb_dist):
@@ -172,8 +178,55 @@ def move2ball(gbx,gby,gb_dist):
         dir1, dir2, dir3, dir4
     """
 
-    pass
-    # return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+    inputx = gbx / 1.00
+    inputy = gby / 1.00
+
+    # ESP 32 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)
+
+
+    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
+
 
 
 
@@ -193,6 +246,9 @@ 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
+    pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
+    dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
 
     ballDetect  = ball_detect(gbx, gby)
     ballCapture = ball_capture(LIDAR_dist)
@@ -210,10 +266,6 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
         else:  # Ball not detected
             seeking()
 
-    # placeholder
-    pwm1, pwm2, pwm3, pwm4 = 255 , 255 , 255 , 255
-    dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
-
     return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
 
 
@@ -233,8 +285,6 @@ if __name__ == '__main__':
 
     # 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
@@ -270,7 +320,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)