waypoint_config_path = os.path.join(brain_dir, "waypoints", "school_points.cfg")
waypoint_config = ConfigParser.ConfigParser()
waypoint_config.read(waypoint_config_path)
waypoints = []
for waypoint in waypoint_config.sections():
waypoints.append(point.Point(waypoint_config.get(waypoint, 'latitude'), waypoint_config.get(waypoint, 'longitude')))
curr_waypoint = 0
curr_waypoint_point = waypoints[curr_waypoint]
#### GPS SETUP
gps_device = gps_device()
gps_device.activate()
gps_device.update()
### SERVO SETUP
servo = servo_device(port=SERVO_PORT)
servo.reset_all()
STEERING_SERVO = 0
DRIVE_SERVO = 1
#THROTTLE_MAX = 1660
THROTTLE_MAX = 1650
#THROTTLE_MIN = 1650
THROTTLE_MIN = 1620
STEERING_FULL_RIGHT = 1660
STEERING_FULL_LEFT = 1460
#STEERING_CENTER = 1557
STEERING_CENTER = 1557
STEERING_GAIN = 1.3
### COMPASS SETUP
"""Reads data from a GPS file as 'pretend' reading of GPS"""
from geopy import distance
from upoints import point
from drivers.servo.maestro import MaestroServoController as servo_device
CORNERS = [
(39.7188683333, -104.702126667),
(39.7188783333, -104.70285),
(39.71887, -104.702193333),
(39.7188983333, -104.702183333)
]
curr_corner = 0
servo = servo_device()
with open('gps_log2.csv') as gps_file:
servo.reset_all()
for row in gps_file:
(date, time, lat, lng, altitude, velocity, heading) = row.split(',')
if date == 'Date':
continue # skip header row
curr_point = (float(lat), float(lng))
curr_point_point = point.Point(lat, lng)
heading = float(heading.rstrip())
curr_corner_point = point.Point(CORNERS[curr_corner][0], CORNERS[curr_corner][1])
dist_to_corner = distance.distance(curr_point, CORNERS[curr_corner]).feet
bearing_to_corner = curr_point_point.bearing(curr_corner_point)
