MotionMagic PID stops after the first call

Technical
1

I am trying to set a command that will run MotionMagic on Falcons.

The command looks like this -

leftDriveTalonFX.set(ControlMode.MotionMagic, leftEncoderVal);

where the leftEncoderVal is calculated target value.

I thought that once I run it in the initialize(), it will run until the isFinished() will return TRUE.

In my case, however, that does not happen. When I put the same command in execute(), it runs, but it seems to “jerk” the motor - starts, stops, starts again really fast. In other words, it seems the motor stops turning before the next scheduler cycle.

I realize that the PID on TalonFX is 1ms, while the scheduler loop on RIO is 20ms. I thought all my timeouts are set to 30ms in the talon configuration.

Any suggestion what parameter should I look at?

I would like to do something very simple:

  1. Zero the encoder
  2. Set the target
  3. End the command when the target is within acceptable error

Ideally I can do it with left and right encoders at the same time, and hopefully the robot will drive straight.

Note that the brake mode is set for the Falcons.

Any suggestions will be much appreciated. If needed, I will post the parameters I set for the motionmagic configuration.

Thank you.

2

Are you ensuring that the encoders are zero and target is set before isFinished can return true? Since you have to send a CAN command for both of those and wait for a updated CAN message with the result, you may be finishing immediately before they take effect.

2 Likes
3

Thank you for the reply.

I am positive the command is not finished immediately. I print a message during EXECUTE and also a message at END as well a boolean INTERRUPTED value.
I am thinking now that maybe some MotionMagic parameters are not done right.
I also print the PID error from the talons and it’s definitely not close to 0

I thought that may be default command (which is manual drive) interrupts it somehow, and added a message print there. But it does not - when this command is running, no messages from the default drive.

I also found that safety should be OFF on the motors. So, I turned it OFF as well, but got the same result.
To run off safety, I am only doing

.setSafetyEnabled(false)

I have read somewhere that there are two safeties - on the Falcon/Talon and RIO. is that correct? If so, how do I turn both of them OFF?

4

So far it looks like that something is telling the motors to stop, and it may not be on the RoboRIO side as far as I could determine so far.

5

Can you post a link to your code?

Sounds to me like you’ve got objects fighting for control of the falcons.

6

I have the same exact feeling. Either an object fights for the motor, or Falcon stops it for some reason.
The link to main repo branch where I am trying all this is:

MotionMagic definition starts here:

Zero Encoder is here:

Command that tries to go to position based on the encoder values is here:

simpleMotionMagic that actually drives is here:

As you see, we use common code base for all robots we test with. The MotionMagic is tested on c2020 and DEMOBOARD with the same result. DEMOBOARD has two falcons that correspond to left and right side of the drivetrain. It’s tank drive.

7

Which of the commands are you trying to run?

8

AutonomousDriveLinear

The button binding for the run is:

9

I can say with some confidence its your Differential Drive not getting updated enough, causing it to disable the drive motors and putting them back to percent output.
If I add the following to your code immediately following the differential drive constructor I can simulate your autonomous routine and see output rising as expected. Without it I see the motor blip forward and later stay at 0.
drive.setSafetyEnabled(false);

This was tested using simulation, so while I expect it would also be correct on a real robot it isn’t on a real robot, and could be different.

10

That binding is broken, assuming that you’re not holding the button down as you go.

I think first things first, change it to:

    new JoystickButton(drivestick, 11).whenPressed(new AutonomousDriveLinear(2).andThen(new DriveStopCommand());

The andThen convenience feature will run the subsequent command after the first one finishes, but you can also just use a DriveTrain lambda function to stop your motors (set them to 0, with PercentOutput).

That would change it to something like:

    new JoystickButton(drivestick, 11).whenPressed(new AutonomousDriveLinear(2).andThen(()-> drivetrain.stopMotors());

Or something like that.

Alternatively, don’t do the andThen() at all, and just stop your motors in your end() method. That is actually what we do.

With regard to that command, your current push doesn’t actually have that command end, so if you press the button and hold it, theoretically it would run forever or until you trigger interruption by scheduling something else requiring the drivetrain.

11

Thanks.

The reason for “WhenReleased” a safety feature. To make sure I can stop the whole thing by simply releasing a button.

I commented out the end condition to make sure it does not get interrupted by that. But it will be interrupted by a button release

Also I set safety to false at the end of the constuctor - same result

12

Are you watching the scheduler widget in Shuffleboard or Glass while you are running this?

You can view your drivetrain instantiation to see what it has scheduled.

13

I do not watch the scheduler, but I can do so. Does it keep a log of the scheduled items? The item stops way too fast.

It seems so far that the PID stops by the Falcon rather than the Rio scheduler. It’s just too fast and the PID errors are not updating that much. It seems it’s just running 1ms loop and then stops. But why - I cannot figure this one out.

14

You can plot the closed-loop error for the duration of the run as well.

15

And I finally figured it out (have a lot less hair after this) -

So, what kills MotionMagic is this -

drive = new DifferentialDrive(leftDriveTalonFX[0], rightDriveTalonFX[0]);

The reason why it kills it - Motor Safety. I was getting

ERROR 1 DifferentialDrive output not updated often enough

Now, the CTR documentation says - disable safety if you plan to use MotionMagic and related CTR-specific items. And so I did things like this -

rightDriveTalonFX[motor].setSafetyEnabled(false);

BUT, the problem is not with the controller object, but the actual DriveSubsystem object. And of course, once I added

drive.setSafetyEnabled(false);

things started working like they should “automagically”.

I hope that will help others. I saw the “DifferentialDrive output not updated often enough” message was discussed before on this forum without final resolution. So, hopefully my research and solution will help other teams and will put this issue to bed.

16

Glad you got it worked out. Just as a side note.

If you were to move your MotionMagic set calls to the execute method of your command, and then in your motion magic public methods being called in your drivetrain add m_your_differential_drive_object.feed(), you won’t get the output not updated enough overrun.

17

Good point. I may try that.

18

I think that the CTRE firmware takes care of the fact that you are calling set with the same setpoint. It doesn’t even send the CAN message, so you don’t need to worry about saturating the bus.

There’s no performance penalty to calling every scheduler loop, that way you can keep the safety drive in there if it will help your robot not hurt someone or break something else.

19

That one I did try originally, and interestingly enough, it lead to “jerking” - the motor still was stopped by safety and started again in the next loop. That lead to VERY uneven trajectory. One possibility - I overrun my 20ms loop and safety stops it during that time.

20

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.