Gain adjustment and PID change for HIL

Changed to velocity based PID in the HIL simulations
Adjusted gains for control