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):
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)
import time
import libraries.gps as gps
import libraries.imu as imu
while True:
print(gps.getGPS())#imu.getCompass())
time.sleep(0.05)
def getCoords():
gps_success = False
while not gps_success:
gps_success, coords = gps.getGPS()
return coords
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])
import libraries.gps as gps
import libraries.imu as imu
import libraries.car as car
import math
car.send()
x = gps.getGPS()
while True:
# print(math.degrees(imu.getCompass()))
if x != gps.getGPS():
x = gps.getGPS()
# print(x)
print(x.latitude, x.longitude)
import time
import libraries.gps as gps
with open("gps_coords.txt", "a") as myfile:
gps_success = False
for i in range(10):
gps_success, coord = gps.getGPS()
time.sleep(0.05)
if gps_success:
break
if gps_success:
myfile.write(str(coord[0]) + ', ' + str(coord[1]) + '\n')
print "Wrote file"
else:
print "Error GPS"
