def follow_point(point, threads):
cord_lat, cord_long = read_GPS_until_success(point, threads)
dist_to_p = bearings.coord_dist_meters(point[0], point[1], cord_lat,
cord_long)
while dist_to_p > DIST_THRES_METER:
heading_want = bearings.coord_bearing_degrees(cord_lat, cord_long,
point[0], point[1])
heading_actual = threads[0].poll() # Should give back degrees
heading_error = heading_want - heading_actual
if heading_error > 180:
heading_error = heading_error - 360
elif heading_error <= -180:
heading_error = heading_error + 360
turn_strength_pid = pid_steer.update(heading_error)
print('turn_strength_pid: ', turn_strength_pid, ', turn direction: ',
'left' if turn_strength_pid > 0 else 'right')
send_to_arduino()
time.sleep(0.01)
# Get the update for the next loop
cord_lat, cord_long = read_GPS_until_success(point, threads)
dist_to_p = bearings.coord_dist_meters(point[0], point[1], cord_lat,
cord_long)
def follow_point(point):
# Try and get GPS coordinates
coords = getCoords()
while bearings.coord_dist_meters(coords[0], coords[1], point[0],
point[1]) > DIST_THRES_METER:
# Get current coords long and lat
coords = getCoords()
if bearings.coord_dist_meters(
coords[0], coords[1], centerOfTrack[0], centerOfTrack[1]
) > TRACK_RADIUS: #check if car is out of the track, if so stop car and wait
car.stop()
car.send()
print("OUT OF TRACK, KILLING ALL SYSTEMS")
time.sleep(0.1)
continue
follow_blob(
) #goto blob until blob dissapears then return to main loop
# Calculate bearing
coordsDirection = bearings.coord_bearing_degrees(
coords[0],
coords[1], # Our location
point[0],
point[1]) # waypoint location
coordsAngleError = coordsDirection - 0 #imu.getCompass()#calculate steering error
print("Steering error:", coordsAngleError)
car.steer(coordsAngleError)
car.send()
time.sleep(0.1)
def follow_point(point):
gps_success = False
while not gps_success:
gps_success, coord = gps.getGPS()
if not gps_success:
print('GPS Error')
dist_to_p = bearings.coord_dist_meters(point[0], point[1], coord[0],
coord[1])
while dist_to_p > lsettings.DIST_THRES_METER:
heading_want = bearings.coord_bearing_degrees(coord[0], coord[1],
point[0], point[1])
heading_actual = imu.getCompass()
heading_error = heading_want - heading_actual
if heading_error > 180:
heading_error = heading_error - 360
elif heading_error <= -180:
heading_error = heading_error + 360
# Color blob
red_target = lcolor.get_largest_blob_x_y(lsettings.RED_THRES_LOW,
lsettins.RED_THRES_HIGH,
radius=30)
# If there is a target
if red_target[0] is not -1:
# Error is the left or right from center
target_x, target_y = red_target
target_error = target_x - (lsettings.IMAGE_WIDTH / 2)
turn_strength_pid = pid_steer.update(target_error)
else:
turn_strength_pid = pid_steer.update(heading_error)
# Positive is right
print('turn_strength_pid: ', turn_strength_pid, ', turn direction: ',
'left' if turn_strength_pid > 0 else 'right')
steering_scaled = steer_scale_value(turn_strength_pid)
car.steer(steering_scaled)
car.send()
time.sleep(0.01)
# Check position
gps_success = False
while not gps_success:
gps_success, coords = gps.getGPS()
if not gps_success:
print('GPS Error')
dist_to_p = bearings.coord_dist_meters(point[0], point[1], coord[0],
coord[1])
def follow_point(point):
# Try and get GPS coordinates
coords = getCoords()
while bearings.coord_dist_meters(coords[0], coords[1], point[0],
point[1]) > DIST_THRES_METER:
# Get current coords long and lat
coords = getCoords()
if bearings.coord_dist_meters(
coords[0], coords[1], centerOfTrack[0], centerOfTrack[1]
) > TRACK_RADIUS: # check if car is out of the track, if so stop car and wait
# TODO
Ardu.updateKill(1)
Ardu.convertAll()
Ardu.sendCommands()
# car.stop()
# car.send()
print("OUT OF TRACK, KILLING ALL SYSTEMS")
input()
###########ADD BLOB IF STATMENT/DETECTION HERE
# if cv.are_blobs():
# follow_blob()
## driveToBlob() goto blob until blob dissapears then return to main loop
# Calculate bearing
coordsDirection = bearings.coord_bearing_degrees(
coords[0],
coords[1], # Our location
point[0],
point[1]) # waypoint location
coordsAngleError = bearings.subtract_angles(
coordsDirection, imu.getCompass()) # calculate steering error
# print("Current Compass: "******" Coord angle: ",coordsDirection," Steer Error: ",coordsAngleError," Distance to target: ",bearings.coord_dist_meters(coords[0], coords[1], point[0], point[1]))
print("Steering error:", coordsAngleError)
# TODO
A_err = coordsAngleError / 90
Ardu.updateSteering(90 + 30 * A_err)
Ardu.convertAll()
Ardu.sendCommands()
# car.steer(coordsAngleError)
# car.send()
time.sleep(0.01)
Ardu.updateKill(1)
Ardu.convertAll()
Ardu.sendCommands()
def follow_point(point):
gps_success = False
while not gps_success:
gps_success, coord = gps.getGPS()
if not gps_success:
print('GPS Error')
dist_to_p = bearings.coord_dist_meters(point[0], point[1], coord[0],
coord[1])
while dist_to_p > lsettings.DIST_THRES_METER:
heading_want = bearings.coord_bearing_degrees(coord[0], coord[1],
point[0], point[1])
heading_actual = imu.getCompass()
heading_error = heading_want - heading_actual
if heading_error > 180:
heading_error = heading_error - 360
elif heading_error <= -180:
heading_error = heading_error + 360
turn_strength_pid = pid_steer.update(heading_error)
print('turn_strength_pid: ', turn_strength_pid, ', turn direction: ',
'left' if turn_strength_pid > 0 else 'right')
time.sleep(0.01)
# Check position
gps_success = False
while not gps_success:
gps_success, coords = gps.getGPS()
if not gps_success:
print('GPS Error')
dist_to_p = bearings.coord_dist_meters(point[0], point[1], coord[0],
coord[1])
def follow_point(point):
# Try and get GPS coordinates
mag = imu.getCompass()
gps_success = False
while not gps_success:
gps_success, coords = gps.getGPS()
while bearings.coord_dist_meters(coords[0], coords[1], point[0],
point[1]) > 10:
# Get current coords long and lat
# Calculate bearing
coordsDirection = bearings.coord_bearing_degrees(
coords[0],
coords[1], # Our location
point[0],
point[1]) # waypoint location
coordsError = coordsDirection - imu.getCompass()
car.steer(coordsError)
time.sleep(0.1)
def follow_point(point1, point2):
gps_success = False
while not gps_success:
gps_success, coord = gps.getGPS()
print(coord)
# Last point
if point2[0] == -1 and point2[1] == -1:
point2 = point1
dist_to_p1 = bearings.coord_dist_meters(
point1[0], point1[1], coord[0], coord[1])
dist_to_p2 = bearings.coord_dist_meters(
point2[0], point2[1], coord[0], coord[1])
while dist_to_p1 > lsettings.DIST_THRES_METER or dist_to_p2 > lsettings.DIST_THRES_METER:
# Calculate cross track distance
cross_track_dist = geo.cross_track_distance(
point1[0],
point1[1],
point2[0],
point2[1],
coord[0],
coord[1]
)
# Update PID and get desired error
desired_heading = pid_cross.update(cross_track_dist)
# Get current coords long and lat
# Calculate bearing
mid_point = geo.midpoint(point1[0], point1[1], point2[0], point2[1])
actual_heading = bearings.coord_bearing_degrees(mid_point[0], mid_point[1], coord[0], coord[1])
# Get yaw rate of change
# TODO: Scale yaw_roc to between 0 and 1
yaw_roc = imu.get_yaw_roc()
yaw_roc = yaw_roc / 250
# Second PID
# Subtracting angle and moding it
error_heading = (((actual_heading + desired_heading) % 360) - imu.getCompass()) % 360
steering_tuned = pid_steer.update(error_heading, delta_term=yaw_roc)
steering_tuned = steering_tuned + 250
# TODO: Scale steering_tuned
print['left', 'right'][steering_tuned < 0], 'steering: ', steering_tuned, ', yaw_roc: ', yaw_roc, 'desired_heading: ', desired_heading
# Steering
# car.steer(steering_tuned)
# car.send()
# 30 updates per second
# time.sleep(0.033)
# TODO: Remove this
time.sleep(0.01)
# Check position
gps_success = False
while not gps_success:
gps_success, coords = gps.getGPS()
dist_to_p1 = bearings.coord_dist_meters(
point1[0], point1[1], coord[0], coord[1])
dist_to_p2 = bearings.coord_dist_meters(
point2[0], point2[1], coord[0], coord[1])
