TrackY and fixed drive

I used the sonar to find the distance so we can drive to a specific
distance to the goal with a buffer so the robot doesn't spaz.  I also
fixed the drive class.