start of robot manager

[ryanknj5]

No description provided.

[jonahsnider]

Closes #17

[jonahsnider]

the final thing for us to add to the robot manager is state requests.

most other subsystems just allow you to set their state directly. the robot manager is complex enough that we need to do some extra logic to ensure everything goes smoothly. rather than setting a state directly, we use requests to ask the state machine to go to a state, and it decides if it's safe & valid to do so.

here's a few examples:

public void unjamRequest() {
  switch (getState()) {
      // Do nothing if climbing, don't want to accidentally fall off chain
    case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {}
      // Otherwise, you can always unjam
    default -> setStateFromRequest(RobotState.UNJAM);
  }
}

public void intakeRequest() {
  switch (getState()) {
      // Don't cancel climbing or scoring if intake button is accidentally pressed
    case CLIMBING_1_LINEUP,
        CLIMBING_2_HANGING,
        SPEAKER_SCORING,
        AMP_SCORING,
        FEEDING_SHOOTING,
        PASS_SHOOTING -> {}
    default -> setStateFromRequest(RobotState.INTAKING);
  }
}

generally, we need a control request for each action the driver/operator can take, as well as a few special ones for auto events. here is a complete list of every request we need:

• confirm shot
  • this is what powers the driver's shoot button
  • AMP_WAITING should go to AMP_PREPARE_TO_SCORE
  • speaker wait -> speaker prepare score
  • feeding wait -> feeding prepare shoot
  • otherwise, go to speaker prepare score (the default is to just do a speaker shot)
• wait amp
• wait subwoofer
• wait speaker (for auto)
• unjam
• intake
• stop intaking
  • if state == INTAKING then go to IDLE_NO_GP
  • otherwise do nothing
  • seems counterintuitive why we have this, but it's necessary because of a quirk in how button bindings are triggered (feel free to ask if you want more info on this)
• outtake
• idle with gp (for auto, to mark a note as preloaded)
• stow (no matter what should ALWAYS stow)
  • this should look at the current state and transition to either IDLE_NO_GP or IDLE_WITH_GP, depending on if a note is probably being held (ex. if you're scoring you have a note)
• wait pass
• prepare pass
• next climb state
  • if in climb 1, go to climb 2
  • if in climb 2, do nothing
  • otherwise, go to climb 1
• previous climb state
  • if in climb 2, go to climb 1
  • if in climb 1, go to idle with gp

this seems like a lot of work, but i promise it goes pretty quick once you get in the groove of it. you got this!