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
def main():
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()
time.sleep(1)
lat = float(input("Destination lat:"))
long = float(input("Destination long:"))
path = Path_Planning(long, lat, sf)
dist = GPS_Interface.haversin(sf.gps_vector,
[lat, long]) # calc distance remaining
while True:
correction = path.follow_path() # calculate the correction vector
angle = tan(correction[1] /
correction[0]) # convert cartesian into an angle
sf_mag = sqrt(sf.position_x**2 + sf.position_y**
2) # convert the sf position into cylindrical coords
dif_angle = angle - sf.orientation # calculate the angular difference
dif_dist = dist - sf_mag # calculate the linear difference
# output is proportional to dif calcs, (props, have saturation implemented)
car.steering_angle = dif_angle
car.motor_speed = dif_dist
print("dist:", dif_dist, "angle:", dif_angle, "correction vector:",
correction)
if dif_dist < 1:
print("Reached destination")
car.stop_thread()
sf.stop_thread()
return
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)
car.motor_speed = 0
for i in range(-30, 30):
car.steering_angle = i
time.sleep(.25)
print("steer angle", i)
for i in range(-30, 30):
car.steering_angle = 60 - i
time.sleep(.25)
print("steer angle:", 60 - i)