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
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
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:
setPos just set a member variable and not set motors directly.
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
- Provide an
isAtTargetHeight method to be called from
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?