Add Feed forward and PID to drivetrain motor controllers

Add P and D to right and left motor controllers on the funbot drivetrain. 

Start by increasing P until oscillation occurs. Then add a bit of D, then add some more P until oscillation occurs again, slowly increase D again. 

The P and D values should be the same for the right and left motor controllers