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;
}