How to tell if Motion Magic profile has reached target

Basically the title. I’m using Motion Magic for an arm, and I want to know when Motion Magic has reached the target position. In the WPILib PIDSubsystem, there was a convenient method ‘onTarget()’ that did this, but I’m not sure of the best way to achieve similar functionality for Motion Magic.

The way that I have typically done this in the past is checking if the position of the mechanism is within a preset range of the setpoint. If the position was in range, the profile is considered complete. Below is the implementation of this from our elevator this year.

Math.abs(mPos.getPos() - mElevator.getPos()) < mConst.el_allowableError

I don’t know if this is the best/intended way to go about this but it has worked reliably for my team.

1 Like

We did something similar to the post above but also added a check to make sure the velocity was under a certain limit. If the P gain is too high for some reason, the mechanism will overshoot the setpoint and the method used above will return true when the mechanism is sailing past the desired goal.

The code was something like this:
Math.abs(Elevator.currentPosition() - desiredPosition) < kPositionTolerance && Math.abs(Elevator.currentVelocity()) < kVelocityTolerance

2 Likes

Hmm, do you know if ‘isMotionProfileFinished()’ works?

I can’t find a definitive answer in the Phoenix documentation, but from what I recall the isMotionProfileFinsihed() call only refers to the MotionProfile ControlMode on the Talon, not motion magic.

https://phoenix-documentation.readthedocs.io/en/latest/ch16_ClosedLoop.html#check-ismotionprofilefinished

If it only refers to the motion profile mode (probably easiest to just manually test and see if it does), and you need to implement your own solution, I’d chose @Prateek_M’s. On my previous team and in my own projects, I always wrote the check manually, but had never considered using velocity as well (to which I’ll start doing from here on out).

1 Like

this is what we do.

1 Like