def autopilot_loop():
frame = decodeImage(Camera().get_frame())
if frame.any():
now = time.time()
telemetry_data = dict()
telemetry_data["accel_val_auto"] = 0.0
telemetry_data["steering_angle_auto"] = 0.0
telemetry_data["speed"] = 0.0
try:
telemetry_data["accel_val_auto"] = float(prefs.get_pref("accel_val_auto"))
except:
prefs.set_pref("accel_val_auto", 0.0)
try:
telemetry_data["steering_angle_auto"] = float(prefs.get_pref("steering_angle_auto"))
except:
prefs.set_pref("steering_angle_auto", 0.0)
try:
telemetry_data["speed"] = float(prefs.get_pref("speed"))
except:
prefs.set_pref("speed", 0.0)
#telemetry_data["speed"] = float(abs(y))
#log("accel_val", round(telemetry_data["accel_val_auto"], 3), "steering_angle", round(telemetry_data["steering_angle_auto"], 3), "[AUTOPILOT]")
accel_val, steering_angle = telemetry(telemetry_data, frame)
steering_angle = 0
prefs.set_pref("accel_val_auto", accel_val)
prefs.set_pref("steering_angle_auto", steering_angle)
def throttle_loop():
#time.sleep(1)
now = time.time()
telemetry_data = dict()
telemetry_data["accel_val_auto"] = 0.0
telemetry_data["steering_angle_auto"] = 0.0
telemetry_data["speed"] = 0.0
try:
telemetry_data["accel_val_auto"] = float(prefs.get_pref("accel_val_auto"))
except:
pass
try:
telemetry_data["steering_angle_auto"] = float(prefs.get_pref("steering_angle_auto"))
except:
pass
try:
telemetry_data["speed"] = float(prefs.get_pref("speed"))
except:
pass
#telemetry_data["speed"] = float(abs(y))
#log("accel_val", round(telemetry_data["accel_val_auto"], 3), "steering_angle", round(telemetry_data["steering_angle_auto"], 3), "[AUTOPILOT]")
accel_val, steering_angle = telemetry(telemetry_data)
print(telemetry_data["speed"], "\t -> \t", accel_val)
prefs.set_pref("accel_val_auto", accel_val)
def loop(accel_val, steering_angle, rec_toggle=False):
global tank_controls
if rec_toggle:
if prefs.get_pref("rec") == "0":
BUZZER_PATTERN("b b b", 0.3)
LED_PATTERN("R R_")
log("Rec ON")
prefs.set_pref("rec", str(time.time()))
else:
BUZZER_PATTERN("b", 1)
LED_PATTERN("R R")
log("Rec OFF")
prefs.set_pref("rec", "0")
speed = prefs.get_pref("speed")
if AUTOPILOT:
#log("accel_val", round(accel_val, 3), "steering_angle", round(steering_angle, 3), "speed", speed, "[AUTOPilot]")
pass
else:
#log("accel_val", round(accel_val, 3), "steering_angle", round(steering_angle, 3), "speed", speed, "[MANUAL]")
pass
av = str(accel_val)
prefs.set_pref("accel_val", av)
sa = str(steering_angle)
prefs.set_pref("steering_angle", sa)
if tank_controls:
tank_mover(steering_angle, accel_val)
else:
set_accel(accel_val)
set_steering(steering_angle)
def loop():
result, frame = cap.read()
global tank_controls
now = time.time()
accel_val = 0
steering_angle = 0
rec = prefs.get_pref("rec")
av = prefs.get_pref("accel_val")
sa = prefs.get_pref("steering_angle")
try:
accel_val = float(av)
steering_angle = float(sa)
#log("accel_val=", av, "\tsteering_angle=", sa)
except:
#log("accel_val=", av, "\tsteering_angle=", sa, "[ERROR]")
pass
if now - last_command_time(
) < 15: # Stop recording if idle for more than 15 seconds
filename = os.path.join(os.getcwd(), 'training_data', rec, 'data.csv')
log("RECORDING TO ", filename)
if not os.path.exists(os.path.dirname(filename)):
try:
os.makedirs(os.path.dirname(filename))
os.mkdir(os.path.join(os.path.dirname(filename), 'images'))
except OSError as exc: # Guard against race condition
if exc.errno != errno.EEXIST:
raise
time_now = str(now)
imagefile = os.path.join(os.path.dirname(filename), 'images',
time_now + ".jpg")
data_imagefile = os.path.join('images', time_now + ".jpg")
cv2.imwrite(imagefile, frame)
speed = accel_val # TODO : Calculate speed using wheel speed
throttle = 0
brakes = 0
if accel_val > 0:
throttle = accel_val
else:
brakes = accel_val
#myCsvRow = ",".join(list(map(str, [data_imagefile, steering_angle, speed, throttle, brakes])))
myCsvRow = ",".join(
list(map(str, [data_imagefile, steering_angle, accel_val])))
log('Append Row : ', myCsvRow)
with open(filename, 'a') as fd: # Append to file
fd.write(myCsvRow + '\n')
def loop():
global tank_controls
rec = prefs.get_pref("rec")
accel_val = int(prefs.get_pref("accel_val"))
steering_angle = float(prefs.get_pref("steering_angle"))
#print(accel_val, steering_angle, sep=" -- ")
#set_accel(accel_val)
if tank_controls:
tank_mover(steering_angle, accel_val)
else:
set_accel(accel_val)
set_steering(steering_angle)
def main():
while True:
try:
if prefs.get_pref("rec") != "0":
loop()
except Exception as e:
log("RECORDER - ", e)
def autopilot_loop():
global LAST_DATA
while True:
try:
if AUTOPILOT:
prefs.set_pref("AUTOPILOT", "1")
now = time.time()
accel_val = 0
steering_angle = 0
try:
accel_val = float(prefs.get_pref("accel_val_auto"))
except:
log("accel_val_auto error")
pass
try:
steering_angle = float(
prefs.get_pref("steering_angle_auto"))
except:
log("steering_angle_auto error")
pass
#accel_val, steering_angle = drive.telemetry(LAST_DATA, recorder.CURRENT_FRAME)
if abs(prefs.get_pref_time("accel_val_auto") -
now) <= 0.5 or abs(
prefs.get_pref_time("steering_angle_auto") -
now) <= 0.5:
loop(accel_val, steering_angle)
LAST_DATA["accel_val"] = accel_val
LAST_DATA["steering_angle"] = steering_angle
LAST_DATA["speed"] = prefs.get_pref(
"speed"
) #chase_value(accel_val, LAST_DATA["speed"], 0.25)
#prefs.set_pref("speed", LAST_DATA["speed"])
else:
prefs.set_pref("AUTOPILOT", "0")
time.sleep(0.1)
except Exception as e:
log("AUTOPILOT - ", e)
def main(c):
global Camera
Camera = c
while True:
try:
now = time.time()
if prefs.get_pref("AUTOPILOT")=="1":# and abs(now - prefs.get_pref_time("AUTOPILOT")) < 1:
#autopilot_loop()
for i in range(1000):
throttle_loop()
else:
time.sleep(1)
except Exception as e:
log("AUTOPILOT error - ", e)
def get():
"""Video streaming home page."""
PAGE = dict()
try:
params = ("accel_val_auto", "steering_angle_auto", "AUTOPILOT",
"accel_val", "steering_angle", "speed", "rpm", "rec")
log("GOT Request")
for d in params:
PAGE[d] = round(float(prefs.get_pref(d)), 5)
PAGE = json.dumps(PAGE)
except Exception as e:
PAGE = "{'status': 'not ok', 'error': '" + str(e) + "' }"
return PAGE
def do_GET(self):
global params
if "/?" in self.path:
PAGE = "{'status': 'ok'}"
try:
params = parse_qs(self.path[2:])
for d in params:
prefs.set_pref(d, params[d][0])
prefs.set_pref("last_message", str(time.time()))
#print("GOT : ", params)
except Exception as e:
PAGE = "{'status': 'not ok', 'error': '" + str(e) + "' }"
self.send_response(200)
self.send_header('Content-Type', 'text/json')
content = PAGE.encode('utf-8')
self.send_header('Content-Length', len(content))
self.end_headers()
self.wfile.write(content)
elif "/get" in self.path:
PAGE = dict()
try:
req_params = ("accel_val_auto", "steering_angle_auto",
"AUTOPILOT", "accel_val", "steering_angle",
"speed", "rpm", "accelerometer_data",
"gyroscope_data", "voltage", "current", "rec")
log("GOT Request")
for d in req_params:
PAGE[d] = prefs.get_pref(d)
PAGE = json.dumps(PAGE)
except Exception as e:
PAGE = "{'status': 'not ok', 'error': '" + str(e) + "' }"
self.send_response(200)
self.send_header('Access-Control-Allow-Origin', '*')
self.send_header('Content-Type', 'text/json')
content = PAGE.encode('utf-8')
self.send_header('Content-Length', len(content))
self.end_headers()
self.wfile.write(content)
elif self.path == '/':
self.send_response(301)
self.send_header('Location', '/index.html')
self.end_headers()
elif self.path == '/stream.mjpg':
self.send_response(404)
return
self.send_header('Age', 0)
self.send_header('Cache-Control', 'no-cache, private')
self.send_header('Pragma', 'no-cache')
self.send_header('Content-Type',
'multipart/x-mixed-replace; boundary=FRAME')
self.end_headers()
try:
while True:
with output.condition:
output.condition.wait()
frame = output.frame
self.wfile.write(b'--FRAME\r\n')
self.send_header('Content-Type', 'image/jpeg')
self.send_header('Content-Length', len(frame))
self.end_headers()
self.wfile.write(frame)
self.wfile.write(b'\r\n')
except Exception as e:
logging.warning('Removed streaming client %s: %s',
self.client_address, str(e))
else:
SERVER_PATH = "web/"
try:
#print('Opening file : ', self.path[1:])
PAGE = ""
self.send_response(200)
if self.path.endswith('.png'):
#print('Inside IF')
f = open(os.path.join(SERVER_PATH, self.path[1:]), 'rb')
PAGE = f.read()
self.send_header('Content-Type', 'image/png')
content = PAGE
elif self.path.endswith('.bin'):
#print('Inside IF')
f = open(os.path.join(SERVER_PATH, self.path[1:]), 'rb')
PAGE = f.read()
self.send_header('Content-Type',
'application/octet-stream')
content = PAGE
elif self.path.endswith('.js'):
#print('Inside IF')
f = open(os.path.join(SERVER_PATH, self.path[1:]), 'r')
PAGE = f.read()
self.send_header('Content-Type', 'text/javascript')
content = PAGE
else:
f = open(os.path.join(SERVER_PATH, self.path[1:]), 'r')
PAGE = f.read()
self.send_header('Content-Type', 'text/html')
content = PAGE.encode('utf-8')
self.send_header('Content-Length', len(content))
self.end_headers()
self.wfile.write(content)
except Exception as e:
self.send_error(404)
self.end_headers()
log("web_server error ", e)
def speed_calculator():
global wheel_speed_counter, wheel_speed_counter_last_set, wheel_speed_delay
global MPU_last_set, MPU_delay, MPU_sensor
global last_data_set, data_delay, ser
gyroscope_data_old = {'x': 0, 'y': 0, 'z': 0}
prefs.set_pref("speed", 0)
#time.sleep(10)
while True:
try:
now = time.time()
#reading = GPIO.input(wheel_speed_data_pin)
#if reading:
#wheel_speed_counter += 1
#while GPIO.input(wheel_speed_data_pin) == 1:
#time.sleep(0.01)
#pass # Wait for the sensor to read 0 again before reading the next 1
#if abs(now - last_data_set)>= data_delay:
if True:
last_data_set = now
rpm, voltage, current, power, energy = list(
map(
float,
ser.readline().decode("utf-8").replace(
'\r', '').replace('\n', '').split(",")))
prefs.set_pref("voltage", voltage)
prefs.set_pref("current", current)
#prefs.set_pref("power", currpowerent)
#prefs.set_pref("energy", current)
#log("power_sensor", voltage, current)
if False: # Todo finish fault trigger
log("speed_calculator FAULT")
prefs.set_pref("speed", 0)
prefs.set_pref("rpm", 0)
else:
speed = float(prefs.get_pref("speed"))
#accel_val = float(prefs.get_pref("accel_val"))
calculated_speed = rpm / 60.0 * gear_ratio
speed = chase_value(calculated_speed, speed)
#speed = calculated_speed
#if speed>100:
# speed = 100
#if speed<5 or calculated_speed<5:
# speed = 0
prefs.set_pref("speed", abs(speed))
prefs.set_pref("rpm", abs(int(rpm)))
if prefs.get_pref("AUTOPILOT") != "1":
log("speed_calculator", speed, rpm)
if abs(now - MPU_last_set) >= MPU_delay:
accelerometer_data = MPU_sensor.get_accel_data()
gyroscope_data = MPU_sensor.get_gyro_data()
tmp = gyroscope_data
for k in gyroscope_data:
gyroscope_data[k] = chase_value(gyroscope_data[k],
gyroscope_data_old[k], 0.5)
gyroscope_data_old = tmp
prefs.set_pref("accelerometer_data", str(accelerometer_data))
prefs.set_pref("gyroscope_data", str(gyroscope_data))
MPU_last_set = now
#global Camera
#frame = decodeImage(Camera().get_frame())
#x, y = image_processing.get_direction(frame, history_frames=15, frame_skip=0, scale_percent=10)
#log("speed_calculator", abs(y))
#prefs.set_pref("speed", abs(y))
#log("speed_calculator", prefs.get_pref("speed"))
except Exception as e:
log("speed_calculator error - ", e)
prefs.set_pref("speed", 0)
time.sleep(1)
def loop():
global last_compile
global tank_controls
now = time.time()
rec = prefs.get_pref("rec")
accel_val = int(prefs.get_pref("accel_val"))
steering_angle = float(prefs.get_pref("steering_angle"))
recompile = prefs.get_pref("recompile")
if now-last_compile>60*10 or recompile=='1': # Recompile training data every 10 minutes
compile_data()
last_compile = now
if recompile=='1':
prefs.set_pref("recompile", '0')
#print(accel_val, steering_angle, sep=" -- ")
#set_accel(accel_val)
# TODO use get_pref_time
#if rec != '0' and time.time()-float(prefs.get_pref("last_message"))<15: # Last command issued within 15 seconds
# Last command issued within 15 seconds
if rec != '0' and rec!="" and (now-float(prefs.get_pref_time("accel_val"))<15 or now-float(prefs.get_pref_time("steering_angle"))<15):
#if False:
# print("REC...")
filename = os.path.join(os.getcwd(), 'training_data', rec, 'data.csv')
if not os.path.exists(os.path.dirname(filename)):
try:
os.makedirs(os.path.dirname(filename))
os.mkdir(os.path.join(os.path.dirname(filename), 'images'))
except OSError as exc: # Guard against race condition
if exc.errno != errno.EEXIST:
raise
time_now = str(now)
imagefile = os.path.join(os.path.dirname(filename), 'images', time_now + ".jpg")
data_imagefile = os.path.join('images', time_now + ".jpg")
#camera.capture(imagefile)
result, frame = cap.read()
if result:
# print("Got Image")
cv2.imwrite(imagefile, frame)
speed = accel_val # TODO : Calculate speed
throttle = 0
brakes = 0
if accel_val>0:
throttle = accel_val
else:
brakes = accel_val
myCsvRow = ",".join(list(map(str, [data_imagefile, steering_angle, speed, throttle, brakes])))
# print('Append Row : ', myCsvRow)
#myCsvRow = " ".join(list(map(str, [imagefile, steering_angle, speed, accel_val])))
with open(filename, 'a') as fd: # Append to file
fd.write(myCsvRow + '\n')
else:
print("REC ERROR - COULD NOT GET IMAGE")
def do_GET(self):
global params
if "/?" in self.path:
PAGE = "{'status': 'ok'}"
try:
params = parse_qs(self.path[2:])
for d in params:
prefs.set_pref(d, params[d][0])
prefs.set_pref("last_message", str(time.time()))
#print("GOT : ", params)
except Exception as e:
PAGE = "{'status': 'not ok', 'error': '" + str(e) + "' }"
self.send_response(200)
self.send_header('Content-Type', 'text/json')
content = PAGE.encode('utf-8')
self.send_header('Content-Length', len(content))
self.end_headers()
self.wfile.write(content)
elif "/get" in self.path:
PAGE = "{'status': 'Searching'}"
try:
req = self.path.split("/")[2]
#print(req)
PAGE = prefs.get_pref(req)
if PAGE=="":
PAGE = "NULL"
#print("GOT : ", {req: PAGE})
#PAGE = str(params)
except Exception as e:
PAGE = "{'status': 'not ok', 'error': '" + str(e) + "' }"
self.send_response(200)
self.send_header('Content-Type', 'text/json')
content = PAGE.encode('utf-8')
self.send_header('Content-Length', len(content))
self.end_headers()
self.wfile.write(content)
elif self.path == '/':
self.send_response(301)
self.send_header('Location', '/index.html')
self.end_headers()
elif self.path == '/stream.mjpg':
self.send_response(200)
self.send_header('Age', 0)
self.send_header('Cache-Control', 'no-cache, private')
self.send_header('Pragma', 'no-cache')
self.send_header('Content-Type', 'multipart/x-mixed-replace; boundary=FRAME')
self.end_headers()
try:
while True:
with output.condition:
output.condition.wait()
frame = output.frame
self.wfile.write(b'--FRAME\r\n')
self.send_header('Content-Type', 'image/jpeg')
self.send_header('Content-Length', len(frame))
self.end_headers()
self.wfile.write(frame)
self.wfile.write(b'\r\n')
except Exception as e:
logging.warning(
'Removed streaming client %s: %s',
self.client_address, str(e))
else:
try:
#print('Opening file : ', self.path[1:])
PAGE = ""
self.send_response(200)
if self.path.endswith('.png'):
#print('Inside IF')
f = open(self.path[1:], 'rb')
PAGE = f.read()
self.send_header('Content-Type', 'image/png')
content = PAGE
else:
f = open(self.path[1:], 'r')
PAGE = f.read()
self.send_header('Content-Type', 'text/html')
content = PAGE.encode('utf-8')
self.send_header('Content-Length', len(content))
self.end_headers()
self.wfile.write(content)
except Exception as e:
self.send_error(404)
self.end_headers()
print(e)
def main():
#global Camera
#Camera = c
global shutdown_request, AUTOPILOT, LAST_DATA
FIRST_COMMAND = True
while True:
try:
LAST_DATA["accel_val"] = 0 # 0 to 100
LAST_DATA["steering_angle"] = 0 # 0 to 1
LAST_DATA["speed"] = 0 # 0 to 100
accel_val = 0
steering_angle = 0
SPEED_MODE = 1
for event in gamepad.read_loop():
if FIRST_COMMAND:
BUZZER_PATTERN("b b", 0.1)
FIRST_COMMAND = False
#accel_val = 0
#steering_angle = 0
if event.code == 16 and event.value == -1 and event.type == 3:
AUTOPILOT = True
BUZZER_PATTERN("b b b", 0.1)
LED_PATTERN("B B_")
if event.code == 16 and event.value == 0 and event.type == 3:
if AUTOPILOT:
AUTOPILOT = False
BUZZER_PATTERN("b b b", 0.1)
LED_PATTERN("G")
rec_toggle = False
if event.code == 16 and event.value == 1 and event.type == 3:
rec_toggle = True
if event.code == 17 and event.value == 1 and event.type == 3:
log("Compile event triggered")
BUZZER_PATTERN("b b", 0.1)
if prefs.get_pref("rec") == "0":
LED_PATTERN("B B")
os.system('python3 compile.py > logs/compile.txt &')
else:
LED_PATTERN("R R_", 0.25)
log("Did not fire compile [currently recording]")
now = time.time()
if event.code == 17 and event.value == -1 and event.type == 3:
if shutdown_request == 0:
log("Shutdown request triggered")
shutdown_request = now
LED_PATTERN("RB" * 4, 0.25)
if event.code == 17 and event.value == 0 and event.type == 3:
if now - shutdown_request < 2 and shutdown_request != 0:
log("Shutdown request dropped")
LED_PATTERN("G_")
shutdown_request = 0
if shutdown_request != 0:
if now - shutdown_request >= 2: # Shutdown button pressed for 2 seconds or more
log("SHUTDOWN SIGNAL")
LED_PATTERN("R_")
BUZZER_PATTERN("b b b b b", 0.1)
time.sleep(0.5)
os.system("halt")
if event.type != 0:
#filters by event type
#log(type(event.code), event.code)
if event.code == 1:
accel_val = corrected_reading(event.value) * 100
elif event.code == 2:
steering_angle = corrected_reading(event.value)
elif event.code == 304: # A
SPEED_MODE = 1
elif event.code == 305: # B
SPEED_MODE = 2
elif event.code == 307: # X
SPEED_MODE = 3
elif event.code == 308: # Y
SPEED_MODE = 4
if abs(accel_val) > 25 * SPEED_MODE:
if accel_val != 0: # To prevent divide by zero error
accel_val = 25 * SPEED_MODE * accel_val / abs(
accel_val)
else:
accel_val = 25 * SPEED_MODE
loop(accel_val, steering_angle, rec_toggle)
LAST_DATA["accel_val"] = accel_val
LAST_DATA["steering_angle"] = steering_angle
except Exception as e:
log(e)
if ("No such device" in str(e)):
log("Bluetooth connection lost")
