Struggling with SimpleDifferentialMechanism in Phoenix6

Coding mentor who is struggling with some of the finer points of PID and control systems, as opposed to just code.

We are trying to replicate our MotionMagic “DriveStraight” code from prior years using the new Phoenix6 APIs and Kraken/TalonFX motors/controllers.

We don’t yet have our Pigeon IMU, so I am trying to figure out how to do this only using the integrated encoders and the SimpleDifferentialMechanism class, which seems to be just what we need. The problem is that I am not sure I understand the usage exactly and we are getting no response when we try to call setControl…

Using a simple MotionMagic PID on just one TalonFX, I can get it to “Drive” to a distance, so I think the general MM PID is okay. I am just not sure how to set it up for Differential.

Sample config code for PID:

        TalonFXConfiguration myConfig = m_rightConfig;

		/* FPID for Distance */
        Slot0Configs slot0 = myConfig.Slot0;
        slot0.kS = 0.0;
        slot0.kV = RobotMap.DrivetrainConstants.DISTANCE_GAINS.kV; // 0.1
	slot0.kP = RobotMap.DrivetrainConstants.DISTANCE_GAINS.kP; // 50.0
	slot0.kI = RobotMap.DrivetrainConstants.DISTANCE_GAINS.kI; // 0.0
	slot0.kD = RobotMap.DrivetrainConstants.DISTANCE_GAINS.kD; // 0.0


        MotionMagicConfigs mm = myConfig.MotionMagic;
        mm.MotionMagicCruiseVelocity = 3.5; 
        mm.MotionMagicAcceleration = 5.0;
        mm.MotionMagicJerk = 50.0;

        FeedbackConfigs fdb = myConfig.Feedback;
        fdb.SensorToMechanismRatio = 9.82;

        DifferentialSensorsConfigs sens = myConfig.DifferentialSensors;
        sens.withDifferentialSensorSource(DifferentialSensorSourceValue.RemoteTalonFX_Diff);
        sens.withDifferentialTalonFXSensorID(m_leftLeader.getDeviceID());
        m_rightLeader.getConfigurator().apply(myConfig);

We set up a similar config for the other side (using the first side’s CAN ID for the DifferentialTalonFXSensor).

The call to try to make the drivetrain drive straight is:

        m_diffMM = new DifferentialMotionMagicDutyCycle(5.0, 0.0);
        m_diffMechanism.setControl(m_diffMM);

This doesn’t work. I guessed that it was because we didn’t have the PID for the differential defined, so we configured Slot1 with the same gains as a test.

We get no response at all when we try to make the mechanism go a couple rotations, whereas with just the target and the MotionMagic on one side it does follow the motion profile.

I am sure we’re missing something simple/dumb, but I am completely failing at finding it. Any help is appreciated. I don’t have the entire robot code public right now, but I can share bits as needed. We still use TimedRobot, so our code is relatively simplistic.

UPDATE: We added a bunch of diagnostic calls and it appears that our “Add” TalonFX is somehow losing it’s setting for the “Differential Sensor” which is the TalonFX on the other side of the bot. I can set it in Phoenix tuner and see it gets cleared for some reason. Our code does set it, but only on init of the drivetrain, so I think we’re getting an error somewhere and it is clearing it. Still puzzled why we get no output at all – not even a single cycle.

Given the response, I am guessing this won’t garner any new responses, but we’ve tried a number of troubleshooting efforts and even removed the SimpleDifferentialMechanism completely. Any time we set the DifferentialSensor to the remote Talon, we get a Fault that it can’t be seen even though in Phoenix Tuner X it appears to be fine and watching its position through Tuner X we can see it change with manual changes. Any time we try to do something via the DifferentialSensor construct we get the Fault_MissingDifferentialFX signaling. We do not understand why we’re getting this.

We have checked all wires and tried reversing sides and we get this error no matter what. We have figured out that this is a sticky fault and have cleared that, but it comes back every time we try to use the DifferentialSensor using the remote TalonFX.

Any help from someone who knows anything about this would be most appreciated.

I apologize that we do not have documentation and examples set up yet; there hasn’t been a lot of usage of the differential mechanisms or any feedback on them.

The intended usage of SimpleDifferentialMechanism is that we do all the DifferentialSensorsConfigs for you, and you’re responsible for everything else. A SimpleDifferentialMechanism runs a control mode on the “average” axis, which essentially controls forward/backward movement, and a position closed-loop on the “differential” axis, driving the difference between the two motors to a fixed value.

When you construct a SimpleDifferentialMechanism, all devices must be on the same CAN bus. Additionally, make sure the motorDirectionsAlign parameter is correctly set. If the two sides should have the same invert, this should be true; otherwise, it should be false.

Typically, slot 0 is used for the average axis PID (such as Motion Magic or a velocity closed-loop), while slot 1 is used for the differential axis PID. Make sure the slot numbers are correctly set up in your control request.

After applying all configs to both motors except the DifferentialSensorsConfigs, call m_diffMechanism.applyConfigs() to apply the differential configs. We automatically set everything up so the correct leader/follower relationship is established.

I would recommend starting off with open-loop control mode such as DifferentialDutyCycle to just make sure that everything is set up correctly and to tune your differential axis PID. Then you can work on using closed-loop for the average axis.

We also have an optional periodic() method that can be called from your subsystem periodic() to automatically disable output for important fault conditions, such as the other differential device disconnecting from the CAN bus. Some faults, such as a device power-cycling, requires manual user action followed by a call to clearUserRequirement() to restore the system to a safe state, while others can be automatically recovered from. We have several APIs to get the current state of the mechanism and the reason for a disable/required user action.

1 Like

First of all, thank you for your response. I understand how things can go when it comes to doc and sample code. Unfortunately, we’re still struggling, so hoping you can maybe help further.

After working with a past coder to get a sim working and using our Pigeon for Yaw instead of just encoders and getting it to work there, we’ve tried putting it back on our bot and we’re still getting the MissingDifferentialFX fault when we call SetControl on the SimpleDifferentialMechaism.

I’ve tried reviewing all the things you’ve pointed out and I believe we are constructing the object correctly and we’re trying to use the PID values for our position PID that seemed to work when just using one side of the drivetrain to move to a specified number of rotations. The turn PID is not tuned for this bot but is based on a prior incarnation of a similar method (using phoenix 5 and falcons/talonFX w/ pigeon)

If I manually clear the faults using Phoenix tuner (or whatever the new incarnation is called) and plot the MissingDifferentialFX fault and the sticky MissingDifferentialFX fault, they are 0. As soon as we call SetControl on the SimpleDifferentialMechanism both plots change to a value of 1. If we just use a WPI DifferentialDrive and arcade drive the drivetrain all of the motor controllers respond normally and in phoenix tuner we don’t have any issues with the CAN bus so I don’t believe we have a signalling issue with one controller not being able to signal another. I’m stumped.

Our code is public at frc5567/2024-Robot: Competition Robot code for 2024 (github.com)
Please look at the “SimpleDiff” branch for the current incarnation of our test code (literally using the “Test” mode to call the drivestraight method on the Drivetrain). Forgive any stylistic issues, please. The code is a work in progress and is student written and maintained (I’m just to blame for it working or not).

There’s a couple of things I notice that are not causing the issue but would be a problem when this does start working:

  1. Make sure you use DifferentialMotionMagicVoltage and not DifferentialMotionMagicDutyCycle, otherwise those drive gains are way too aggressive.
  2. You must configure the regular FeedbackConfigs object on both differential motors (right and left leaders).

Testing your code in simulation, after fixing the two points mentioned above, everything seems to work as expected. However, after doing a quick code review of firmware, I did find an issue that could cause the MissingDifferentialFX fault you are seeing. I do not have an expected ETA of when we will release firmware with the fix (since I’m at university right now), but hopefully it will be picked up in the next release.

1 Like

You’re right about the Voltage v DutyCycle – that was a mistake when I translated from the sim back to our bot code. We had been using Voltage in our PID tests, but on the sim DutyCycle was used. Sorry about that.

Good to know about the Feedback configs – that will be valuable going forward.

Finally, I am glad to hear (at least in this regard) that I’m not just crazy and there is something funky in the firmware. That was the conclusion I had kind of arrived at, but we couldn’t figure out how to find the code to verify further. For the time being we are doing a simplified position PID (just do it on both sides of the drivetrain with no feedback between) to accomplish what we need for the first comp. We’ll revisit the Differential mechanism once a fix for the firmware has been released.

Again, thank you very much for your responses and your time. Overall, we are liking Phoenix 6 a LOT for the way it has simplified many things and made some of the constructs more consistent. Kudos to the team overall.

1 Like

Is there any GitHub issue or other way for me to track when this is addressed? I see an updated firmware as of this week, but right now we’re still getting the fault. It is unclear to me whether that is due to our mistake(s) or if the issue you seemed to identify is still present.