PID Not Sending Voltage

My team has recently been working on swerve drive. We are using a PID system along with some basic swerve math. We have the drive working, however we cannot seem to get the turning motor to work. We are printing the Error, Current Motor Voltage, SetPosition, and Raw Encoder Value. According to the telemetry the motor should be receiving

power in order to get the error as low as possible. Although the Error is high, no power is being sent to the motor. Any help is appreciated.

What motor controller are you using, and what are its lights doing?

We are using a PG71 gearmotor along with a Victor SPX for the controller. The light are blinking orange like normal. I can control the motor directly with the joystick to make sure it wasn’t wiring. I just think that I am somehow using PID wrong?

I don’t see where you’re actually angleMotor.setVoltage()-ing with the output of the PID controller (a method I don’t know offhand)

1 Like

I thought the PID declaration had it in it: frc::PIDController pid1{-.057, 0.25, 0, &motor1Encoder, angleMotor1}; The motor is called angleMotor1

What kinds of values are you expecting out of that PID controller. Its output is directly applied as a (-1, 1) output to the motor IIRC. In addition, that setpoint clamping stuff looks way off. Looks like it’s wrapping instead of clamping.

1 Like

We were expecting the voltage for the motor. We were trying to fine tune the P I D values. We had them set higher at first, however the motor was overcorrecting and doing a constant back fourth motion. Then we got the back and fourth motion to stop, however there was no reaction from the movement of the joystick. So I decided to print the voltage and it stayed at 0. I don’t know if this is important, but the motor was holding its position(couldn’t be moved by hand). so its trying to so something. I just don’t understand why it wont be affect by the Error and Setpoint like it’s supposed too.

17 counts on an encoder is usually tiny. What does that 17 on the error actually signify?

1 Like

I think we had just deployed code when I took that snapshot, so the motor had only moved a little. The 17 on the error is avg error for the PID. I just used the Average error function in the PID class to call it and put it to the Dashboard for telemetry.

If the motor is holding its position, it’s either just brake mode with maybe a high gearing, or your PID is actually holding a setpoint, just not the one you thought it should be. Again, why are you feeding a setpoint using constants of MAX_VOLTS, why are you wrapping that setpoint, and where do you actually use motor1angle?

1 Like

How are you controlling the motor with the joystick? Since the old PIDController writes to the motor directly, it seems possible that your joystick control code is overwriting the PID control. This is one reason that the new PIDController was written synchronously, rather then asynchronously like the old PID controller. It makes it much easier to control the flow.

The math for the turning motor takes the joystick value into its calculations. Once all of the math is done and an angle is found. That is then fed into the SetPoint for the PID System. The PID System takes in the encoder value for the input and the angleMotor is the output. I thought the SetPoint is what the PID control “goal” is, while the Encoder value is where it currently is.

You have the theory mostly correct, but that’s…not at all what you’re actually doing. Can you show us the line where you convert your angle into the “setpoint” variable that you feed into the controller?

1 Like

double a = STR- RCW * (L / r);
double b = FWD + RCW * (L / r);
double c = STR - RCW * (W / r);
double d = FWD + RCW* (W / r);

STR = Rotational Axis
FWD= Forward Axis
STR = Strafe Axis

 double motorAngle1 = Math.atan2 (a, d) / Math.pi;
double setpoint = motorAngle1* (MAX_VOLTS * 0.5) + (MAX_VOLTS * 0.5); // Optimization offset can be calculated here.
    if (setpoint < 0) {
        setpoint = MAX_VOLTS + setpoint;
    if (setpoint > MAX_VOLTS) {
        setpoint = setpoint - MAX_VOLTS;

    pid1.setSetpoint (setpoint);

MAX_VOLTS is the max voltage of the motor.

Now that I am looking at the actual code I don’t know if the motorAngle1 needs to be converted to something else in order for the PID controller for this to work. I am not sure about that calculation though.

This whole section does not do what you think it will. A little bit of theory. The PID controller consants that you give it when you declare it (actually we’ll start with the first one) is the P constant. This basically says “Take the number of angle units we are off by, multiply that by this constant, and that gives you a motor output.” Now. Given that, there are several things in your code that you don’t want to be doing.
Let’s say that your target angle is 100 encoder counts, and you’re currently at 10 encoder counts. Your P constant is 0.5.

  1. You’re multiplying the setpoint by MAX_VOLTS (in this case 4.95), which will make your setpoint about five times the actual target angle and put it in units of angle-volts. The setpoint is now 495, in units of count-volts.
  2. You then in your if statements do some sort of wrapping of the new setpoint base MAX_VOLTS.
    With our example numbers, the second case will trigger and subtract 4.95 from that setpoint. New setpoint is 490.05 count-volts.
  3. You then feed that into a P controller. The P controller takes the setpoint input, which it assumes to be in counts, and subtracts from it the current position to find the error. 490.05 -10 = 480.05.
    It then multiplies that by the P constant to get a (-1,1) output. (line 50 for the clamp)
    However, this multiplication comes out to 240.025. It clamps that to 1, which should full-speed the angle motor forward. I’m not sure why it’s not, but there’s probably something else completely wrong.

TLDR: You shouldn’t be using the voltage in the way you do, if at all. Take the MAX_VOLTS entirely out of the picture. It’s not doing at all what you want it to do.

1 Like

Your angle needs to be in encoder ticks. What model of encoder are you using?
Wait a second…If you’re using an absolute encoder that goes 0-4.95 volts, I can see why you would do what you did. However, that’s not at all clear from your descriptions. Additionally, your wrapping code can simply be a modulo operator, which is % in Java, probably the same in most other languages. Your angle calculation would then need to be in a percentage of the full range of motion of the encoder.
I’m actually not at all familiar with how absolute voltage-based encoders are handled in WPILib, but it’s not with the Encoder class, which is for quadrature encoders as seen here:
Encoder (WPILib API 2020.3.2-60-g3011ebe) (java but the same thing) You probably want the AnalogInput class.

1 Like

Ok thank you very much for the time and help

Yeah no problem. Go ahead and try with the AnalogInput instead of Encoder. It should (but this is software so it won’t) actually be a drop-in replacement. You’ll need to plug the absolute encoder into the analog input row on the RIO though.

1 Like

Yeah that’s how we have the encoder reading. What exactly does the AnalogInput do for the encoder value? I will also try getting rid of the wrapping of the setpoint. So the feed of the values should be motor1Angle->setpiont… Then that will be the PID’s “goal”, while the Analog Input of the encoder would be the current “error”?

The Encoder class expects a set of two channels, alternately going high and low. It is a quadrature encoder. Meanwhile what you want is to get a voltage on a pin somewhere in the middle. This should help: Analog Inputs - Software — FIRST Robotics Competition documentation
In this case, you want to convert your angle from radians to percentage of a full revolution. Then you can multiply it by MAX_VOLTS to get the setpoint for the PID. The analog input. getVoltage() method would be your current position, and the difference between the two would be error. You’ll definitely need to change your PID constants.

In fact, you may be able to get out of multiplying your setpoint by MAX_VOLTS if you use this: Analog Potentiometers - Software — FIRST Robotics Competition documentation
However, it might be a bit off because it scales 0-5 instead of 0-4.95.

1 Like