Arm state machine

[jay-j]

Adds a trapezoidal profiler to the arm.

Move arm-related joystick mappings to the end of the file, change to purposeful style ("purpose" functions called throughout the code, connection between purpose and specific hardware buttons only happens in one place)

[ToothbrushB]

Implemented changes in this pull request into master branch under EnhancedTeleOp.java. I chose not to just merge the pull request mainly because I wanted new changes in a different file just in case we needed to have easy access (not dig through commits to find it).

Change list:

• Cleaned up TrapezoidalProfiler.java (had issues with implicit casting booleans as numbers)
• Added velocity control for drive train with quadratic curve gamepad control
• Decided not to use an arm state just to handle duck mode
  • Instead made enum with states for all manipulator heights and detect transition to duck mode to decide when to move arm to duck mode height (is this a better or worse way to do it)
  • Reasoning: If statements are hard to read, didn't seem right to make a state machine just for duck mode, so I generalized it to all positions and instead detect transitions
    • By feeding setTargetPosition() a trapezoidal trajectory, we essentially put a controller on top of the pre-existing PIDF controller on the hubs. Why would this be necessary? If the PIDF is tuned properly and is being used, I don't think that you would need this; it should already be good enough to do the job on its own. If we don't tune/use the PIDF, I can see how this would be useful.
    • Ported smooth_delay to Java
    • Placed utilities in org.firstinspires.ftc.teamcode.util

[jay-j]

PID(F) and the (trapezoidal) profiler serve very different functions.

An electric motor with no feedback controller (e.g. just using voltage, pwm, or the basic version of this library's setPower()) has coupled speed and torque.. short story you're not really in control of the motor. By using a PID controller, you can use the motor as if torque and speed are independent; you set the speed, and the motor will use however much torque it needs to to maintain/achieve the speed you commanded (within various limits; maximum voltages, maximum currents, etc).

But PID controllers don't respond well to big step inputs. For example if the arm is at the bottom and you give it a new command to go straight to the top. The PID controller will see an instant huge increase in error and respond with an instant huge increase in motor effort. Depending upon the system and gains this can cause a variety of undesired behaviors (e.g. accelerating arm too fast makes the robot tippy).

But you clam that the second, poorly documented mode of setPower() should handle this? Based on the arm bobbing as it is descending, I think setPower() is not working as we'd like. The arm can go up fast, so clearly the gearmotor has plenty of torque for the application. But shouldn't it then have tight tracking and a smooth descent? Not if setPower() is limiting the voltage available to the PID controller - not what we want.

So we need a different technique for turning big, relatively infrequent, jumps in desired position into a smooth trajectory of points to feed to the PID controller. That's what the trap profiler is for.

[jay-j]

Using the STOP_AND_RESET_ENCOER mode on every limit switch falling edge may result in undesired behavior. (and a race condition? if arm lifts off the switch, falling edge is detected, motor power is cut, arm falls, then motor re enables and tries to lift again...). We may instead need a way to "zero" the encoder without stopping. For example just keeping track of some offset ourselves, let the falling edge detection just update a variable.

[ToothbrushB]

For the switches themselves, FALSE (in code) means the switch is engaged and TRUE means the switch is disengaged (active low). So when the arm first hits the switch, the motor power is cut (still an undesired effect) and the encoder is reset. The switch should still be depressed at this point, so there will not be another transition/edge to detect and the encoder will only be reset once. When power eventually is resumed (hopefully quickly), the switch will disengage when the arm lifts. This transition will not trigger a reset. I agree that it is confusing to have FALSE mean the switch is on, and I debated having immediately NOT-ing it at the beginning when first getting the state. I eventually decided against it as it might make things more confusing when documentation says FALSE is on while my code would say TRUE is on.

I also agree that it doesn't make sense to reset every time that the limit switch is pressed, especially if motor power is cut. There seems to be a way in the SDK just to send a command to reset the encoder, but that means sending commands to the hubs ourselves (not through the SDK) which might cause conflicts.

On Thu, Jan 27, 2022 at 12:58 PM Jay J @.***> wrote:

Using the STOP_AND_RESET_ENCOER mode on every limit switch falling edge may result in undesired behavior. (and a race condition? if arm lifts off the switch, falling edge is detected, motor power is cut, arm falls, then motor re enables and tries to lift again...). We may instead need a way to "zero" the encoder without stopping. For example just keeping track of some offset ourselves, let the falling edge detection just update a variable.

— Reply to this email directly, view it on GitHub https://github.com/ToothbrushB/FtcRobotController/pull/7#issuecomment-1023543606, or unsubscribe https://github.com/notifications/unsubscribe-auth/ALYXSATBEOK37EMKQDJ367TUYGIVNANCNFSM5M3ROBLQ . You are receiving this because you commented.Message ID: @.***>