Merge branch 'main' of git.uclalemur.com:shahrulkamil98/november-2021-blimp-competition

Code/Ball_Detection/PyTorch_with_ESPCAM/imageTread.py

@@ -6,31 +6,31 @@ def nothing(x):
     pass
 
 
+if __name__ == "__main__":
+    #change the IP address below according to the
+    #IP shown in the Serial monitor of Arduino code
+    # url='http://192.168.4.1/cam-hi.jpg'
+    # url='http://192.168.1.107/cam-hi.jpg'
+    url='http://192.168.4.1/cam-mid.jpg'
 
-#change the IP address below according to the
-#IP shown in the Serial monitor of Arduino code
-url='http://192.168.4.1/cam-hi.jpg'
-# url='http://192.168.1.107/cam-hi.jpg'
-# url='http://192.168.1.107/cam-mid.jpg'
 
+    # cv2.namedWindow("live transmission", cv2.WINDOW_AUTOSIZE)
 
-# cv2.namedWindow("live transmission", cv2.WINDOW_AUTOSIZE)
+    cv2.namedWindow("live transmission", cv2.WINDOW_NORMAL)
 
-cv2.namedWindow("live transmission", cv2.WINDOW_NORMAL)
+    while True:
+        header = {"User-Agent": "Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/92.0.4515.159 Safari/537.36."}
+        req = Request(url, headers=header)
+        img_resp = urlopen(req, timeout=60)
+        imgnp=np.array(bytearray(img_resp.read()),dtype=np.uint8)
+        frame=cv2.imdecode(imgnp,-1)
 
-while True:
-    header = {"User-Agent": "Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/92.0.4515.159 Safari/537.36."}
-    req = Request(url, headers=header)
-    img_resp = urlopen(req, timeout=60)
-    imgnp=np.array(bytearray(img_resp.read()),dtype=np.uint8)
-    frame=cv2.imdecode(imgnp,-1)
 
+        cv2.imshow("live transmission", frame)
+        h,w,_ = frame.shape
+        print("with:{},high:{}".format(w,h))
+        key=cv2.waitKey(5)
+        if key==ord('q'):
+            break
 
-    cv2.imshow("live transmission", frame)
-    h,w,_ = frame.shape
-    print("with:{},high:{}".format(w,h))
-    key=cv2.waitKey(5)
-    if key==ord('q'):
-        break
-
-cv2.destroyAllWindows()
+    cv2.destroyAllWindows()

Code/Control/Feather Code - Aaron/Motor_Basic_Test/Motor_Basic_Test.ino

@@ -3,8 +3,8 @@
 #include <Wire.h>
 #include <Adafruit_MotorShield.h>
 
-int largeVal = 255;
-int smallVal = 127;
+int largeVal = 100;
+int smallVal = 50;
 
 Adafruit_MotorShield AFMS = Adafruit_MotorShield();
 Adafruit_DCMotor *motorVL = AFMS.getMotor(1);

Code/Control/Laptop_Code/Test_keyboard/keyboard.py

+import cv2
+import pygame
+
+kpx,kix,kdx = 1,0.2,0.5
+
+
+def auto_control():
+    print("auto_control function")
+
+def stop_all():
+    print("stop_all function")
+
+def manual_command(val1,val2,val3,val4,sign1,sign2,sign3,sign4):
+    pwm1 = val1
+    pwm2 = val2
+    pwm3 = val3
+    pwm4 = val4
+    dir1 = sign1
+    dir2 = sign2
+    dir3 = sign3
+    dir4 = sign4
+    return pwm1,pwm2,pwm3,pwm4,dir1,dir2,dir3,dir4
+
+def manual_control(Ctl_com):
+
+    if get_key("w"):
+        val = start_speed
+        Ctl_com = manual_command(val,val, 0, 0, "+","+","+","+")
+
+    elif get_key("s"):
+        val = start_speed
+        Ctl_com = manual_command(val, val, 0, 0, "-", "-", "+", "+")
+
+    if get_key("UP"):
+        val = start_speed
+
+        Ctl_com = manual_command(0,0, val, val, "+","+","+","+")
+    elif get_key("DOWN"):
+        val = start_speed
+        Ctl_com = manual_command(0,0, val, val, "+","+","-","-")
+
+    elif get_key("LEFT"):
+        val = start_speed
+        Ctl_com = manual_command(0,0, val, val, "+","+","-","+")
+
+    elif get_key("RIGHT"):
+        val = start_speed
+        Ctl_com = manual_command(0,0, val, val, "+","+","+","-")
+
+    return Ctl_com
+    # print("manual_control function")
+
+def decode_ctl(Ctl_com):
+    pwm1 = Ctl_com[0]
+    pwm2 = Ctl_com[1]
+    pwm3 = Ctl_com[2]
+    pwm4 = Ctl_com[3]
+    dir1 = Ctl_com[4]
+    dir2 = Ctl_com[5]
+    dir3 = Ctl_com[6]
+    dir4 = Ctl_com[7]
+    return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+def dynamic_variable(str_name_v):
+
+    global kpx,kix,kdx,start_speed
+    if str_name_v == "kpx":
+        kpx = input("Enter your value: ")
+        print("kpx:{}".format(kpx))
+    elif str_name_v == "kix":
+        kix = input("Enter your value: ")
+        print("kix:{}".format(kix))
+    elif str_name_v == "kdx":
+        kdx = input("Enter your value: ")
+        print("kdx:{}".format(kdx))
+    elif str_name_v == "stsp":
+        start_speed = input("Enter your value: ")
+        print("start_speed:{}".format(start_speed))
+
+def variables_change_once():
+
+    str_name = input("Enter your variable: ")
+    dynamic_variable(str_name)
+
+    # print("variables_change function")
+
+def init():
+    pygame.init()
+    win= pygame.display.set_mode((400,400))
+
+def keyboard_stop(flag_s,print_count_s):
+
+    if get_key('q'):
+        flag_s = 0
+        print_count_s = 1
+    return flag_s,print_count_s
+
+def get_key(keyname):
+    ans = False
+    for eve in pygame.event.get(): pass
+    keyInput = pygame.key.get_pressed()
+    myKey = getattr(pygame,'K_{}'.format(keyname))
+
+    if keyInput[myKey]:
+        ans = True
+    pygame.display.update()
+    return ans
+
+if __name__ == '__main__':
+    global start_speed
+    start_speed = 70
+    Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
+    flag = 0
+    print_count = 1
+    init()
+    while True:
+
+        if get_key('a'):
+            flag = 1
+            while (flag == 1):
+                auto_control()
+
+                cap = cv2.VideoCapture(0)
+                ret, frame = cap.read()
+                if not ret:
+                    continue
+                cv2.imshow("image", frame)
+
+                flag, print_count = keyboard_stop(flag,print_count)
+                if flag == 0:
+                    cap.release()
+                    cv2.destroyAllWindows()
+
+        elif get_key('s'):
+            stop_all()
+            print("stop all motors")
+
+        elif get_key('m'):
+            flag = 2
+            while (flag == 2):
+                Ctl_command = manual_control(Ctl_com)
+                pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = decode_ctl(Ctl_command)
+                print("Ctl_com:{},{},{},{},{},{},{},{}".format(pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4))
+                flag, print_count = keyboard_stop(flag,print_count)
+
+        elif get_key('v'):
+            flag = 3
+            while (flag == 3):
+                variables_change_once()
+                flag = 0
+                print_count = 1
+                # flag, print_count = keyboard_stop(flag,print_count)
+                
+        elif get_key('k'): # kill the program
+            break
+
+        if print_count is not 0:
+            print("No subsystem is running")
+            print("kpx:{}".format(kpx))
+            print("kix:{}".format(kix))
+            print("kdx:{}".format(kdx))
+            print_count = 0
\ No newline at end of file

Code/Control/Laptop_Code/main_keyboard.py

+import time
+import serial
+import pygame
+import simple_pid.PID as PID
+import cv2
+
+from constants import *
+
+global ml
+ml = 0
+if ml == 1:
+    import ball_detection.ball_detection as ball_detection
+
+# ========= Serial Port I/O ===========
+
+def serial_port_in(serial_port):
+    '''
+    Description:
+        Take all ESP32_Master serial port's printIn and take all necessary input object
+
+    Input:
+        serial_port     :    serial.Serail object
+
+    Output:
+        tx, ty, tz, rx, ry, rz, LIDAR_dist, DebugM
+    '''
+
+    # DEBUG Verbose
+    print("initiating one round of serial in ...")
+
+    for i in range(7):
+        line = serial_port.readline()
+        val = int(line.decode())
+
+        if   i == 0:
+            tx = val
+        elif i == 1:
+            ty = val
+        elif i == 2:
+            tz = val
+        elif i == 3:
+            rx = val
+        elif i == 4:
+            ry = val
+        elif i == 5:
+            rz = val
+        elif i == 6:
+            LIDAR_dist = val
+
+    line = serial_port.readline()
+    debugM = line.decode()
+
+    # DEBUG Verbose
+    print("tx:{}".format(tx))
+    print("ty:{}".format(ty))
+    print("tz:{}".format(tz))
+    print("rx:{}".format(rx))
+    print("ry:{}".format(ry))
+    print("rz:{}".format(rz))
+    print("dist:{}".format(LIDAR_dist))
+    print(debugM)
+
+    return tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM
+
+
+def serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4):
+    '''
+    Description:
+        Feed to ESP32_Master to send ESP32_Slave necessary information
+        the format of sending is pwm are 3 digit space
+
+    Input:
+        serial_port                                     :   serial.Serail object
+        pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4  :   variables to send
+
+    Output:
+        None
+    '''
+    output_message = ''
+
+    for pwm_itr in [pwm1, pwm2, pwm3, pwm4]:
+        # 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'
+    print("serial out ...")
+    print(output_message)
+    serial_port.write(output_message.encode())
+
+
+# ====== Logic-directing Functions ====
+def ball_detect(gbx, gby):
+    '''
+    return True if green ball is detected
+    '''
+    if gbx == -1 and gby == -1:
+        return False
+    else:
+        return True
+
+def goal_detect(tx,ty):
+    '''
+    return True if April Tag is detected
+    '''
+    if tx == 100000 and ty == 100000:
+        return False
+    else:
+        return True
+
+def ball_capture(LIDAR_dist):
+    '''
+    return True if April Tag is detected
+    '''
+    if (LIDAR_dist < LIDAR_Thres) and (LIDAR_dist > 0):  # Ball captured
+        return True
+    else:
+        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):
+    """
+    Description:
+        Given the center of the AT tx, ty. Call PID control to output the blimp
+        motor to manuver to the goal
+
+    Input:
+        tx    :    x component, center of April Tag
+        ty    :    y component, center of Aprol Tag
+
+    Output:
+        pwm1, pwm2, pwm3, pwm4
+        dir1, dir2, dir3, dir4
+    """
+    inputx = tx / 1.00
+    inputy = -1.00 * (ty + AT_goal_Delta) / 1.00 #
+
+    # April Tag Center
+    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.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
+
+
+def seeking():
+    """
+    Description:
+        By default, when there ball is not determined capture, the manuver of the
+        motors to have it scan its surronding 360 degrees
+
+    Input:
+        none
+
+    Output:
+        pwm1, pwm2, pwm3, pwm4
+        dir1, dir2, dir3, dir4
+    """
+    pwm1, pwm2, pwm3, pwm4 = 0 , 0 , seeking_speed , seeking_speed
+    dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
+
+    return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
+
+def move2ball(gbx, gby, gb_dist):
+    """
+    Description:
+        Given the center of x y dist of green ball detected. Call PID control to
+        output the blimp motor to manuver to the green ball
+
+    Input:
+        gbx     :  x component, center of green ball
+        gby     :  y component, center of green ball
+        gb_dist :  distance to green ball
+
+    Output:
+        pwm1, pwm2, pwm3, pwm4
+        dir1, dir2, dir3, dir4
+    """
+    inputx = gbx / 1.00
+    inputy = gby / 1.00
+
+    # ESP-Cam Center
+    setpoint_x = 400
+    setpoint_y = 300
+
+    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.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 = 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):
+    '''
+    Description:
+        Given green ball information and AT information, the main control logic
+        to manuver the blimp motors
+
+    Input:
+        gbx, gby, gb_dist                   :   green ball information
+        tx, ty, tz, rx, ry, rz, LIDAR_dist  :   AirTag information
+        debugM                              :   Debug Message
+
+    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)
+    goalDetect  = goal_detect(tx, ty)
+    ballCapture = 1
+    if ballCapture: # Ball captured
+        if goalDetect:  # Goal detected
+            # stop_all()  # Debug
+            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
+        else:  # Goal not detected
+            # stop_all()  # Debug
+            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()  # Debug
+            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
+
+    return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+# ============= keyboard interruption ===================
+
+def init():
+    pygame.init()
+    win= pygame.display.set_mode((200,200))
+
+def keyboard_stop(flag_s,print_count_s):
+
+    if get_key('q'):
+        flag_s = 0
+        print_count_s = 1
+    return flag_s,print_count_s
+
+def get_key(keyname):
+    ans = False
+    for eve in pygame.event.get(): pass
+    keyInput = pygame.key.get_pressed()
+    myKey = getattr(pygame,'K_{}'.format(keyname))
+
+    if keyInput[myKey]:
+        ans = True
+    pygame.display.update()
+    return ans
+
+def auto_control(serial_port):
+    # =================================== tested autonomous control ======================================================
+    # ===== STEP 1: TAKE ALL INPUT =====
+    waitTime = 0.05
+    if ml == 1:
+        gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight=True)
+    else:
+        gbx, gby = -1, -1
+        gb_dist = -1
+    line = serial_port.readline()
+
+    if line == b'SERIAL_IN_START\r\n':
+        tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in(serial_port)
+        print("gbx,gby:{},{}".format(gbx, gby))
+        time.sleep(waitTime)
+
+    # ===== 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)  # second
+    # =========================================================================================
+
+def decode_ctl(Ctl_com):
+    pwm1 = Ctl_com[0]
+    pwm2 = Ctl_com[1]
+    pwm3 = Ctl_com[2]
+    pwm4 = Ctl_com[3]
+    dir1 = Ctl_com[4]
+    dir2 = Ctl_com[5]
+    dir3 = Ctl_com[6]
+    dir4 = Ctl_com[7]
+    return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+def manual_command(val1,val2,val3,val4,sign1,sign2,sign3,sign4):
+    pwm1 = val1
+    pwm2 = val2
+    pwm3 = val3
+    pwm4 = val4
+    dir1 = sign1
+    dir2 = sign2
+    dir3 = sign3
+    dir4 = sign4
+    return pwm1,pwm2,pwm3,pwm4,dir1,dir2,dir3,dir4
+
+def manual_control(Ctl_com,serial_port):
+
+    if get_key("w"):
+        val = start_speed
+        Ctl_com = manual_command(val, val, 0, 0, "+", "+", "+", "+")
+
+    elif get_key("s"):
+        val = start_speed
+        Ctl_com = manual_command(val, val, 0, 0, "-", "-", "+", "+")
+
+    if get_key("UP"):
+        val = start_speed
+
+        Ctl_com = manual_command(0, 0, val, val, "+", "+", "+", "+")
+    elif get_key("DOWN"):
+        val = start_speed
+        Ctl_com = manual_command(0, 0, val, val, "+", "+", "-", "-")
+
+    elif get_key("LEFT"):
+        val = start_speed
+        Ctl_com = manual_command(0, 0, val, val, "+", "+", "-", "+")
+
+    elif get_key("RIGHT"):
+        val = start_speed
+        Ctl_com = manual_command(0, 0, val, val, "+", "+", "+", "-")
+
+    pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = decode_ctl(Ctl_com)
+
+    waitTime = 0.05
+
+    serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
+    time.sleep(waitTime)
+
+
+def dynamic_variable(str_name_v):
+
+    global kpx,kix,kdx,start_speed
+    if str_name_v == "kpx":
+        kpx = input("Enter your value: ")
+        print("kpx:{}".format(kpx))
+    elif str_name_v == "kix":
+        kix = input("Enter your value: ")
+        print("kix:{}".format(kix))
+    elif str_name_v == "kdx":
+        kdx = input("Enter your value: ")
+        print("kdx:{}".format(kdx))
+    elif str_name_v == "stsp":
+        start_speed = input("Enter your value: ")
+        print("start_speed:{}".format(start_speed))
+
+def variables_change_once():
+
+    str_name = input("Enter your variable: ")
+    dynamic_variable(str_name)
+
+# ===== Main Function =====
+if __name__ == '__main__':
+    # =========== SET UP ============
+    # Defining Variables for ESP 32 Serial I/O
+    PORT = "COM22" # Based on your own serial port number
+    serial_port = serial.Serial(PORT, 115200)
+    serial_port.close()
+    serial_port.open()
+
+    # Weit Time
+    waitTime = 0.05
+
+    # Loading the PyTorch ML model for ball detection
+    if ml == 1:
+        model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
+
+    # =========== DECLARE VARIABLES ===========
+    # ESP CAM In
+    global gbx,gby,gb_dist
+    gbx, gby  = -1, -1   # by default (-1 means no found green ball)
+    gb_dist = -1         # by default (-1 means no found green ball)
+
+    # Serial Port In
+    tx, ty, tz = 100000, 100000, 100000  # by default (0 means no found AirTag)
+    rx, ry, rz = 0, 0, 0
+    LIDAR_dist = 0
+    debugM = 'Testing'
+
+    # Serial Port Out
+    pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0   # Not moving
+    dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
+
+    # Trigger the ESP32_SLAVE to talk first
+    if ml == 1:
+        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)
+
+    Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
+    flag = 0
+    print_count = 1
+    global start_speed
+    start_speed = 70
+
+    init()
+    # =========== LOOP FOREVER===========
+    while True:
+
+        if get_key('a'):
+            flag = 1
+            while (flag == 1):
+                auto_control(serial_port)
+                flag, print_count = keyboard_stop(flag,print_count)
+
+        elif get_key('s'):
+            stop_all()
+            print("stop all motors")
+
+        elif get_key('m'):
+            flag = 2
+            while (flag == 2):
+                manual_control(Ctl_com,serial_port)
+                flag, print_count = keyboard_stop(flag,print_count)
+
+        elif get_key('v'):
+            flag = 3
+            while (flag == 3):
+                variables_change_once()
+                flag = 0
+                print_count = 1
+
+        elif get_key('k'):
+            break
+
+        if print_count is not 0:
+            print("No subsystem is running")
+            print_count = 0
+
+
+

Code/Control/test_motor/test_motor.ino

+#include <esp_now.h> 
+#include <WiFi.h> 
+#include <Wire.h> 
+#include <SparkFun_VL53L1X.h> 
+#include <Arduino.h> 
+#include <Adafruit_MotorShield.h> 
+
+int Rec_pwm1; 
+int Rec_pwm2; 
+int Rec_pwm3; 
+int Rec_pwm4; 
+String Rec_dir1; 
+String Rec_dir2; 
+String Rec_dir3; 
+String Rec_dir4; 
+
+// ========================== Motor part ==================================== 
+// Create the motor shield object with the default I2C address 
+Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
+// changed 
+Adafruit_DCMotor *motorVertical_L = AFMS.getMotor(1);  // pwm1 
+Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(2);  // pwm2 
+Adafruit_DCMotor *motorLeft = AFMS.getMotor(3);        // pwm3 
+Adafruit_DCMotor *motorRight = AFMS.getMotor(4);       // pwm4  
+
+void setup() {
+  // put your setup code here, to run once:
+ // Init Serial Monitor 
+  Serial.begin(115200); 
+  //  Wire.begin(); 
+AFMS.begin();  // create with the default frequency 1.6KHz 
+
+}
+void motor_control(int pwm1,int pwm2,int pwm3,int pwm4){
+  motorVertical_L->setSpeed(abs(pwm1)); 
+  motorVertical_R->setSpeed(abs(pwm2)); 
+  motorLeft->setSpeed(abs(pwm3)); 
+  motorRight->setSpeed(abs(pwm4));  
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  int t_speed = 50;
+  
+
+
+    motorVertical_L->setSpeed(t_speed); 
+  motorVertical_R->setSpeed(t_speed); 
+  motorLeft->setSpeed(t_speed); 
+  motorRight->setSpeed(t_speed); 
+    motorVertical_L->run(BACKWARD); // 
+  motorVertical_R->run(BACKWARD); // 
+    motorLeft->run(BACKWARD); // 
+  motorRight->run(BACKWARD); //   
+  delay(1000);
+  /* 
+  delay(2000);
+  motor_control(t_speed,0,0,0);
+  motorVertical_L->run(BACKWARD); //  
+  Serial.println("V_L: backward"); 
+  delay(2000);
+  motorVertical_L->run(FORWARD); 
+  Serial.println("V_L: forward");
+  
+    delay(2000);
+  motor_control(0,t_speed,0,0);
+  motorVertical_R->run(BACKWARD); // 
+  Serial.println("V_R: backward"); 
+  delay(2000);
+  motorVertical_R->run(FORWARD); // 
+  Serial.println("V_R: forward");
+  
+    delay(2000);
+  motor_control(0,0,t_speed,0);
+  motorLeft->run(BACKWARD); // 
+  Serial.println("H_L: backward"); 
+  delay(2000);
+  motorLeft->run(FORWARD); // 
+  Serial.println("H_L: forward");  
+
+    delay(2000);
+  motor_control(0,0,0,t_speed);
+  motorRight->run(BACKWARD); // 
+  Serial.println("H_R: backward"); 
+  delay(2000);
+  motorRight->run(FORWARD); // 
+  Serial.println("H_R: forward"); 
+  */
+  
+ 
+}

Code/OpenMV Code/Test_code/blue_main.py

+import image, network, math, rpc, sensor, struct, tf, ucollections, pyb
+import ubinascii
+sensor.reset()
+sensor.set_pixformat(sensor.RGB565)
+sensor.set_framesize(sensor.QVGA)
+sensor.set_auto_gain(False)
+sensor.set_auto_whitebal(False)
+sensor.skip_frames(time = 2000)
+red_led = pyb.LED(1)
+green_led = pyb.LED(2)
+blue_led = pyb.LED(3)
+red_led.off()
+green_led.off()
+blue_led.off()
+red_led.on()
+interface = rpc.rpc_i2c_slave(slave_addr=0x12)
+MAX_BLOBS = 4
+TAG_SIZE = 138
+MAX_TAGS = 2
+XRES = 320
+YRES = 240
+SIZE = 16.3
+f_x = (2.8 / 3.673) * XRES
+f_y = (2.8 / 2.738) * YRES
+c_x = XRES * 0.5
+c_y = YRES * 0.5
+def draw_detections(img, dects):
+	for d in dects:
+		c = d.corners()
+		l = len(c)
+		for i in range(l): img.draw_line(c[(i+0)%l] + c[(i+1)%l], color = (0, 255, 0))
+		img.draw_rectangle(d.rect(), color = (255, 0, 0))
+def face_detection(data):
+	sensor.set_pixformat(sensor.GRAYSCALE)
+	sensor.set_framesize(sensor.QVGA)
+	faces = sensor.snapshot().gamma_corr(contrast=1.5).find_features(image.HaarCascade("frontalface"))
+	if not faces: return struct.pack("<HHHH", 0, 0, 0, 0)
+	for f in faces: sensor.get_fb().draw_rectangle(f, color = (255, 255, 255))
+	out_face = max(faces, key = lambda f: f[2] * f[3])
+	return struct.pack("<HHHH", out_face[0], out_face[1], out_face[2], out_face[3])
+def find_position(box):
+	x = box[0]
+	y = box[1]
+	w = box[2]
+	h = box[3]
+	cx = x + w/2
+	cy = y + h/2
+	frame_cx = sensor.width()/2
+	frame_cy = sensor.height()/2
+	if cx < frame_cx:
+		if cy < frame_cy:
+			pos = "UL"
+		else:
+			pos = "LL"
+	else:
+		if cy < frame_cy:
+			pos = "UR"
+		else:
+			pos = "LR"
+	x_diff = cx - frame_cx
+	y_diff = cy - frame_cy
+	area = w*h
+	return pos
+def find_dist(box, w_actual, l_actual):
+	w_dist = w_actual*2.8/box[2]
+	l_dist = l_actual*2.8/box[3]
+	dist = (w_dist+l_dist)/2
+	return dist*100
+ignore_blue = (0, 0, sensor.width(), sensor.height())
+def color_detection(thresholds):
+	sensor.set_pixformat(sensor.RGB565)
+	sensor.set_framesize(sensor.QVGA)
+	sensor.set_auto_gain(False)
+	sensor.set_auto_whitebal(False)
+	thresholdss = struct.unpack("<bbbbbb", thresholds)
+	img = sensor.snapshot()
+	blobs = [0,0,0,0,0,0,0,0,0,0,0,0]
+	n_blobs = 0
+	for blob in img.find_blobs([thresholdss], pixels_threshold=200, area_threshold=200, merge=True):
+		img.draw_edges(blob.min_corners(), color=(0,0,255))
+		item = blob.rect()
+		area = item[2] * item[3]
+		if n_blobs < MAX_BLOBS:
+			blobs[0 + (n_blobs*3): 2 + (n_blobs*3)] = [int(blob.cx()), int(blob.cy()), int(area)]
+			n_blobs += 1
+	print(blobs)
+	return struct.pack("<hhhhhhhhh", blobs[0], blobs[1], blobs[2], blobs[3], blobs[4], blobs[5], blobs[6],
+					   blobs[7], blobs[8])
+def color_detection_single(data):
+	red_led.off()
+	green_led.on()
+	sensor.set_pixformat(sensor.RGB565)
+	sensor.set_framesize(sensor.QVGA)
+	sensor.set_auto_gain(False)
+	sensor.set_auto_whitebal(False)
+	thresholds = struct.unpack("<bbbbbb", data)
+	print(thresholds)
+	blobs = sensor.snapshot().find_blobs([thresholds],
+										 pixels_threshold=500,
+										 area_threshold=500,
+										 merge=True,
+										 margin=20)
+	if not blobs:
+		red_led.on()
+		green_led.off()
+		return struct.pack("<HH", 0, 0)
+	for b in blobs:
+		sensor.get_fb().draw_rectangle(b.rect(), color = (255, 0, 0))
+		sensor.get_fb().draw_cross(b.cx(), b.cy(), color = (0, 255, 0))
+	out_blob = max(blobs, key = lambda b: b.density())
+	red_led.on()
+	green_led.off()
+	return struct.pack("<HH", out_blob.cx() , out_blob.cy())
+def recalibration():
+	img = sensor.snapshot()
+	for blob in img.find_blobs(thresholds[2], invert=True, pixels_threshold=200, area_threshold=200, merge=True):
+		img.draw_edges(blob.min_corners(), color=(0,0,255))
+		ignore_blue = (blob.x(), blob.y(), blob.w(), blob.h())
+def change_tag_size(size):
+	TAG_SIZE = size
+def degrees(radians):
+	return (180 * radians) / math.pi
+def dist_conversion(z):
+	z = z*100*2
+	scale = TAG_SIZE/138
+	res_scale = (240/X_RES + 240/Y_RES)/2
+	return z*scale*res_scale
+def xy_to_mm(tag1_cx, tag1_cy, tag2_cx, tag2_cy, tag3_cx, tag3_cy):
+	mm_distx = 590
+	mm_disty = 1
+	pixel_distx = tag1_cx - tag2_cx
+	pixel_disty = tag1_cy - tag2_cy
+	target_distx = tag1_cx - tag3_cx
+	target_disty = tag1_cy - tag3_cy
+	distx = target_distx*mm_distx/pixel_distx/2.8
+	disty = target_disty*mm_disty/pixel_disty/2.8
+	return (distx, disty)
+def apriltags(data):
+	red_led.off()
+	green_led.on()
+	sensor.set_pixformat(sensor.RGB565)
+	sensor.set_framesize(sensor.QQVGA)
+	sensor.set_auto_gain(False)
+	sensor.set_auto_whitebal(False)
+	LENS_MM = 2.8
+	LENS_TO_CAM_MM = 22
+	datas = struct.unpack("<bb", data)
+	id1 = datas[0]
+	tagsize1 = 138
+	if id1 == 0:
+		tagsize1 = 138
+	img = sensor.snapshot()
+	f_x = (LENS_MM / 3.984) * X_RES
+	f_y = (LENS_MM / 2.952) * Y_RES
+	c_x = X_RES * 0.5
+	c_y = Y_RES * 0.5
+	tags = [0, 0, 0]
+	for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y):
+		img.draw_rectangle(tag.rect(), color = (255, 0, 0))
+		img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
+		dist = dist_conversion(tag.z_translation())
+		img.draw_string(tag.cx(), tag.cy(), str(dist))
+		if tag.id() == id1:
+			print(tag.id())
+			TAG_SIZE = tagsize1
+			tags[0:2] = [int(tag.cx()), int(tag.cy()), int(degrees(tag.z_rotation()))]
+	red_led.on()
+	green_led.off()
+	return struct.pack("<hhh", tags[0], tags[1], tags[2])
+def qrcode_detection(data):
+	sensor.set_pixformat(sensor.RGB565)
+	sensor.set_framesize(sensor.VGA)
+	sensor.set_windowing((320, 240))
+	codes = sensor.snapshot().find_qrcodes()
+	if not codes: return bytes()
+	draw_detections(sensor.get_fb(), codes)
+	return str(codes).encode()
+def ATDistance(val,size):
+	new_val = -10*val*size/16.3
+	return new_val
+def goalfinder(data):
+	green_led.off()
+	red_led.off()
+	blue_led.on()
+	sensor.set_pixformat(sensor.RGB565)
+	sensor.set_framesize(sensor.QVGA)
+	sensor.set_auto_gain(False)
+	sensor.set_auto_whitebal(False)
+	goal_id = struct.unpack("<bbb", data)
+	nearest_tag = [0,0,0,10000,0,0,0]
+	img = sensor.snapshot()
+	for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y):
+		img.draw_rectangle(tag.rect(), color = (255, 0, 0))
+		img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
+		for i in goal_id:
+			if tag.id() == i and ATDistance(tag.z_translation(),SIZE) < nearest_tag[3]:
+				nearest_tag[0] = int(tag.id())
+				nearest_tag[1] = int(tag.cx())
+				nearest_tag[2] = int(tag.cy())
+				nearest_tag[3] = int(ATDistance(tag.z_translation(),SIZE))
+				nearest_tag[4] = int(tag.x_rotation())
+				nearest_tag[5] = int(tag.y_rotation())
+				nearest_tag[6] = int(tag.z_rotation())
+	red_led.on()
+	green_led.off()
+	blue_led.off()
+	return struct.pack("<hhhhhhh", nearest_tag[0],nearest_tag[1],nearest_tag[2],nearest_tag[3],nearest_tag[4],nearest_tag[5],nearest_tag[6])
+interface.register_callback(face_detection)
+interface.register_callback(color_detection)
+interface.register_callback(apriltags)
+interface.register_callback(color_detection_single)
+interface.register_callback(recalibration)
+interface.register_callback(goalfinder)
+interface.loop()