diff --git a/3D Design/joints/pentacross.SLDPRT b/3D Design/joints/pentacross.SLDPRT
new file mode 100644
index 0000000000000000000000000000000000000000..90cc7e04f93f99ba466991b36d902032641ab4c6
Binary files /dev/null and b/3D Design/joints/pentacross.SLDPRT differ
diff --git a/3D Design/joints/pentagon_corner.SLDPRT b/3D Design/joints/pentagon_corner.SLDPRT
new file mode 100644
index 0000000000000000000000000000000000000000..569eab31c7b27ce8c235f27228b333ee89901376
Binary files /dev/null and b/3D Design/joints/pentagon_corner.SLDPRT differ
diff --git a/3D Design/joints/pentagon_corner_tilt.STL b/3D Design/joints/pentagon_corner_tilt.STL
new file mode 100644
index 0000000000000000000000000000000000000000..b732d6dcdbe2438ccfeeac0ab2b61073d0ce1a7c
Binary files /dev/null and b/3D Design/joints/pentagon_corner_tilt.STL differ
diff --git a/Code/Control/Laptop_Code/main_backup/main_keyboard_before.py b/Code/Control/Laptop_Code/main_backup/main_keyboard_before.py
new file mode 100644
index 0000000000000000000000000000000000000000..ccb07db92236c67e4aa4363735f2c12f0b7b402f
--- /dev/null
+++ b/Code/Control/Laptop_Code/main_backup/main_keyboard_before.py
@@ -0,0 +1,680 @@
+import time
+import serial
+
+import simple_pid.PID as PID
+import timeit
+import pygame
+from constants import *
+from ESP32_AT.imageTread_AT import get_AT_6DOF_info
+
+global ml,esp_cam_on,openmv_on
+ml = 0
+esp_cam_on = 0
+openmv_on = 1
+if ml == 1:
+ import ball_detection.ball_detection as ball_detection
+
+# ========= Serial Port I/O ===========
+
+def serial_port_in_v1(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_in_v2(serial_port):
+ '''
+ Description:
+ Take all ESP32_Master serial port's printIn and take all necessary input object
+
+ Input:
+ serial_port : serial.Serail object
+
+ Output:
+ LIDAR_dist,DebugM
+ '''
+ # debug info
+
+ print("initiating serial in V2...")
+ for i in range(2):
+ line = serial_port.readline()
+
+ if i == 0:
+ val = int(line.decode())
+ LIDAR_dist = val
+ elif i == 1:
+ debugM = line.decode()
+
+ print("Lidar_dist:{}".format(LIDAR_dist))
+ print(debugM)
+
+ return 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 AT_detect(tx,ty):
+ if tx == 100000 and ty == 100000:
+ return False
+ else:
+ return True
+
+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(kpx_g, kix_g, kdx_g, setpoint = setpoint_x1)
+ pid_y = PID(kpy_g, kiy_g, kdy_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 ball_seeking(count_h,tx,ty):
+ """
+ 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
+ """
+ global set_h
+ if openmv_on == 1:
+ delta_h = 100
+ threshold_h = 20
+ elif esp_cam_on == 1:
+ delta_h = 2 # meter
+ threshold_h = 0.3
+
+ AT_detected = AT_detect(tx, ty)
+
+ # count_h = 0 #
+ if AT_detected:
+ current_h = get_altitude_from_AT(AT_h1,ty)
+
+ if count_h == 0:
+ set_h = current_h + delta_h # in meter
+ count_h = 1 # moving up
+
+ # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = vertical_control(current_h, set_h)
+ diff_h = abs(set_h - current_h)
+ if diff_h < threshold_h:
+ new_ssp = 200
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction_ssp(new_ssp)
+ count_h = 0
+ else:
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = vertical_control(current_h, set_h)
+ else:
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
+
+ return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
+def vertical_control(current_h,set_h):
+
+ pid_h = PID(kph, kih, kdh, setpoint = set_h)
+ pid_h.auto_mode = True
+ pid_h.set_auto_mode(True, last_output = 8.0)
+ pid_h.output_limits = (-255,255)
+
+ input_h = current_h
+ output_h = pid_h(input_h)
+
+ pwm1 = abs(output_h)
+ pwm2 = abs(output_h)
+ pwm3 = 0
+ pwm4 = 0
+ dir1 = '+'
+ dir2 = '+'
+ dir3 = '+'
+ dir4 = '+'
+
+ return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
+def rotate_one_direction():
+ 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 rotate_one_direction_ssp(ssp):
+ pwm1, pwm2, pwm3, pwm4 = 0, 0, ssp, ssp
+ 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,count_h):
+ '''
+ 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
+
+ # debug
+ # ballCapture = 1
+ # goalDetect = 0
+ 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 = rotate_one_direction()
+ 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 = rotate_one_direction()
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = ball_seeking(count_h,tx,ty)
+
+ return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM,count_h):
+ # ===== STEP 1: TAKE ALL INPUT =====
+ # gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ line = serial_port.readline()
+
+ if openmv_on == 1:
+ if line == b'SERIAL_IN_START\r\n':
+ tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM = serial_port_in_v1(serial_port)
+ print("gbx,gby:{},{}".format(gbx, gby))
+ time.sleep(waitTime)
+
+
+ if esp_cam_on == 1:
+ url = 'http://192.168.1.118/cam-hi.jpg'
+ tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
+
+ # ===== 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,count_h)
+
+ # ===== STEP 3: FEED ALL OUTPUT =====
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
+ time.sleep(waitTime)
+
+def auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
+ count_h = 0
+ # 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)
+ init_count += 1
+
+ return init_count,count_h
+
+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 stop_all():
+ pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
+ dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
+ return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+def dynamic_variable(str_name_v):
+
+ global start_speed
+ global kpx,kix,kdx
+ global kpy,kiy,kdy
+ global kpx_g,kix_g,kdx_g
+ global kpy_g,kiy_g,kdy_g
+
+ 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))
+ elif str_name_v == "kpy":
+ kpx = input("Enter your value: ")
+ print("kpy:{}".format(kpy))
+ elif str_name_v == "kiy":
+ kix = input("Enter your value: ")
+ print("kiy:{}".format(kiy))
+ elif str_name_v == "kdy":
+ kdx = input("Enter your value: ")
+ print("kdy:{}".format(kdy))
+ if str_name_v == "kpx":
+ kpx = input("Enter your value: ")
+ print("kpx:{}".format(kpx))
+ elif str_name_v == "kix_g":
+ kix = input("Enter your value: ")
+ print("kix_g:{}".format(kix_g))
+ elif str_name_v == "kdx":
+ kdx = input("Enter your value: ")
+ print("kdx_g:{}".format(kdx_g))
+ elif str_name_v == "kpy_g":
+ kpx = input("Enter your value: ")
+ print("kpy_g:{}".format(kpy_g))
+ elif str_name_v == "kiy_g":
+ kix = input("Enter your value: ")
+ print("kiy_g:{}".format(kiy_g))
+ elif str_name_v == "kdy_g":
+ kdx = input("Enter your value: ")
+ print("kdy_g:{}".format(kdy_g))
+
+def variables_change_once():
+
+ str_name = input("Enter your variable: ")
+ dynamic_variable(str_name)
+
+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, "+", "+", "+", "-")
+ elif get_key("r"):
+ val = start_speed
+ Ctl_com = manual_command( val, val,0,0, "+", "-", "+", "+")
+
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = decode_ctl(Ctl_com)
+
+ waitTime = 0.05
+ # changed
+ # gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+
+ time.sleep(waitTime)
+
+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 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 get_altitude_from_AT(AT_h,ty):
+ # below AT center, ty > 0
+ # above AT center, ty < 0
+ altitude = AT_h - ty
+ return altitude
+
+def test_function():
+ url = 'http://192.168.1.118/cam-hi.jpg'
+ tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
+ print("testing new function")
+ print("-----------------------")
+ print("tid:{}".format(tid))
+ print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
+ print("rx,ry,rz:{},{},{}".format(rx,ry,rz))
+
+
+# ===== Main Function =====
+if __name__ == '__main__':
+ # =========== SET UP ============
+ # Defining Variables for ESP 32 Serial I/O
+ PORT = "COM6" # for Alienware
+ 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
+ model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
+
+ # =========== DECLARE VARIABLES ===========
+ # ESP CAM In
+ 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 = '+', '+', '+', '+'
+ Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
+ # Trigger the ESP32_SLAVE to 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)
+ """
+ flag = 0
+ init_count = 0
+ print_count = 0
+ global start_speed
+ start_speed = 70
+
+ init()
+ # =========== LOOP FOREVER===========
+ while True:
+ if get_key('a'):
+ flag = 1
+ while (flag == 1):
+ if init_count == 0:
+ init_count,count_h = auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM)
+ auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM,count_h)
+ flag, print_count = keyboard_stop(flag,print_count)
+ elif get_key('s'):
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = stop_all()
+ serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
+ 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('t'):
+ flag = 4
+ while (flag == 4):
+ test_function()
+ flag, print_count = keyboard_stop(flag, print_count)
+
+ elif get_key('k'):
+ break
+
+ if print_count != 0:
+ print("No subsystem is running")
+ print_count = 0
diff --git a/README.md b/README.md
index 0aa4f9e815baa5581ca59812114887b31c163cc0..948bc6f4216eddb0fd3e28211577e78e2882ba6b 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,191 @@
# November 2021 Blimp Competition
-This git will solely be used for the development of the autonomous blimps for the defend the republic competition
\ No newline at end of file
+Components Needed
+
+One ESP32 Feather Board, Stacking Header (https://www.adafruit.com/product/3405)
+One Adafruit Neopixel LED Panel, Plain Header (https://www.adafruit.com/product/2945)
+One Adafruit Radio FeatherWing 433MHz, Stacking Header (https://www.adafruit.com/product/3230)
+Wires for I2C and motor connection
+Electronic Components Mount* (in General 3D Print Design)
+1 to 3 Lipo Batteries 3.7V-3.8V (https://www.amazon.com/Crazepony-Battery-PowerWhoop-Connector-Inductrix/dp/B07L9SHHFX)
+One OpenMV H7 or H7 Plus (https://openmv.io/)
+Velcro Tape**
+Two Blimp*** (https://www.amazon.com/Swimmers-Remote-Control-Flying-Clownfish/dp/B005FYCBR6/ref=sr_1_5?crid=1QSXYA4FQYTC1&dchild=1&keywords=air+swimmers+remote+control+flying+fish&qid=1629779292&sprefix=air+swimmers%2Caps%2C229&sr=8-5)
+Two 2.2kOhms resistors
+Helium
+Three DC motors (https://www.amazon.com/Hobbypower-Coreless-53000rpm-8-5x20mm-Propeller/dp/B076M7G24G/ref=pd_sbs_6/140-9159272-8519647?pd_rd_w=TKtuN&pf_rd_p=0f56f70f-21e6-4d11-bb4a-bcdb928a3c5a&pf_rd_r=VR3D6QW63FJ4FG19JE97&pd_rd_r=c10d4879-cb04-4453-87fe-8cfa37d04396&pd_rd_wg=7TWip&pd_rd_i=B076M7G24G&psc=1)
+Three Propellers (https://www.amazon.com/Hobbypower-Coreless-53000rpm-8-5x20mm-Propeller/dp/B076M7G24G/ref=pd_sbs_6/140-9159272-8519647?pd_rd_w=TKtuN&pf_rd_p=0f56f70f-21e6-4d11-bb4a-bcdb928a3c5a&pf_rd_r=VR3D6QW63FJ4FG19JE97&pd_rd_r=c10d4879-cb04-4453-87fe-8cfa37d04396&pd_rd_wg=7TWip&pd_rd_i=B076M7G24G&psc=1)
+Putty (https://www.amazon.com/Removable-Adhesive-Non-Toxic-Multipurpose-Photography/dp/B08DTHJTSZ/ref=sr_1_9?crid=38S8BXT4S632I&dchild=1&keywords=sticky+clay+adhesive+putty&qid=1629782480&sprefix=sticky+clay%2Ctoys-and-games%2C221&sr=8-9)
+
+*: Although we are currently using a 3D-printed gondola as a mount, anything else that can hold the components together can be used.
+**: velcros are used to hold the mount to the blimp. If you don't have it, normal tape would suffice but it will be a little bit messier
+***: The total mass of all components weight approximately around 120g. Therefore, a blimp that can carry that much weight is needed. For the Amazon product listed, you need two of them for the blimp to float in the air.
+Connecting the ESP32 Feather with OpenMV
+
+If you are planning on using the I2C communication protocol, you need to connect the following from Feather to OpenMV:
+
+GND pin - GND pin
+SCL/22 pin - P4 pin
+SDA/23 pin - P5 pin
+
+
+Just like the ESP32, we need to make sure the battery connected to OpenMV is of the right polarity. With the battery suggested, you need to reverse the wiring in order to connect a battery to OpenMV.
+If the battery is properly connected, the OpenMV LED will blink white at the start. In addition to that, a debugging feature was added such that the OpenMV LED will be red once it is switched on. Everytime a function is called, it will blink green for the period of the function.
+In addition to that, since OpenMV is not a stackable feather component, you need to add pull up resistors for both the SDA and SCL lines. The way to do this is by connecting two 2.2kOhms resistors where one end is connected to ESP32 3V and the other one is connected to the SDA and SCL respectively.
+
+Pullup resistor circuit example:
+
+(https://www.analog.com/en/technical-articles/i2c-primer-what-is-i2c-part-1.html)
+Camera setup example:
+
+Note: Failure to properly setup the I2C protocol might cause the code to not work properly
+Connecting the ESP32 Feather with the Motor Shield
+
+The Feather and the shield should be stackable. Make sure you have soldered the pins properly.
+The shield runs on a separate power source. Make sure that you have a 3.7V Lipo Battery if you are planning to use the shield.
+If the battery is properly connected, a green LED should light up. This LED also approximately indicate how much power the battery has left as it will get dimmer with lower voltage.
+Connect the vertical motor to M1, right motor to M2, and left motor to M4.
+The motor itself can be connected differently if you are using a different vehicle. If so, changes might be needed for the motor controller code section.
+
+Note: depending on the wiring and the propeller design, the propeller might push the blimp in the opposite direction as the code intended. Ways to solve this problem include reversing the polarity wiring of the motor, use the oppsite-direction propeller of the one you have right now (if it is clockwise, use the counter-clockwise propeller instead), or change the code so that all BACKWARD motor calls are FORWARD and vice versa.
+DC Motor Shield setup example:
+
+Other Physical Setup
+
+The stack, starting from the bottom is the ESP32 Feather, Radiowing Feather, DC Motor Shield, and the Neopixel LED Panel.
+Just like the OpenMV, reverse the wiring before connecting a battery to the ESP32. Optional: if weight is an issue, you can use a single battery and connect the ESP32 and OpenMV in parallel. However, it is strongly advised that you use a separate battery for the DC Shield as it consumes a lot of power.
+Fill the two blimp balloons with Helium until both of them together can carry around 130-140g. In order to test this, tie a string to the blimp and attach the string to a heavier object like a smartphone. First, weight the smartphone by itself. Then, weight the smartphone when it is attached to the string. The difference will be what the blimp can carry.
+Using the velro tape, attach the top of the electronic mount to the bottom of the blimp
+Similarly, use velcro tape to attach the OpenMV camera to the front of the mount in such a way that the camere is facing the forward direction.
+Plug the motor to its corresponding location on the mount
+Place the ESP32 and the rest of the stack into the mount
+Test to see if the whole blimp floats weightlessly in the air. If it is falling down, add more helium to the blimp balloon. If it is floating up, add putty into the mount. Repeat this until it approximately stays still in the air without any external force acting on it.
+
+Blimp stack setup:
+
+Blimp front view:
+
+Blimp bottom view:
+
+Complete setup:
+
+Setting Up the General Libraries and Codes
+
+Go to https://git.uclalemur.com/arnhold/espmeshnetworking/-/tree/master/MeshCode
+
+In that repository, follow the instructions to install the ArnholdMesh.zip and Painless_Mesh.zip libraries.
+If you are planning on doing demo using the LED panel, please also install Adafruit Neopixel library through the Library Manager in your Arduino IDE
+If you are using the Adafruit Motor shield, do not install the default library through the library manager. Instead, in your Arduino IDE, go to Sketch > Include Library > Add .ZIP Library
+Download or Clone the Adafruit_Motor_Shield_V2_Library.zip from this repository and use that for Step 4. The reason we are using an alternate library from the default one is because the motor library and the mesh library uses the same name for one of their macros. This changes the macro's name in the motor library to ensure that this doesn't happen.
+Download the PID library, which is needed for the motion control. (https://playground.arduino.cc/Code/PIDLibrary/)
+
+Running the Code
+
+Create a folder name feather_main_personaldemo in your directory of choice.
+Copy everthing in the Main Code repository into the feather_main_personaldemo folder except for OpenMV_main.py, the motor library, and the other ino file that is not feather_main_personaldemo.
+Open feather_main_personaldemo.ino and upload the sketch into your ESP32 Feather
+Upload the OpenMV_main.py into your OpenMV camera as main.py so that it can run without being connected to the IDE
+By default, it should start detecting and responding to green blob.
+
+Varying the Parameters
+If we want to vary the behaviour of the blimp, there are a couple of parameter that we can use to do that. The section below explains what message needs to reach the ESP32 through the mesh network. However, the methodology of how the message is sent to the blimp node is completely up to you. Another section further below will give you an example remote controller you can use (although not the best)
+
+
+Color Detection Threshold (C)
+
+By default, the blimp will start trying to detect the biggest green blob. However, there are 2 ways we can change the threshold so that the program will look for other colored blobs.
+The first way is to provide a single-digit color code. The code along with the command character:
+
+"C1": generic green blob
+"C2": generic blue blob
+"C5": generic red blob
+
+
+If the generic colors are not enough, you can also prescribe a specific thresholds for the robot to use. The format will be something like:
+
+"C(Lmin) (Lmax) (Amin) (Amax) (Bmin) (Bmax)", without the brackets
+"C30 100 12 -19 0 -120"
+
+
+
+
+
+PID Constants (T)
+
+You can also vary the PID constant for the horizontal motion. By default, these are the constants:
+
+Kpx = 1
+Kix = 0.05
+Kdx = 0.25
+
+
+In order to change the constant which are double datatype, the generic form of the message should be:
+
+"TP0.1 I0.2 D0.3"
+
+
+For this command, you can also just change one or 2 parameters by doing the command like:
+
+"TP0.5"
+"TI0.9 D1.5"
+
+
+
+
+
+Pause the Blimp (P)
+
+If you want to stop the mesh from moving, a quick way is to use the pause feature:
+
+"P1": pause the blimp program
+"P0": resume the blimp program
+
+
+
+
+
+Change the Base Speed (S)
+
+The base speed is used to move towards the tracked object. If it is 0, it the blimp will not actively move towards the object if it is detected by the camera. The command format is:
+
+"S0"
+"S250"
+
+
+
+
+
+LED Panel Debugging with the Blimp
+
+OpenMV LED: blink green when a function is called. red on standby
+PIN31: state of the blimp. red is standby, yellow is paused, white is seeking, blue is done
+PIN30: kp change. blink green if new kp is higher then old kp. red if less. white if the same
+PIN29: ki change. blink green if new ki is higher then old ki. red if less. white if the same.
+PIN28: kd change. blink green if new kd is higher then old kd. red if less. white if the same
+PIN27: base speed indicator. the white LED intensity scale with the base speed, with 255 speed being the brightest
+PIN26: minimum threshold for color detection, display the minimum LAB value in terms of RGB
+PIN25: maximum threshold for color detection, display the maximum LAB value in terms of RGB
+PIN24: left motor speed. display the speed of the motor that scales with the light intensity. led will be red if the motor is pushing the blimp forward, blue if backward
+PIN17: vertical motor speed. display the speed of the motor that scales with the light intensity. led will be red if the motor is pushing the blimp upward, blue if downward
+PIN16: right motor speed. display the speed of the motor that scales with the light intensity. led will be red if the motor is pushing the blimp forward, blue if backward
+
+Using the Camera.cpp Class Separately
+
+
+Install OpenMV Arduino RPC Library into your Arduino IDE.
+
+
+At the top of your main code, include the following lines to create the i2c interface that the OpenMV will communicate with the feather:
+ #include "Camera.h"
+ openmv::rpc_scratch_buffer<256> scratch_buffer; // All RPC objects share this buffer.
+ openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, consider making address and rate as constructor argument
+
+
+You need to pass the interface's address when you are constructing the camera:
+ Camera cam(&interface);
+
+
+Now, you should be able to use all the function in the Camera.h file.
+
+
+If you want to make a new function in the OpenMV and create an interfacing function corresponding to it on Arduino, please refer the RPC library for further clarification. https://github.com/openmv/openmv-arduino-rpc