protected void setVelPIDParams(PIDParams velParams)
 {
     velController.p = velParams.p;
     velController.i = velParams.i;
     velController.d = velParams.d;
 }
 protected void setPosPIDParams(PIDParams posParams)
 {
     posController.p = posParams.p;
     posController.i = posParams.i;
     posController.d = posParams.d;
 }