Write commands to automatically intake and score game pieces

[n-arms]

To minimize the operator fooling around with the controls, the operator controller should have a number buttons that move the arm to preset positions for scoring and intaking. Since we're only going to be scoring cubes, there's not many positions!

Each command should be run when the operator holds down the appropriate button; if they release it, the command should be canceled.

intaking

There should be commands for double substation and ground intake. Both commands should make sure the elevator is completely retracted (position mode), then rotate to the correct angle (PID mode) and run the intake continuously.

scoring

There should be commands for scoring a cube in mid and low. Both commands should lift the arm to the appropriate angle using PID mode, then (slowly) extend the elevator to the correct position (position mode). The claw shouldn't outtake until a separate button is pressed, as there's a decent chance the driver will need to manoeuvre the robot to get into ideal scoring position.

[rynky]

Added rotation for intaking. Integrating cube scoring should be trivial, but need to debrief about button mapping.

[SomewhatMay]

Added rotation for intaking. Integrating cube scoring should be trivial, but need to debrief about button mapping.

I took a look at your code and I don't think the .setPositionMode(true) and .setPositionMode(false) do anything and are unnecessary on lines 42, 45, 49, 52 since they're in periodic(). What was your initial thought process for adding them?

[rynky]

Assumed that .setPositionMode() would switch between position mode and PID mode as per the instructions of the issue. Feel free to resolve the particularities of the elevator, as it isn't finalized yet.

[n-arms]

I feel like we might want to leave TeleopIntake alone, it's poorly named but it was originally supposed to just run the claw (and I don't want to get rid of manual claw functionality, if stuff breaks at comp, all the manual control options should still be available).

I think it might make sense to get the pre-existing Intake commands working instead? I didn't see them before, but you reference them in your code.

[rynky]

The functions seem to be fully functional (although I suppose you can't really tell until testing). There also seems to be functions that score cubes mid and low already. What file do you suggest we move to then? Or is this worth opening another issue?

[SomewhatMay]

Assumed that .setPositionMode() would switch between position mode and PID mode as per the instructions of the issue. Feel free to resolve the particularities of the elevator, as it isn't finalized yet.

You don't need to switch it off, and then switch it back on in the same periodic loop, that does practically nothing... I would remove that line. The position mode boolean is there just in case the encoder doesn't work and we need to resort to manually setting the speed instead of using a position I believe. I can remove them for you.

[rynky]

Alright, perfect. Thanks!