Minor changes to allow ptam data to be used as input for the PD controller. Reduced differential gain to deal with noisy ptam. Waypoints are a path around the arena