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