TalonSRX and velocity control with a REV Through bore encoder

[seanw]

We’re using two talon SRX to drive two motors that have the REV through bore encoder attached. Using the encoders and doing a standard Encoder + PIDController from WPILib works only OK (even after looking at this PSA, so we wanted to try running the encoders and PIDs from the Talons.

Wired them up with a JTAG breakout board and I can see activity in Phoenix Tuner. With PID=0 we figured out an F that gets it pretty close. As soon as we add some P everything goes wild.

At P=0.01 it doesn’t look like the error ever gets smaller. At P=0.05 the motor goes to full speed. On values lower than that I see the motor barely turning, or it turns backwards (from the graphs, looks like it starts spinning the right way then quickly reverses).

The code is basically:

   WPI_TalonSRX _talon = new WPI_TalonSRX(port);
    _talon.configFactoryDefault(0);
    _talon.setNeutralMode(NeutralMode.Coast);
    _talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, Constants.kPIDLoopIdx, Constants.kTimeoutMs);

    _talon.setSensorPhase(false);

    /* Config the peak and nominal outputs */
    _talon.configNominalOutputForward(0, Constants.kTimeoutMs);
    _talon.configNominalOutputReverse(0, Constants.kTimeoutMs);
    _talon.configPeakOutputForward(1, Constants.kTimeoutMs);
    _talon.configPeakOutputReverse(-1, Constants.kTimeoutMs);

    /* Config the Velocity closed loop gains in slot0 */
    _talon.config_kF(Constants.kPIDLoopIdx, ShooterConstants.kF, Constants.kTimeoutMs);
    _talon.config_kP(Constants.kPIDLoopIdx, ShooterConstants.kP, Constants.kTimeoutMs);
    _talon.config_kI(Constants.kPIDLoopIdx, ShooterConstants.kI, Constants.kTimeoutMs);
    _talon.config_kD(Constants.kPIDLoopIdx, ShooterConstants.kD, Constants.kTimeoutMs);
    _talon.configClosedLoopPeriod(Constants.kPIDLoopIdx, Constants.kPIDLoopTimeMs, Constants.kTimeoutMs); // 1ms loop

and then to shoot:
_talon.set(ControlMode.Velocity, ticksPer100ms);

I’m lost as to what to look at next. I’ve been looking at the CTRE docs and was trying to do the tuning entirely in Phoenix Tuner to rule out code problems but I can’t get around that FRC lock even after uploading a dummy project and resetting everything.

I should also mention we’re using velocity control with the CTRE mag encoder on our drive train and it works beautifully. Other than the sensor phase and using quadrature instead of CTRE_Mag it’s the same code.

Any guidance would be appreciated, thanks!

Sean

[gdefender]

If the sensor phase and readings for position and velocity look correct in Pheonix Tuner, this should be right. We are using these encoders with Talon SRX controllers, but we’re using the CTRE Breakout Board.

Did you try any kP values between 0.01 and 0.05? It’s possible that your optimal P value is between these values, this is a 5X spread. If your kF is tuned pretty well, a small kP is expected.

[NewtonCrosby]

When you shoot a ball, what is your average error on it?

How we have been doing it is like this:

1. Find the percent output (phoenix tuner) that is useful for one shot to score. Use that as the starting point for kF calculation. For example, to shoot from the front of the trench we are spinning at 15000 ticks/100ms, at about 60% motor output
2. kF calculation is then (.6 * 1023) / 15000
3. Observe using phoenix tuner plot, the error encountered during a shot, ours typically has been about 20%
4. Calculate kP such that when 20% error is experienced, we react with 20 % motor increase (.2 * 1023) / 3000
5. Incrementally tinker with kF, kP, and set points until you get the magic combination for the given shot you want to take.

We actually tune all of this using Shuffleboard, so if you want the same feel free and adapt our code to your codebase:
Shooter Tuning Command

[Jacob_C]

but I can’t get around that FRC lock

The FRC lock is intentional, it’s not something you’re supposed to be able to remove in FRC. It means you have to use the Driver Station to enable any motor control, which is consistent with the enabling/safety philosophy in the entire control system.

So when using Tuner, use the Driver Station to enable robot control and then use Tuner’s controller enable to allow Tuner to override control of the selected motor controller.

[gdefender]

If you’ve narrowed the problem to PIDF tuning, you could also use the FRC Characterization tool, and SimpleMotorFeedforward. This worked really well for us. I wasn’t sure if you had ruled out a hardware/wiring issue yet.

[seanw]

Are the docs at https://phoenix-documentation.readthedocs.io/en/latest/ch13_MC.html current then or am I reading the wrong thing? The instructions there were guiding me in a direction to get frclock=0 and explicitly said not to be running DS. I’ll try it your way, thanks.

[seanw]

My initial testing was just trying to get the motors to spin at a given speed, no balls going through the shooters. Thanks for the code, I will look more closely at it.

[gdefender]

The instructions there were guiding me in a direction to get frclock=0 and explicitly said not to be running DS.

The docs say you have two options, FRC and non-FRC. You should use the FRC option and use Driver Station.

[seanw]

I tried a few like 0.2 and 0.35 and they were giving me that behaviour where the motor goes at full tilt backward, or barely spins. Made me think I was doing something wrong.

Only thing I can think of is to add pull up resistors, but I keep coming back to “it all looks fine when p=0”

[gdefender]

If you’re telling the motor to have a forward velocity and it’s going backwards at full speed, that sounds like the sensor is out of phase. The motor starts to turn and there’s immediately error, the motor speeds up to make the error smaller but it actually gets larger. Repeat until the motor is at 100% power and the error is growing.

I would go through this documentation and triple check the sensor phase.
https://phoenix-documentation.readthedocs.io/en/latest/ch14_MCSensor.html#sensor-check-with-motor-drive

[seanw]

Thanks. With the sensor phase at true, the encoder reads the opposite value when I run with just an F. But now that I understand how to check it in Phoenix Tuner, I’ll give it a try that way.

[NewtonCrosby]

My initial testing was just trying to get the motors to spin at a given speed, no balls going through the shooters. Thanks for the code, I will look more closely at it.

I actually overlooked the part of your post that said the motor was going backward. That is definitely a run-away error, which in the case of the velocity pid is certainly going to mean that your sensor and motor aren’t in phase.

Once you have them both in phase (confirmed via tuner), then you can use our code to tune.

[seanw]

Summary - the sensor is in phase, and the talons were switched so the left controller was getting feedback from the right and vice-versa

Went through the method above to verify the phase and play with velocity control to prove it would work without the Rio’s code, which lead me to the discovery of the crossed Talon sensors.

Thanks for everyone’s help!

[TW_6391]

Gdefender, We are currently using PID with through bore encoder going to the robo rio for our shooter PID but would like to move it to be directly connected to talon SRX via CTR breakout board. It sounds like that is the same as your setup. Can you share the proper solder connections of the through bore cable for the CTR breakout? Thanks.

[seanw]

We used a JTAG breakout board which is similar to the CTR breakout board:

Look at page 23 of the Talon user’s guide at http://www.ctr-electronics.com/Talon%20SRX%20User’s%20Guide.pdf and the encoder’s data sheet: http://www.revrobotics.com/content/docs/REV-11-1271-DS.pdf. You’ll want to hook up the 3.3v and GND coming out of the breakout board to the +V/GND on the breakout board and then wire up the ENC A/ENC B pins to each other. The colors of ENC A/B on the data sheet match up to the colors of the wires you get with the encoders.

We wired the breakout board to a set of 2x3 male Dupont pins and used the PWM connectors from the encoder (using only 2 of the 4) so that we could switch between reading them on the Talon or the Rio and not have to cut cable.

[gdefender]

The encoder comes with a number of wire harnesses. We used the JST-PH 6-pin to 4-pin and cut off the 4-pin connector.

We stripped the wires and connected:

• Red - 5V
• Yellow - B
• Blue - A
• Black - GND (on the back)

As @seanw pointed out, you can wire it to the 3.3v power, we wired ours to the 5v power. I don’t think it matters the encoder datasheet says you can use 3.3v-5v.