Implementation: Basic Command Groups

[tomtzook]

The following are the command/capabilities of each system:

Coral Elevator:

• Raise (RaiseCoralElevator)
• Lower (LowerCoralElevator)

Coral Gripper

• In (CollectCoral)
• Out (ReleaseCoral)
• Hold (HoldCoral)

Coral Arm

• not used via command, can be requested to move to specific angles
• to request motion to an angle, in Robot class, call coralArmCommand.setNewTargetPosition(angle)
• to check if the arm reached the position, int Robot class, call coralArmCommand.didReachTargetPosition()
• to integrate those into a command group, use
  • to start arm motion: `Commands.runOnce(()-> coralArmCommand.setNewTargetPosition(angle))
  • to wait until arm reached target position: Commands.waitUntil(()-> coralArmCommand.didReachTargetPosition())

Algae Arm

• Extend (ExtendedAlgaeArm)
• Retract (RetractAlgaeArm)

Algae Gripper

• In (CollectAlgae)
• Out (ReleaseAlgae)
• Hold (HoldAlgae)

With those building blocks, now create the following series of command groups. Create a single instance of each in the Robot class in robotInit. We will attach them to buttons and pathplanner later.

• coralLevel2Place:
  • Retract Elevator
  • Move Coral Arm to angle A (create constant for this angle)
  • Activate Coral Gripper In
• coralLevel3Place:
  • Extend Elevator
  • Move Coral Arm to angle A (create constant for this angle)
  • Activate Coral Gripper In
• coralCollect:
  • Retract Elevator
  • Move Coral Arm to angle B (create constant for this angle)
  • Activate Coral Gripper Out
• algaeCollect:
  • Extend Algae Arm
  • Activate Algae Gripper In
• algaeOut:
  • Retract Algae Arm
  • Activate Algae Gripper Out

Other than these group we also have the default robot state. This state dictates the status of subsystems when the groups are not used. For each subsystem, those states are:

• Coral Gripper:
  • if has Coral, Hold
  • if no Coral, idle
• Coral Arm:
  • idle/stop holding
  • do this later
• Coral Elevator
  • Retracted
• Algae Arm
  • Retracted
• Algae Gripper
  • if has Algae, Hold
  • if no Algae, idle

Add these states as default commands for the subsystems. For the grippers, because there is a condition here, use deferred command:

Command command = Commands.defer(()-> {
    if (subsystem.hasItem()) {
        return new HoldItemCommand(subsysem);
    }
    return Commands.none();
}, subsystem);

For the Coral Arm do not do defaults yet.