ArmTeleop() will immediately override any other commands that set a goal

[brettle]

ArmTeleop.execute() says:

RobotCode2024/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java

Lines 49 to 58 in 0dd661b

	double speeds = getRequestedSpeeds();
	double currTime = Timer.getFPGATimestamp();
	double deltaT = currTime - lastTime;
	double goalArmRad = goalState.position + speeds * deltaT;
	goalArmRad = MathUtil.clamp(goalArmRad, UPPER_ANGLE_LIMIT_RAD, LOWER_ANGLE_LIMIT_RAD);
	goalArmRad = MathUtil.clamp(goalArmRad, armSubsystem.getArmPos() + ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD,
	armSubsystem.getArmPos() - ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
	goalState = new TrapezoidProfile.State(goalArmRad, 0);
	armSubsystem.setArmTarget(goalState.position);

Because ArmTeleop is the default command, it will run whenever there is no other command scheduled that requires Arm. None of the arm's target-setting InstantCommands in RobotContainer require Arm and they all finish immediately anyway. That means that as soon as they finish ArmTeleop will start again and the above code will run. Note that it will set a new goal that is clamped to be close to the arm's current position. As a result, the goal set by the instant command will be overridden.

Suggested fix:

If speeds is 0 (ie. the joystick value is within Constants.OI.JOY_THRESH of 0) that means the user has not requested to move the arm, so return immediately (so the goal will not be modified). Otherwise, the rest of the method should run as written to enforce the user's most recent desired which should override the last goal set by an instant command.

[BrandonS09]

Fixed in ec3ea78

[brettle]

Please revert this change. It is not what you want to do.

[brettle]

The deadzoned axis thing you added isn't necessary because Arm.Teleop.getSpeeds() already does it.

More importantly, the original issue is still not fixed. You need execute() to return immediately (before setting a new goal) if the speed that getSpeeds() returns is 0.

[BrandonS09]

would the changes made in 863565c fix it?

[brettle]

Getting much closer. Unfortunately, now deltaT will end up being very large if the user tries to move the arm after not moving it for a while (a common occurrence). You can fix that by moving where lastTime is set. Two other requests:

1. Group the stuff related to calculating deltaT separately from the stuff related to getting and using requested speed. It will make the code easier to follow and maintain.
2. Add a comment explaining why you are returning early, possibly also referencing this issue, so that it will be clear to others.

[brettle]

While it's now "good enough" to fix the issue, I still recommend separating the speed stuff from the deltaT stuff. I also recommend explaining why you want to return immediately in this case. The code itself already effectively says "if no input, return", so that part of the comment doesn't add anything. Comments should explain why the code is the way it is, not explain what the code does. And the reference to this issue is not a substitute for a brief explanation in the comment. This issue just provides context and additional detail if the reader doesn't understand that explanation.

Also, the reference to this issue should be an URL in case the code gets copied to another repo or forked.