Moar PID testing

put autonomous command back in autonomous section. also began tuning Kp
and Kd. Voltage is going over the roof