Lift safeties

[amikhalev]

Right now, the lift will go to whatever target you set it too for both height and angle. The only thing it enforces is the hard limit switches and software angle/height limits. (as a side note, the software height/angle limits are not enforced on the setDesired* methods, and they should be)

As a part of this, I think the preset heights/angles should be made into a Map<[HeightPreset | AnglePreset}, Double> in the config. Just to make it easier to add more.

Angle safety psuedocode

double getSafePivotAngle(double desiredAngle) {
  if ( currentLiftHeight < HeightPreset.MinRotate - heightTolerance ) {
    return AnglePreset.Center;
  }
  return desiredAngle;
}

Height safety psuedocode

double getSafeLiftHeight(double desiredHeight) {
  if ( currentPivotAngle < (AnglePreset.center - pivotAngleTolerance) ||
       currentPivotAngle > (AnglePreset.center + pivotAngleTolerance)) { // if the picker is out far enough that we can't go below level of elevators
    if (currentLiftHeight < HeightPreset.MinRotate) { // if we are not above the elevators
      return HeightPreset.MinRotate; // don't move
    }
   if ( desiredHeight < HeightPreset.MinRotate ) { // if we want to descend to below the elevators
      return HeightPreset.MinRotate; // then descend to the minimum height at which we can rotate
   }
  }
  return desiredAngle; // if picker is all good, go wherever we need to
}

Safety in commands

For now, the commands for setting the desired lift height & pivot angle should check if the movement is legal by comparing the result of getSafePivotAngle/getSafeLiftHeight with the desired height/angle they wish to set. If it does not match, the command should print a warning and not set it.