Create CarriageControl

[gerth2]

https://docs.wpilib.org/en/stable/docs/software/advanced-controls/controllers/trapezoidal-profiles.html

[gerth2]

Two ~ProfiledPIDConrollers~ Trapezoidal Profiles needed - one to control height of elevator, one to control angle of singer.

Per discussion, need a state machine to sequence the order of elevator and singer angle.

Elevator needs to raise to some minimum height before the singer angle is allowed to change.

Singer angle must be aligned to the intake angle before descending below that minimum height.

[gerth2]

See architecture.pdf for the possible commanded positions.

[gerth2]

For the trapezoid Profile:

https://robotpy.readthedocs.io/projects/wpimath/en/latest/wpimath.trajectory/TrapezoidProfile.html#wpimath.trajectory.TrapezoidProfile

Rough, untested usage example for our case:

# Init:

self.kMaxV -> maximum speed the thing should go (in whatever units you're using). Probably a calibration
self.kMaxA -> maximum acceleration - do it as a fraction of max speed (IE, 1 second from stopped to max speed)

# periodic Update:

curSetpoint = ... #  comes in from other code to say where this thing should be physically (in units of meters or degrees or whatever)

curTime = Timer.getFPGATimestamp()

if(prevSetpoint != curSetpoint):
  #New setpoint, need to recalcualte the trajectory
  constraints = TrapezoidProfile.Constraints(self.kMaxV, self.kMaxA)
  profile = TrapezoidProfile(constraints, curSetpoint, self.previousProfiledSetpoint)
  self.profileStartTime = curTime 

curCmdState = profile.calculate(curTime - self.profileStartTime)

# Use curCmdState.velocity and .position for feedforward and feedback motor control

self.prevCmdState = curCmdState