Code update

Implemented driving rotation count
Implemented pickup PID, needs calibration