MotionMagic - Hardware PID turning

Technical Technical Discussion
#1

We successfully use MotionMagic PID for linear driving and Software PID via WPILIB for turning in the regular WestCoast drive chassis.

We’re trying to figure out how to use Pigeon IMU with Falcons for hardware PID turning. In other words, the task is - use hardware PID on Falcon to turn, say, 45 degrees. Note that software PID turn is working, so that’s not the point. Right now we experiment with just one TalonFX. The chassis has two, so once we get things going with one, we will apply similar logic to the other one to make in-place turn.

We’re trying to configure PID using Pigeon on slot #1 of the TalonFX.

Here is the code we have:

The Pigeon is connected to TalonSRX, and is defined like this -

  WPI_TalonSRX pigeonIMUController = new WPI_TalonSRX(4);
  private WPI_TalonSRX pigeyTalonSRX;
  private PigeonIMU bird = new PigeonIMU(pigeonIMUController);

There is a public getBird() method that exposes it to other subsystems.

The Talon in question is defined like this:

WPI_TalonFX rightmotor = new WPI_TalonFX(10);

The PID configuration for it is defined like this:

rightConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.Pigeon_Yaw;
rightConfig.remoteFilter0.remoteSensorDeviceID = 
  (RobotContainer.pigeonIMUSubsystem.getBird()).getDeviceID();

System.out.println((RobotContainer.pigeonIMUSubsystem.getBird()).getDeviceID());

rightConfig.primaryPID.selectedFeedbackSensor = 
TalonFXFeedbackDevice.RemoteSensor0.toFeedbackDevice();

rightConfig.primaryPID.selectedFeedbackCoefficient = 3600/8192;

rightmotor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 20, 30);
rightmotor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 20, 30);
rightmotor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, 20, 30);

(RobotContainer.pigeonIMUSubsystem.getBird()).setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR , 5, 30);

rightConfig.neutralDeadband = 0.001;
leftConfig.peakOutputForward = +0.3;
leftConfig.peakOutputReverse = -0.3;
rightConfig.peakOutputForward = +0.3;
rightConfig.peakOutputReverse = -0.3;

rightConfig.slot1.kP = 0.75;
rightConfig.slot1.kI = 0;
rightConfig.slot1.kD = 0;
rightConfig.slot1.kF = 0.0;
rightConfig.slot1.integralZone = 200;
rightConfig.slot1.closedLoopPeakOutput = 0.3;
rightConfig.slot1.allowableClosedloopError = 0;

int closedLoopTimeMs = 1;
rightConfig.slot1.closedLoopPeriod = closedLoopTimeMs;

rightmotor.configAllSettings(rightConfig);

Finally, to actually activate the PID, I believe I need to do this:

rightmotor.selectProfileSlot(1, 0);
rightmotor.set(TalonFXControlMode.MotionMagic, 45);

and … nothing happened. The robot did not move. Note that Pigeon values are displayed correctly on SmartDashboard. We zero the Pigeon Yaw before activating the PID.

Now, I also saw the TalonFXControlMode.Position, and I was not sure if I need to use that one instead.

Note that similar logic works just fine for linear driving when a local encoder sensor is used instead of a remote Pigeon. If you have any suggestions, they would be much appreciated.

Thank you.

1 Like
#2

I’m not sure if this is the only issue, but 3600/8192 does integer division here and results in a feedback coefficient of 0, which is probably not what you want. You can replace it with 3600.0/8192.0 to get the expected fractional coefficient.

1 Like
#3

We will try it! Anything else?
By the way, what does this coefficient do exactly? Not sure we got a good answer to that from the documentation.

#4

The coefficient is just multiplied by the sensor value. See the docs:

Scalar (0,1] to multiply selected sensor value before using.
Note this will reduce resolution of the closed-loop.

As noted in the docs, when a pigeon is the remote sensor it uses native units. 360-degrees is 8192 units. If you want to work with degrees you can use the coefficient, or do the math in your code. But, it seems to me you’d want 360.0/8192.0, not 3600.0/8192.0, but I may be missing something.

1 Like
#5

I think this needs to be RemoteSensorSource.GadgeteerPigeon_Yaw. The docs say Pigeon_Yaw is for a pigeon connected to CAN.

#6

Awesome! Thank you very much. We will try all of that on Monday.

#7

Tried all that - same result (the robot is not moving). I also tried changing things to the SLOT0 without success.

So, the latest code looks like this -

leftmotor.setSafetyEnabled(false);
rightmotor.setSafetyEnabled(false);

rightConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.GadgeteerPigeon_Yaw;
rightConfig.remoteFilter0.remoteSensorDeviceID = (RobotContainer.pigeonIMUSubsystem.getBird()).getDeviceID();

System.out.println((RobotContainer.pigeonIMUSubsystem.getBird()).getDeviceID());

rightConfig.primaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.RemoteSensor0.toFeedbackDevice();

rightConfig.primaryPID.selectedFeedbackCoefficient = 360.0/8192.0;

rightmotor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 20, 30);
	rightmotor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 20, 30);
	rightmotor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, 20, 30);
	(RobotContainer.pigeonIMUSubsystem.getBird()).setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR , 5, 30);

rightConfig.neutralDeadband = 0.001;
	leftConfig.neutralDeadband = 0.001;

leftConfig.peakOutputForward = +0.3;
	leftConfig.peakOutputReverse = -0.3;
	rightConfig.peakOutputForward = +0.3;
	rightConfig.peakOutputReverse = -0.3;

rightConfig.slot0.kP = 0.75;
	rightConfig.slot0.kI = 0;
	rightConfig.slot0.kD = 0;
	rightConfig.slot0.kF = 0.0;
	rightConfig.slot0.integralZone = 200;
	rightConfig.slot0.closedLoopPeakOutput = 0.3;
	rightConfig.slot0.allowableClosedloopError = 0;

int closedLoopTimeMs = 1;
rightConfig.slot0.closedLoopPeriod = closedLoopTimeMs;
rightConfig.slot0.closedLoopPeriod = closedLoopTimeMs;

rightmotor.configAllSettings(rightConfig);

With the actual command to turn:

rightmotor.selectProfileSlot(0, 0);
rightmotor.set(TalonFXControlMode.MotionMagic, 45);

Any help will be much appreciated.

#8

I just noticed you’re using Motion Magic but you haven’t set a motionCruiseVelocity or motionAcceleration. That might be the problem since they default to 0.

You might want to try starting with Position control. Once you have that working you can switch to Motion Magic.

1 Like
#9

Hmm… Think you are commanding the primary MotionMagic control loop to 45 encoder counts. Think there is an auxilary control loop that can move to angle (or radians… not sure which). That’s the exact command we used last night to drive about 170,000 counts or about 10’ with our drive train.

#10

I don’t think so. The feedback coefficient is set up for degrees. It’s set to 360.0/8192.0, so a command of 45 is in degrees, and it is converted to 1024 encoder ticks pigeon native units.

#11

Another thought is to use Pheonix Tuner and confirm the sensor is configured properly. Move the mechanism manually and confirm your readings, as described here.

#12

OK… 1024 encoder counts is a half spin of a Falcon motor. Might see the robot twitch. Also, in this case, why would KF = 0? This just seems like an odd controller for a simple turn.

#13

Is the Primary MotionMagic control only for encoders?

My point was - I wanted to do a turn just by using the Pigeon. The encoder turns are very imprecise as they greatly depend on the surface quality (e.g. floor vs carpet). The Software PID works just fine. We want, however, to convert this into PID using Pigeon.
In fact, my understanding, for that we do not even need to define encoders as sensors.

So, we want to turn 45 degrees, not 45 encoder clicks

#14

Also if you use auxilary control loop - could you share the logic for turning on it?

Basically for initial testing I wanted to exclude encoders altogether from this turning

#15

Re: KF=0 -the idea is to make a prototype of the Kp-only robot (with zero Kd and Ki) and then play with the numbers or do proper characterization.

#16

That’s a great suggestion. For some reason I did not think using Phoenix Tuner to actually check configuration manually. Will do.

#17

In this case the sensor is a Pigeon, not encoders. It’s actually 1/4 spin of the device since the pigeon has 4092 units per rotation.

#18

Gotcha. But this whole implementation still seems way over complicated. If the point is to do a tank spin with Motion Magic, we just did it with the drive encoders. Essentially we spun the drive train to get an encoder count per degree kinda thing and just drove it with distance. The only IMU work we’ve done was from the CTRE github where heading is an auxilary controller, not the primary.

David

#19

We did something similar with our parade tank robot, it has a t-shirt shooting turret. A pigeon on the turret is connected to the gadgeteer port on a Talon SRX. In our case the motor is a mini CIM connected to a Talon SRX that turns the turret to an angle (with position control,) so it’s a little different than what you’re doing. It will hold the position while the robot drives underneath. Here’s a link to the code, in the hopes it will help.

#20

Just encoders wouldn’t work very well if you want to spin in place to a specific heading. It’s difficult to predict how the wheels will scrub. A common way to do it is in robot code with a PIDController or ProfiledPIDController. Here’s our turn-to-angle command from 2022.

I think @atsekhan said that’s what they’re doing now, but they want to try to move to closed-loop on the motor controller, which makes sense since it’s faster and offloads the work from the roboRIO.