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 index():
"""Video streaming home page."""
PAGE = "{'status': 'ok'}"
try:
params = dict()
params["accel_val_auto"] = request.args.get('accel_val_auto')
params["steering_angle_auto"] = request.args.get('steering_angle_auto')
for d in params:
prefs.set_pref(d, params[d])
prefs.set_pref("last_message", str(time.time()))
log("Set : ", params)
except Exception as e:
PAGE = "{'status': 'not ok', 'error': '" + str(e) + "' }"
return PAGE
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 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 do_GET(self):
if "/api/request?" in self.path: # This is used to accept inputs from outside
# For example, by requesting /api/?callibrate_apps=1 we can remotely trigger the calibration
global params
PAGE = "{'status': ok}"
try:
params = parse_qs(self.path[2:])
for d in params:
prefs.set_pref(d, params[d][0])
prefs.set_pref(d + "_last_message", str(time.time()))
prefs.set_pref("last_message_all", 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 "/api/data" in self.path: # send data
data_gen()
self.send_response(200)
PAGE = json.dumps(DATA)
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)
elif self.path == '/': # Redirect / to /index.html
self.send_response(301)
self.send_header('Location', '/index.html')
self.end_headers()
else: # Try opening the requested file
try:
print('Opening file : ', self.path[1:])
PAGE = ""
self.send_response(200)
if self.path.endswith(
'.png'): # Currently supports only PNG images
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 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 start_scan():
#input("Enter to start")
print("Connecting to serial")
s = False
for i in range(4):
COM_PORT = "/dev/ttyUSB" + str(i)
print("Trying to connect to", COM_PORT)
try:
s = serial.Serial(COM_PORT, 115200)
break
except:
print("FAILED", COM_PORT)
time.sleep(1)
if s == False:
print("No COM PORTS found")
return
print("Connected to", COM_PORT)
POINTS = []
while True:
try:
time.sleep(0.2)
s.write(1)
r, theta, phi = list(map(int, s.readline().split()))
#r, phi, theta = list(map(int, s.readline().split()))
#print("Analysing", [r, theta, phi])
if r == 0:
raise Exception("r == 0")
# r = r + (random.uniform(-0.3, 0.3))
phi = phi - 100
theta = theta + 90
theta = theta * pi / 180
phi = phi * pi / 180
#r = r/3.125 # Scaling
#r = r*0.8 # Scaling
sf = 1
d = 2.7 * sf
R = 1.2 * sf
phi_d = atan(((r + R) * sin(phi) + d * cos(phi)) /
((r + R) * cos(phi) - d * sin(phi)))
r_d = ((r + R) * cos(phi) - d * sin(phi)) / cos(phi)
#r = r_d
#phi = phi_d
dist = r * cos(phi)
if dist < 15: # Distance in cm
print("Too close ", {"r": r, "dist": dist})
POINTS.append([r, theta, phi, dist])
if len(POINTS) > 6:
print("Saving to distance_sensor")
prefs.set_pref("distance_sensor", str(POINTS))
POINTS = []
except Exception as e:
print(e)
