[-27.8560783333, 153.15216],
[-27.8561183333, 153.15206],
[-27.85611, 153.151958333],
[-27.856065, 153.151771667],
[-27.85603, 153.151628333],
[-27.8560016667, 153.151535],
[-27.85596, 153.151348333],
[-27.85584, 153.151086667],
[-27.8555833333, 153.150935],
[-27.8554083333, 153.150948333],
[-27.8553283333, 153.150986667],
[-27.8552716667, 153.151021667],
[-27.855235, 153.151121667]]
waypoints.reverse()
pid_cross = lpid.PID(P=2.0, I=0.001, D=0.001)
pid_steer = lpid.PID(P=1.5, I=0.001, D=0.002)
# car.send()
# car.acceleration(25)
def follow_point(point1, point2):
gps_success = False
while not gps_success:
gps_success, coord = gps.getGPS()
print(coord)
# Last point
import libraries.geometry as geo
# Lat, Long
waypoints = [[-27.8552616667, 153.151291667], [-27.8553216667, 153.151405],
[-27.8554266667, 153.151566667], [-27.8555, 153.151725],
[-27.8555866667, 153.15189], [-27.8557016667, 153.152083333],
[-27.855865, 153.152198333], [-27.85597, 153.152228333],
[-27.8560216667, 153.152198333], [-27.8560783333, 153.15216],
[-27.8561183333, 153.15206], [-27.85611, 153.151958333],
[-27.856065, 153.151771667], [-27.85603, 153.151628333],
[-27.8560016667, 153.151535], [-27.85596, 153.151348333],
[-27.85584, 153.151086667], [-27.8555833333, 153.150935],
[-27.8554083333, 153.150948333], [-27.8553283333, 153.150986667],
[-27.8552716667, 153.151021667], [-27.855235, 153.151121667]]
pid_steer = lpid.PID(P=2.0, I=0.001, D=0.001)
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: