Implementing CTRE's MotionMagic_AuxStraightPigeon with Falcon 500s

Heinz57 · January 26, 2020, 9:44pm

We are trying to get the MotionMagic_AuxStraightPigeon on CrosstheRoadElec/Phoenix-Examples-Languages to work with Falcon 500 (https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/tree/master/Java/MotionMagic_AuxStraightPigeon). Prior to the firmware update the motion magic was diving to set encoder points, but not straight. Changed FeedbackDevice.QuadEncoder -> FeedbackDevice.IntegratedSensor, FeedbackDevice.CTRE_MagEncoder_Relative -> FeedbackDevice.IntegratedSensor, kSensorUnitsPerRotation from 4096 ->2048. Installed latest firmware 20.1.0.0 which address issue with reading remote sensors (per https://phoenix-documentation.readthedocs.io/en/latest/ch21_Errata.html#remote-sensors-not-working-with-talon-fx), but not working - drives indefinitely which saw in past when sensors out of phase. Thought was issue with inverting motor and .setSensorPhase not working, but from Question on Falcon 500 Sensor Phase understand the sensor is guaranteed to be in phase. Any suggestions on what else needs to be updated for TalonFX would be greatly appreciated.

TytanRock · January 27, 2020, 1:52am

Take a self test snapshot of the master talon while it’s misbehaving and post it here. It gives detailed information that’s crucial to understanding what’s going on, and should be done whenever a ctr product is misbehaving.

otherguy · January 28, 2020, 12:40am

We were working through what seems to be a similar issue to your tonight.

The changes we’ve made.

Revise the code which zeros the sensors

 _left.getSensorCollection.setIntegratedSensorPosition(0, timeout)
 _right.getSensorCollection.setIntegratedSensorPosition(0, timeout)

Removed:

_leftMaster.setSensorPhase(true);
_rightMaster.setSensorPhase(true);

Changed motor controller class to TalonFX, if you don’t do this I think the lines for setting sensor phase will actually have an impact. Commanding the motors will still result in motion AFAIK

Heinz57 · January 28, 2020, 4:20am

MotorTest.txt (3.2 KB)

Here are two dumps of the Falcon motors. The one labeled BAD is almost exactly the sample program with the changes described above. The “Good” listing is the same program with the _rightMaster.setInverted(true); changed to false. With this change, the motors reach the goal, but spin the robot in a circle. . .

otherguy · January 28, 2020, 4:35am

To follow up, I spoke too soon before. We thought we had something working, but not exactly.

Since I haven’t found a working example of F500 running motion magic with a Pigeon yet…
Here’s where we ended up:

gist.github.com

https://gist.github.com/jcorcoran/e56d985fd1ee290b075d13c9f9aa3595

Robot.java

/**
 * Phoenix Software License Agreement
 *
 * Copyright (C) Cross The Road Electronics.  All rights
 * reserved.
 * 
 * Cross The Road Electronics (CTRE) licenses to you the right to 
 * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and 
 * Phoenix Software API Libraries ONLY when in use with CTR Electronics hardware products
 * as well as the FRC roboRIO when in use in FRC Competition.

This file has been truncated. show original

We’re not experts on using motion magic ourselves, so if we’re doing something dumb, feel free to point it out… positing here in hopes that we help anyone else having the same problems, and get any issues in the libraries addressed if any of the problems observed below are correct and not a result of problems we introduced somehow.

Basic takeaway.

_rightMaster.configSelectedFeedbackCoefficient( 1, ...
This method doesn’t appear to actually affect the setpoint correctly. We tried 0.5, 0.25, 1.0 all ended up having the wheel rotate 1/2 of the target destination position. (because you’re adding the distance traveled on the left and right)
setting sensor phase does appear to affect convergence/divergence of the position and error of PID0; even with the latest TalonFX firmware 20.1).
```
_leftMaster.setInverted(false);
_leftMaster.setSensorPhase(false);
_rightMaster.setInverted(true);
_rightMaster.setSensorPhase(true);
```
To travel fwd, one gear box on our skid-steer robot is inverted. It’s reported position decrements while the motors spin fwd… so when it’s added to the other side. Their positions cancel out. This is likely why the OP’s seeing commands that take a long long time to stop. They will basically run forever until there’s enough accumulated travel error due to performance deltas on the left/right side of the DT. Reaching the destination position is likely not going to be observable unless the robot is on blocks.
To resolve this we used the sensordifference feedback device type to negate the negative encoder accumulation:
```
/* Setup Sum signal to be used for Distance */
_rightMaster.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.RemoteSensor0, Constants.kTimeoutMs);				// Feedback Device of Remote Talon
_rightMaster.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.IntegratedSensor, Constants.kTimeoutMs);	// Quadrature Encoder of current Talon

/* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */
_rightMaster.configSelectedFeedbackSensor(	FeedbackDevice.SensorDifference, 
											Constants.PID_PRIMARY,
											Constants.kTimeoutMs);
```
So right + (-left) becomes: right - (-left)

@ozrien feel free to tell me we’re doing something stupid. Not an expert on CTRE libraries.

TytanRock · January 28, 2020, 4:38pm

So based off this comment and the self tests, I suspect you have the wrong aux pid polarity set. The major reason why I suspect this is that the SensorSum in BAD test shows a velocity of slightly positive (53), while the integrated sensor is very negative (-750). This means the remote sensor is very positive (803), and if the robot moves forward, this indicates there is an incorrect phasing (which is not possible on TalonFX’s) or incorrect pid polarity. See here in the example for a quick explanation to aux pid:

github.com

CrossTheRoadElec/Phoenix-Examples-Languages/blob/ad8d852c7e448201a23f534663bf9f421ce33571/Java/MotionMagic_AuxStraightPigeon/src/main/java/frc/robot/Robot.java#L220-L225


		/**
		 * configAuxPIDPolarity(boolean invert, int timeoutMs)
		 * false means talon's local output is PID0 + PID1, and other side Talon is PID0 - PID1
		 * true means talon's local output is PID0 - PID1, and other side Talon is PID0 + PID1
		 */
		_rightMaster.configAuxPIDPolarity(false, Constants.kTimeoutMs);

Next steps

Before trying anything different in code, I recommend doing the bring up procedure on the documentation: https://phoenix-documentation.readthedocs.io/en/latest/ch13_MC.html

Specifically, ensure that when you apply a positive duty cycle to each of the motors individually (the easiest way to do this is most likely using Tuner), each Talon blinks green, and the wheels move forward, with a positive velocity reported in self test.

Only after you verified each of the controllers are configured correctly would I recommend trying the example again.

Heinz57 · January 29, 2020, 7:08pm

We tried changing the polarity as you suggested and it did not solve the issue and we have previously gone through the “Bring Up” procedures on the individual motors.

On another note, I am being told that CTR Electronics. is working on an updated example project for the MotionMagic_AuxStraightPigeon example by maccopacco. Looking forward to seeing this!

otherguy · February 2, 2020, 12:44am

Did you try the example code I provided?
It works…

system · February 1, 2021, 12:54am

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