def __init__(self): self.config = Config() self.mcu = MCU() self.detecting = Detecting() # fb = Firebase() ws = webserver.WebServer() dl = DataLogger()
def main(): # read arguments ap = argparse.ArgumentParser() ap.add_argument("-s", "--speed") args = vars(ap.parse_args()) set_speed = args.get('speed', 0) if set_speed is None: set_speed = 0 set_speed = float(set_speed) speed = 0 # inits MCU mcu = MCU(2222) # inits IMU imu = IMU("/dev/ttyUSB_IMU") # inits CV cv = CVManager( 0, # choose the first web camera as the source enable_imshow=True, # enable image show windows server_port=3333) cv.add_core("Depth", DepthCore(), True) # start subprocess imu.start() mcu.start() cv.start() mcu.wait() pidR = pidRoll(1, 0.2, 0) # 1, 0 , 0 pidP = pidPitch(0.6, 0, 0) # 5 ,0.1 ,8 pidD = pidDepth(0, 0, 0) pidY = pidYaw(1, 0.4, 0) motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0 try: motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0 counter = 0 while True: counter += 1 if not cv.get_result('Depth')[0] is None: depth = 150 - cv.get_result('Depth')[0] * 5 else: depth = 70 pinger = mcu.get_angle() pitch = imu.get_pitch() roll = imu.get_roll() yaw = imu.get_yaw2() pidR.getSetValues(roll) pidP.getSetValues(pitch) pidD.getSetValues(100 - depth) pidY.getSetValues(-yaw) finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(), pidY.start()) sentValues = [] for values in finalPidValues: subValues = values sentValues.append(subValues) motor_fl = sentValues[0] motor_fr = sentValues[1] motor_bl = sentValues[2] + set_speed motor_br = sentValues[3] + set_speed motor_t = sentValues[4] mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t) if counter % 5 == 0: print('Depth:', depth) print('Pinger:', pinger) print('Pitch:', pitch) print('Roll:', roll) print('Yaw:', imu.get_yaw2()) print('Yaw_sent:', yaw) print('Motors: %.2f %.2f %.2f %.2f %.2f' % (motor_fl, motor_fr, motor_bl, motor_br, motor_t)) print() time.sleep(0.1) except KeyboardInterrupt: pass finally: print("Stopping remaining threads...") imu.stop() mcu.stop()
def main(): # read arguments ap = argparse.ArgumentParser() ap.add_argument("-s", "--speed") args = vars(ap.parse_args()) set_speed = args.get('speed', 0) if set_speed is None: set_speed = 0 set_speed = float(set_speed) speed = 0 # inits MCU mcu = MCU(2222) # inits IMU imu = IMU("/dev/ttyUSB_IMU") # start subprocess imu.start() mcu.start() mcu.wait() start_time = time.time() depth_speed = 0 pidR = pidRoll(0.4, 0, 0) # 1, 0 , 0 pidP = pidPitch(0.6, 0, 0) # 5 ,0.1 ,8 pidD = pidDepth(0, 0, 0) pidY = pidYaw(1, 0.4, 0) motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0 try: motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0 counter = 0 while True: counter += 1 depth = mcu.get_depth() pinger = mcu.get_angle() pitch = imu.get_pitch() roll = imu.get_roll() yaw = imu.get_yaw2() pidR.getSetValues(roll) pidP.getSetValues(pitch) pidD.getSetValues(70 - depth) pidY.getSetValues(-yaw) finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(), pidY.start()) sentValues = [] for values in finalPidValues: subValues = values sentValues.append(subValues) if abs((time.time() - start_time) % 5) < 1: depth_speed = 0.4 else: depth_speed = 0 motor_fl = sentValues[0] + depth_speed motor_fr = sentValues[1] + depth_speed motor_bl = sentValues[2] + set_speed motor_br = sentValues[3] + set_speed motor_t = sentValues[4] + depth_speed mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t) if counter % 5 == 0: print('Depth:', depth) print('Pinger:', pinger) print('Pitch:', pitch) print('Roll:', roll) print('Yaw:', imu.get_yaw2()) print('Yaw_sent:', yaw) print('Motors: %.2f %.2f %.2f %.2f %.2f' % (motor_fl, motor_fr, motor_bl, motor_br, motor_t)) print() time.sleep(0.1) except KeyboardInterrupt: pass finally: print("Stopping remaining threads...") imu.stop() mcu.stop()
def main(): # read arguments ap = argparse.ArgumentParser() ap.add_argument("-v", "--video", help="path to the (optional) video file") ap.add_argument("-c", "--camera", help="index of camera") ap.add_argument("-o", "--output", help="path to save the video") ap.add_argument("-s", "--speed") ap.add_argument("-t", "--time") args = vars(ap.parse_args()) if args.get("video", False): vs = args.get("video", False) elif args.get("camera", False): vs = int(args.get("camera", False)) else: vs = 0 set_speed = float(args.get('speed', 0)) set_time = float(args.get('time', 0)) print('Speed', set_speed) print('Time', set_time) if set_speed is None: set_speed = 0 set_speed = float(set_speed) speed = 0 yaw = 0 # inits CV cv = CVManager(vs, # choose the first web camera as the source enable_imshow=True, # enable image show windows server_port=3333, # start stream server at port 3333 delay=5, outputfolder=args.get("output")) cv.add_core("GateTracker", GateTrackerV3(), True) # inits MCU mcu = MCU(2222) # inits IMU imu = IMU("/dev/ttyUSB_IMU") # start subprocess cv.start() imu.start() mcu.start() mcu.wait() time.sleep(3) start_time = time.time() depth_speed = 0 pidR = pidRoll(1, 0.2, 0) # 5, 0.1 , 5 pidP = pidPitch(0.6, 0, 0)# 5 ,0.1 ,8 pidD = pidDepth(0, 0, 0) pidY = pidYaw(1, 0.3, 0) motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0 try: motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0 counter = 0 last_cv_gate = 0 gate_passed = False while time.time() - start_time < set_time: counter += 1 gate, _, gate_size = cv.get_result("GateTracker") depth = mcu.get_depth() pinger = mcu.get_angle() pitch = imu.get_pitch() roll = imu.get_roll() if True or gate_passed or gate is None: # just go straight yaw = imu.get_yaw2(0) else: if gate != last_cv_gate: imu.reset_yaw2(-gate * 0.1, 1) last_cv_gate = gate else: yaw = imu.get_yaw2(1) if gate_size > 350: gate_passed = True if abs(yaw) > 10: speed = 0 else: speed = set_speed if abs((time.time() - start_time) % 5) < 1: depth_speed = 0.45 else: depth_speed = 0 pidR.getSetValues(roll) pidP.getSetValues(pitch) pidD.getSetValues(70-depth) pidY.getSetValues(-yaw) finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(), pidY.start()) sentValues = [] for values in finalPidValues: subValues = values sentValues.append(subValues) motor_fl = sentValues[0] + depth_speed motor_fr = sentValues[1] + depth_speed motor_bl = sentValues[2] + set_speed motor_br = sentValues[3] + set_speed motor_t = sentValues[4] + depth_speed # Put control codes here mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t) if counter % 20 == 0: print('Gate', gate) print('GateSize', gate_size) print('Passed?', gate_passed) print('Depth:', depth) print('Pinger:', pinger) print('Pitch:', pitch) print('Roll:', roll) print('Yaw:', imu.get_yaw2()) print('Yaw_sent:', yaw) print('Motors: %.2f %.2f %.2f %.2f %.2f'%(motor_fl, motor_fr, motor_bl, motor_br, motor_t)) print() time.sleep(0.1) except KeyboardInterrupt: pass finally: mcu.set_motors(0, 0, 0, 0, 0) print("Stopping remaining threads...") cv.stop() imu.stop() mcu.stop()
def main(): """ main body """ ap = argparse.ArgumentParser() ap.add_argument("-o", "--output", help="path to save the video") args = vars(ap.parse_args()) mcu = MCU(2222) mcu.start() mcu.wait() key_listener = KeyListener() key_listener.start() # prepare video streaming cv = CVManager(VideoStream(src=0), server_port=3333, outputfolder=args.get('output')) cv.add_core("Stream", Blank(), True) cv.start() overall_speed = 0.4 while not key_listener.stopped: key, t = key_listener.get_key() # if the key is released more than 0.05s # stop the car if time.time() - t > 0.05: action = (0, 0, 0, 0, 0) else: if key == 'm': overall_speed += 0.01 if key == 'n': overall_speed -= 0.01 # map keyboared to mcu actions action = KEY_MAP.setdefault(key, (0, 0, 0, 0, 0)) action = [round(i * overall_speed, 2) for i in action] # reduce speed mcu.set_motors(action[0], action[1], action[2], action[3], action[4]) print('Action:', action, 'Depth:', mcu.get_depth(), 'Speed:', round(overall_speed, 2), end='\r\n') time.sleep(0.04) cv.stop() mcu.stop()
def main(): # read arguments ap = argparse.ArgumentParser() ap.add_argument("-o", "--output") ap.add_argument("-s", "--speed") ap.add_argument("-t", "--time") # time to turn after passing ap.add_argument("-ft", "--forceturn") # time to force turn after launching, 30 ap.add_argument("-a", "--angle") ap.add_argument("-d", "--depth") # 80 args = vars(ap.parse_args()) set_speed = float(args.get('speed', 0)) set_depth = float(args.get('depth', 0)) time_after_passing = float(args.get('time', 0)) time_force_turn = float(args.get('forceturn', 0)) angle_to_turn = float(args.get('angle', 0)) # inits CV cv = CVManager( 0, # choose the first web camera as the source enable_imshow=True, # enable image show windows server_port=3333, # start stream server at port 3333 delay=5, outputfolder=args.get("output")) cv.add_core("GateTracker", GateTrackerV3(), False) cv.add_core("Flare", Flare(), False) # inits MCU mcu = MCU(2222) # inits IMU imu = IMU("/dev/ttyUSB_IMU") # start subprocess cv.start() imu.start() mcu.start() imu.reset_yaw2(-angle_to_turn, 2) # for state 3 mcu.wait() cv.enable_core("GateTracker") pidR = pidRoll(1, 0.2, 0) # 5, 0.1 , 5 pidP = pidPitch(0.6, 0, 0) # 5 ,0.1 ,8 pidD = pidDepth(1, 0, 0) pidY = pidYaw(1, 0.3, 0) motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0 try: motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0 counter = 0 last_cv_gate = 0 yaw = 0 speed = 0 state = 0 timer_for_state0 = time.time() timer_for_state1 = 0 timer_for_state2 = 0 # 0 -> go find the gate # 1 -> continue after passing the gate # 2 -> find flare # 3 -> surfacing while True: counter += 1 if state == 0: gate, _, gate_size = cv.get_result("GateTracker") depth = mcu.get_depth() pitch = imu.get_pitch() roll = imu.get_roll() speed = set_speed if gate is None: # just go straight yaw = imu.get_yaw2(0) # original heading else: if gate != last_cv_gate: imu.reset_yaw2(-gate * 0.1, 1) last_cv_gate = gate else: yaw = imu.get_yaw2(1) # heading with CV if gate_size > 350: state = 1 if time.time() - timer_for_state0 > time_force_turn: state = 1 print('Gate', gate) print('GateSize', gate_size) # go straight elif state == 1: if timer_for_state1 == 0: # first time cv.disable_core('GateTracker') timer_for_state1 = time.time() elif time.time() - timer_for_state1 > time_after_passing: state = 2 cv.enable_core('Flare') depth = mcu.get_depth() pitch = imu.get_pitch() roll = imu.get_roll() yaw = imu.get_yaw2(0) # original heading speed = set_speed # go to the flare elif state == 2: gate, _, gate_size = cv.get_result("Flare") depth = mcu.get_depth() pitch = imu.get_pitch() roll = imu.get_roll() speed = set_speed print(gate, gate_size) if gate is None: # just go straight yaw = imu.get_yaw2(2) else: if gate != last_cv_gate: imu.reset_yaw2(-gate * 0.1, 1) last_cv_gate = gate else: yaw = imu.get_yaw2(1) if not gate_size is None: if gate_size > 200: timer_for_state2 = time.time() if timer_for_state2 != 0 and time.time( ) - timer_for_state2 > 10: state = 3 # surfacing else: depth = 500 pitch = imu.get_pitch() roll = imu.get_roll() yaw = 0 speed = 0 pidR.getSetValues(roll) pidP.getSetValues(pitch) pidD.getSetValues(set_depth - depth) pidY.getSetValues(-yaw) finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(), pidY.start()) sentValues = [] for values in finalPidValues: subValues = values sentValues.append(subValues) motor_fl = sentValues[0] motor_fr = sentValues[1] motor_bl = sentValues[2] + speed motor_br = sentValues[3] + speed motor_t = sentValues[4] mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t) if counter % 20 == 0: print('State:', state) print('Depth:', depth) print('Pitch:', pitch) print('Roll:', roll) print('Yaw:', imu.get_yaw2()) print('Yaw_sent:', yaw) print('Motors: %.2f %.2f %.2f %.2f %.2f' % (motor_fl, motor_fr, motor_bl, motor_br, motor_t)) print() time.sleep(0.1) except KeyboardInterrupt: pass finally: print("Stopping remaining threads...") cv.stop() imu.stop() mcu.stop()
def main(): """ main body """ mcu = MCU(2222) mcu.start() power = 0 step = 0.02 mcu.wait() while True: print('Depth:', mcu.get_depth()) print('Angle:', mcu.get_angle()) power += step if abs(power) > 0.3: step = -step mcu.set_motors(power, power, power, power, power) print('Power:', power) print() time.sleep(0.5) mcu.stop()
import micropython micropython.alloc_emergency_exception_buf(100) import config Pin('Buzzer').init(Pin.OUT_OD) Pin('Buzzer').high() # silent buzzer from board import Board from mcu import MCU from speed import SpeedManager from slope import SlopeManager mcu = MCU() # blink yellow spd_mngr = SpeedManager() slp_mngr = SlopeManager() uboard = Board(spd_mngr, slp_mngr) #user board while True: #making sure speed and slope leaders go down on red light mcu.yellow() uboard.start() # yellow color while uboard.isReady() is not True: delay(1000) wfi() else: mcu.green() uboard.setReady() #green light and beep: it's safe to get onboard