diff --git a/Code/Ball_Detection/PyTorch_with_ESPCAM/imageTread.py b/Code/Ball_Detection/PyTorch_with_ESPCAM/imageTread.py index 2720416411e39f25b3195d750333581ff2b05469..8795251b8d37923f2a40f4a4c5db49d189a0d311 100644 --- a/Code/Ball_Detection/PyTorch_with_ESPCAM/imageTread.py +++ b/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() diff --git a/Code/Control/Feather Code - Aaron/Motor_Basic_Test/Motor_Basic_Test.ino b/Code/Control/Feather Code - Aaron/Motor_Basic_Test/Motor_Basic_Test.ino index cae401001cc68f2a681b9e7dff122ae005bd136a..b1254f395d44d6b23e4cd1372b532f580de89abb 100644 --- a/Code/Control/Feather Code - Aaron/Motor_Basic_Test/Motor_Basic_Test.ino +++ b/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); diff --git a/Code/Control/Laptop_Code/Test_keyboard/keyboard.py b/Code/Control/Laptop_Code/Test_keyboard/keyboard.py new file mode 100644 index 0000000000000000000000000000000000000000..3e3f90dd9f57c9d900feea778072bfba92c54392 --- /dev/null +++ b/Code/Control/Laptop_Code/Test_keyboard/keyboard.py @@ -0,0 +1,162 @@ +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 diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py new file mode 100644 index 0000000000000000000000000000000000000000..5484805f9cb220477814c69e67ce697c21ca3c83 --- /dev/null +++ b/Code/Control/Laptop_Code/main_keyboard.py @@ -0,0 +1,515 @@ +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 + + + diff --git a/Code/Control/test_motor/test_motor.ino b/Code/Control/test_motor/test_motor.ino new file mode 100644 index 0000000000000000000000000000000000000000..62e70e124187f994842168699f721f50903317aa --- /dev/null +++ b/Code/Control/test_motor/test_motor.ino @@ -0,0 +1,91 @@ +#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"); + */ + + +} diff --git a/Code/OpenMV Code/Test_code/blue_main.py b/Code/OpenMV Code/Test_code/blue_main.py new file mode 100644 index 0000000000000000000000000000000000000000..b5deee3de1a5859612111662975d7da4c3832aa0 --- /dev/null +++ b/Code/OpenMV Code/Test_code/blue_main.py @@ -0,0 +1,214 @@ +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()