Added feedback loops to the code

The feedback loops and accompanying math should work now, but it hasn't been tested yet. There are two loops: one to control distance from the wall, and the other to control the angle that the car is making with the wall (referred to as velocity, or the rate of change of distance from the wall). The output of the velocity loop should be used to control the steering servo.