Reset arm/elevator encoder position at start of auto

[edf42001]

I suggest a) removing the line new RunCommand(() -> X.setSpeed(X)).withTimeout(0.2). I suggested it just to make sure that the command didn't end the split second it started, because the mechanishm wouldn't yet be moving. The debouncer should take care of it, and we want to save time in auto.

Then b) we don't want the arm to automatically move to 0, as that would hit the nodes, we could have it move to turtle mode instead

    private static Command resetArmEncoderCommand(Arm a) {
        Debouncer debouncer = new Debouncer(0.2);
        return new SequentialCommandGroup(
            new PrintCommand("Resetting arm encoder"),
            new InstantCommand(() ->  a.disable()),
            new RunCommand(() -> a.setSpeed(0.15)).until(() -> debouncer.calculate(Math.abs(a.getEncoderSpeed()) < 0.01)),
            new InstantCommand(() -> a.setPosition(ArmConstants.INITIAL_OFFSET)),
            makeSetPositionCommand(a, ArmConstants.TURTLE_ANGLE)
        );
    }

    private static Command resetElevatorEncoderCommand(Elevator e) {
        Debouncer debouncer = new Debouncer(0.2);
        return new SequentialCommandGroup(
            new PrintCommand("Resetting elevator encoder"),
            new InstantCommand(() ->  e.disable()),
            new RunCommand(() -> e.setSpeed(-0.10)).until(() -> debouncer.calculate(Math.abs(e.getEncoderSpeed()) < 0.01)),
            new InstantCommand(() -> e.setPosition(0)),
            makeSetPositionCommand(e, 0)
        );
    }

Then you could create a function to call these in parallel, and call that at the start of every auto

   new ParallelCommandGroup(
            RobotContainer.resetArmEncoderCommand(arm),
            RobotContainer.resetElevatorEncoderCommand(elevator)
        );

We can do them in parallel because we know that at the start of auto they are basically in the right position already, so we don't have to move the elevator down first, and it'll save time.

[alicelin27]

okey