PID inconsistent on climb

[Smallguy89]

We (Team 4829) are trying to automate our climb for the upcoming offseason events. However, our PID is slightly inconsistent. On the first run after the deploy of the code, the PID error is way more than it should be. After that, the error of the PID controller is consistent.

Raw data:
1st run after deploy:
Height of arm: 1.6m
Desired height: 1.2m
PID error from controller: -1.6m → this error continues to be off by a scale (best guess is 4-5 times what it should be)

2nd run (and every run after) after deploy:
Height of arm: 1.6m
Desired height of arm: 1.2m
PID error from controller: -0.4 → this error stays consistent with the setpoint - current_pos

Code:
Climb subsytem: https://github.com/TitaniumTigers4829/zuntue2022/blob/main/src/main/java/frc/robot/subsystems/ClimbSubsystem.java

Command: https://github.com/TitaniumTigers4829/zuntue2022/blob/main/src/main/java/frc/robot/commands/climb/ClimbSetPos.java

[bovlb]

Nice use of comments. I wish I could get our students to do that.

Can you tell us more about the encoders you’re using, and what units they use? In particular, how did you come up with the values for kClimbLeftMinHeightEncoderEstimate and kClimbRightMinHeightEncoderEstimate?

I don’t see that you’re resetting encoder values anywhere. The code in getLeftHookHeight appears to assume that the encoder will read zero when the arms are at maximum height. How do you ensure that? Suppose it instead got implicitly reset to zero when the robot was powered on, would that be consistent with the behaviour you’re seeing?

My suggestion would be that, in the subsystem periodic , you reset the encoder to zero whenever the limit switch is on, and you adjust the calculation in getLeftHookHeight accordingly.

Probably not causing your problem, but a few more things I would change, primarily about moving logic out of the command and into the subsystem:

• Have setPos just set a member variable and not set motors directly.
• Have setPos also check heights against max and min.
• Have the subsystem periodic do the PID calculation and motor settings.
• Also have periodic test the limit switches and prevent downward power.
• Provide a stop method on the subsystem to set motors to zero to be called from Command.end.
• Provide an isAtTargetHeight method to be called from Command.isFinished.

Also, have you tried graphing the target height, current height, and error, just to make sure they’re doing what you think they’re doing?

[Smallguy89]

Hello. Thank you for your quick response. To test (maybe a bad idea) we made a separate robot project to test the code to eliminate things that might be causing it to go wrong. The same thing happens in both projects.
In the separate project we have buttons to reset the encoder pos (at the top) and a button to set the position to half height.
The values of the estimates were gotten from resetting the encoders at the top, and running them down.

[bovlb]

Can you post a graph showing target height, current height, and error?

[Smallguy89]

https://drive.google.com/file/d/1k1HIA6wD7jHBDiYHlF75RcsZUDxEUlef/view?usp=drivesdk

[bovlb]

Thanks! This is nice, but it’s still a little hard for me to tell what heights are being set here, and which command they’re using.

Because you’re using a profiled PID controller, you’re setting a goal, and then the setpoint is moving towards that goal, subject to constraints. This means that your feedforward calculate should be using getSetpoint instead of getGoal. (See Combining Motion Profiling and PID Control with ProfiledPIDController.) Also, note that getPositionError is based on the setpoint, not the goal, so you’d expect it to be nearer zero than the goal error. It would be helpful to add both the setpoint and goal on the same graph so we can see more clearly what is going on.

One other thing I notice is that, in ClimbCommand, you’re calling ClimbSetPos with ClimbConstants.kSecondBarPos, which is set to 471289, which I think would get you past the International Space Station.

[Smallguy89]

Thanks! We will try this

[Smallguy89]

Found the solution about a month and a half ago, here it is:
Use your own PID:

private double getMotorOutput(ProfiledPIDController controller, SimpleMotorFeedforward ff, double desired, double current) {
   controller.calculate(current, desired);
   return ((desired - current) * controller.getP()) + ff.calculate(controller.getGoal().velocity);
 }

[Glitch_3360]

As head of robot design for my team who doesn’t have a sufficient programming team nowadays (all new people) i strongly suggest you try out more option to automate your climb like using more mechanical component instead of software. My team did this in 2022 where we had limit switches on our hooks that sent a reliable enough signal when climbing to automate it. Hit me up if you wanna know more about it.

[Oblarg]

This solution doesn’t make much sense to me. Why go through the overhead of constructing ProfiledPIDController if you’re not going to use the profile or the PID? It’s just a wasteful double at that point.

[Ashray.g]

We’ve had good results using motion magic on the falcons