SERVO_PORT = config.get('ports', 'servos')
COMPASS_PORT = config.get('ports', 'compass')
COMPASS_OFFSET = float(config.get('compass', 'offset'))
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
import ConfigParser
import os
import sys
from drivers.gps.gpsd import GPSDGPS as gps_device
waypoint_dir = os.path.abspath(os.path.dirname(__file__))
### PARSE ARGUMENTS
waypoint_file = sys.argv[1]
waypoint_num = sys.argv[2]
config_path = os.path.join(waypoint_dir, waypoint_file)
#### GPS SETUP
gps_device = gps_device()
gps_device.activate()
### GPS READING
lat = 0.0
lng = 0.0
while lat == 0.0 and lng == 0.0:
gps_device.update()
lat = gps_device.get_current_latitude()
lng = gps_device.get_current_longitude()
### READ CONFIG
config = ConfigParser.ConfigParser()
config.read(config_path)
### CREATE CONFIG
try:
