def main():
print("Starting all interfaces")
car = Car_Interface(loc="/dev/ttyUSB2")
imu = IMU_Interface(loc="/dev/ttyUSB0")
gps = GPS_Interface(loc="/dev/ttyACM0")
sf = Sensor_Fusion(IMU=imu, GPS=gps)
imu.start()
gps.start()
car.start()
sf.start()
fk_lat = 43.180208
fk_long = -79.790125
time.sleep(5) # allow everything to start
lat = float(input("Destination lat"))
long = float(input("Destination long"))
#pp = Path_Planning(long, lat, sf)
while True:
#dist = GPS_Interface.haversin(sf.gps_vector, [lat, long])
#angle = GPS_Interface.bearing_to(sf.gps_vector, [lat, long])
dist = GPS_Interface.haversin([fk_lat, fk_long], [lat, long])
angle = GPS_Interface.bearing_to([fk_lat, fk_long], [lat, long])
# convert and compare the correction vector to the car direction
# turn until theta = 0, then drive towards the point
dif_angle = angle - sf.orientation
print("Distance:", dist, "angle:", angle, "error:", dif_angle)
if dist > 3: # 3m is the tolerance of the gps
if abs(dif_angle) > 1: # 1 degree of tolerance (car can't steer to that accuracy anyways)
car.steering_angle = dif_angle
car.motor_speed = 50
car.left_led = True
car.right_led = False
else:
# drive straight
car.steering_angle = 0
car.motor_speed = 100
car.left_led = False
car.right_led = True
else:
print("Done")
car.motor_speed = 0
car.stop_thread()
sf.stop_thread()
return
car.start()
print("interface status", car.running)
print("Starting Car")
time.sleep(5)
print("Indicating left led")
car.left_led = True
time.sleep(2.5)
car.left_led = False
time.sleep(1)
print("Indicating right led")
car.right_led = True
time.sleep(2.5)
car.right_led = False
time.sleep(1)
print("Flashing both lights")
car.flash_led()
time.sleep(3)
car.steering_angle = 0
for i in range(-128, 128):
car.motor_speed = i
time.sleep(.25)
print("Motor speed:", i)