Why doesn't this swerve code work?

Technical Programming
1

Greetings,

The Problem
I am the only programmer on a small team, and I’ve been trying to get out WCP swerve drive modules working since last build season until now. We’ve tried many things, but so far we just can’t hit the nail on the head, as it were. I’ve tried posting more specific questions here along the way, but I am now so completely lost that I’m not sure what to check next.

I understand that the question I’m asking is extremely vague, and I will do my best to provide as much information as I can to make it less so.

Background Knowledge
The code repository in question is https://github.com/SirFireE/Swerve8551. Please note that I may be actively making new commits to this repository in the future.
Our swerve modules, as mentioned, are WCP swerve drive modules with Rev NEO motors and CTRE CANCoders. We have confirmed that all of this hardware works as expected. Our spark motor controllers are currently factory reset, and our CANCoders are factory settings except for absolute magnetic offset.

Program History
We received our CANCoders about a month ago, and since then, we have been trying to simply set the position of one module. Our first attempt was to use the WPILib PIDController class, and call .calculate() periodically, every 20ms, and set the returned value directly to the motor using the CANSparkMax’s set() method. The result of this was that the motor always overshot the target and was shaking violently, or sometimes the motor did not move at all, even though the PID error was large.

We also tried using the PIDController for all four modules in the code repo mentioned above, but they were not outputting any value no matter what the setpoint was. We did confirm that the SwerveModuleStates were updating and working as expected based on the Joystick inputs.

Even before this, we’ve had much better success with the SparkMaxPIDController, but it does not take the CANCoder as an argument for setFeedbackDevice(). My weird idea to fix this was to get a RelativeEncoder object and set its position to the CANCoder’s absolute position periodically, again, every 20ms, and set the SparkMaxPIDController’s feedback device to the RelativeEncoder. This actually did work, but only with a very low PID proportional term (0.2) and a very high PID derivative term (0.7). The movement of the motor was fairly slow, (As expected) and sometimes the motor wouldn’t move for some reason, but we were willing to start with that as that was the best we had.

With that, we set up the same thing with all four swerve modules, as can be seen in our code repo here. The result of this was that all of the modules were overshooting their setpoint and shaking, even with the same PID terms as before (P: 0.2, D: 0.7). This was all in our last meeting, so that’s as much as we’ve found thus far.

Conclusion
All of this has been very frustrating to me personally as the only programmer because I do feel like I’m missing something obvious, but I’m not sure… We’re really hoping that we can get our swerve drive working before the 2024 reveal. I’m wondering if there is something completely obvious in our existing code that I’m just not seeing. I will try my best to provide more information as this goes along, and even if it only barely works at the end of this, I and my team will be so so so thankful for your help! We are still a relatively new and small team, so swerve drive was a big thing for us. Hope y’all have a fantastic day.

2

The relative encoder measures the position of the motor shaft. For this solution to work, you’ll have to set a conversion factor equal to the gear ratio, so the relative encoder will measure the position of module. I glanced at your code and didn’t see you do that, but maybe I just missed it.

1 Like
3

That is an excellent point, and I do know that it’s the ideal way to do it, but I chose to do my weird way at the time because I’m not entirely sure what our gear ratio actually is. That will be my goal for next meeting. I’ll post an update here as to what we get from that.

4

Hello again,
I’m currently at a meeting but taking a break from programming because some mechanical work is being done to the robot.
I’ve attempted to set up a normal PIDController setup with two of our modules. (the other two are under maintenance) I’ve updated the code repository to the code I’ve tried tonight.
The results of running this code was that the the motors, again, would either spin or shake rapidly, or the motor simply wouldn’t move at all, despite the PIDController outputting a sufficient value to move the motor, and despite testing several different PID values. Currently one of the turn motors actually froze after running this code, but the other ones are fine. Not sure if that’s related or not. It’d be great if I could get some feedback before I leave the shop. Please let me know what other info I need to send.

5

  • You don’t call configAbslouteSensorRange on your CANCoder, nor do you call restoreFactoryDefaults. And since this is a persistent config, you don’t know if your CANCoder is measuring 0 to 360, or 180 to -180. So you should take care of that.

  • Make sure anything that needs to be inverted is, and nothing else. I’m not familiar with the WCP swerve, so that’s up to you. (Side note, instead of adding a minus sign like you did, you can call configSensorDirection).

  • Spin the module by hand, look at the CANCoder’s measurements, and make sure they’re really what your code expects them to be.

6

If you have the positive rotation of the motor backwards from the sensor, this can make your PID control unworkable. (As shush has pointed out.) Check this very carefully.

ProfiledPIDController works much better than regular PIDController, in my experience. You should be able to set I and D to 0 this way, so you only have to tune P. You do have to provide max. velocity and max. acceleration with this class, but you can measure max. velocity pretty easily. Max. acceleration isn’t much harder to measure, but you can start with a low estimate and see how that works – maybe take the motor’s theoretical max. acceleration with no load and divide by two or three.

I’d start with just using the absolute encoder to control things. Trying to reset the built-in relative encoder every 20ms is probably not going to work well. One issue is there are small time delays in all of the CAN messages, so doing this properly when things are moving can be really hard to do. Since you are effectively running at least part of the control loop on the RIO anyhow, there’s not much point in using the built-in relative encoder. It has pretty low resolution, and it’s really more for where you don’t have any other sensor or are running control directly on the motor controller.

Taking a step back, getting swerve working on your own code can be fairly involved, so you really want to go step-by-step in a systematic way. Make sure you can run your motor controllers, and check that when you command positive rotation, the steering turns counter-clockwise (as viewed from above). Then check that your sensors are returning reasonable values and that positive rotation of the steering results in increasing steering angle readings. You can check to be sure your offsets are at least close at this point. If you are doing your own code, the zero position is up to you, but having zero be straight ahead is the usual convention and works best when using WPILib. (See https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html.)

Once you are sure this is all good, you can focus on the code. Start with the general algorithm and know how you want things to work, then work through the code a step at a time. At a high level, you want to find the steering angle by reading the absolute sensor, to compute the desired steering angle, and to derive an error from the difference between the two. Then, use the error to work out what to command the motor to do. The ProfiledPIDController class does most of the control for you, and the WPILib SwerveDrive classes can give you the desired steering angle for each module.

The other part of this is using information from past computations and sensor readings to do things like find the D and I values that are used in PID computations. Or, in the case of ProfiledPID controller, to do motion planning. This is where you are probably best off reading the docs, or starting from some code that’s already been debugged. You can simplify though, for example by commanding your steering angles based only on button pressed for a few fixed angles.

If your issues are things like improperly tuned PID or not having directions inverted in the right places, it’s generally hard to work out from just the code. On the other side of things, a problem in the algorithm/approach can sometimes be found just from a high-level description. If nothing else, the relative encoder is complicating things, so my recommendation is to take that out, at least until things are working with a simpler set up.

7
  • I’ve set that setting via the Tuner X tool and saved it. Is there an advantage to 0 to 360 or 180 to -180 or does it matter?
  • I believe that everything that should be inverted is, though I will verify this. I should be expecting positive voltage and positive encoder position to be the same direction once properly inverted, correct?
  • I have done this, though my measurements are entirely dependent on the question in my second point.
8

I reccomend setting configs in the code, not in the Tuner. That way you always get the settings you want, even if you have to change an encoder or motor controller, or if you run different code that uses different settings.

There isn’t an advantage to one or the other, but there are certain pieces of code that rely on it being one or the other, and if it isn’t, the code doesn’t do what you want (e.g. enableContinousInput).
Also, if I remember correctly, the kinematics supplies SwerveModuleStates that have -180 to 180 rotation.

Correct! And as @nuttle pointed out, this should be counter-clockwise.

Great :sunglasses:

9

I’m now setting the CANCoder configuration via the code. I am running this to set the encoder range:

m_canCoder.configAbsoluteSensorRange(AbsoluteSensorRange.Signed_PlusMinus180);

For some reason, however, the readings from m_canCoder.getAbsolutePosition() are still showing 0 to 360. For example, when the wheel is turned 45 degrees to the left, it shows 315 degrees instead of -45. Any idea why this is happening? This is not inside the code repository in the original post by the way. Any idea why this is happening?
EDIT: Always learning things the hard way here at RoNex Robotics… Using the Shuffleboard gyro widget will convert -180 to 180, to 0 to 360. Not sure why it does that but in any case, that problem is solved.

On another note, I have confirmed that the motor and the encoder are working as intended. They are rotating in the same direction.

10

Alright here’s a problem that’s been persistent among the last several meetings. I have a PIDController with a P term of 1, I and D terms of 0. If I supply a setpoint of Math.PI / 4, (45deg) the PID error is 0.78 when the wheel is at 0 degrees. This seems pretty high to me already, and then if I turn the wheel to -45 degrees, the PID error is 1.5. At -135 degrees, the PID error inverts itself to -3.1. The problem is, if I’m going to supply this value to the motor, doesn’t it need to be -1 to 1?

11

The error is just the setpoint minus the measurement. So (seeing your controller uses radians) this makes perfect sense.

The output of the PID controller depends not only on the error, but also on the PID terms.

If you only use a P term, then the output is simply P times the error.

So if you want a smaller output… make P a smaller number.

12

That makes sense. I did try to use a smaller P value of 0.5, which worked but with a considerable amount of shaking before it finally settled at it’s setpoint. To solve this, I tried adding a D value of 0.1 which made it far worse, making it not settle at the setpoint at all. I reduced the D value to 0.01 and this worked, but still with some amount of shaking still. I then added a position tolerance of 1, which actually made it work perfectly the first time with resistance from the floor. We kept running it over and over again to confirm that it was not a fluke, and it always worked, though the shaking was variable each time. We’re going to try to run this code on all four modules next week, as before we’ve only been testing one at a time. We would very much like to eliminate the shaking, especially with the setpoint changing many times a second when driving.

1 Like
13

Position tolerance of 1 (radian?) sounds like a lot.

FWIW, for nonzero “D”, it is common to low-pass filter the input, since “D” amplifies high-frequency noise.

In our experience it’s also helpful to use a feedforward term using the profile’s target velocity, so that “P” doesn’t have to do all the work.

14

Yes, it does seem large. (Yes, radians) We’re using the .setTolerance() method of the PIDController. In my mind, position tolerance is the range around the setpoint that will make the error effectively 0, therefore giving the motor a larger space to stop and not overshoot. We initially chose 1 because we assumed that if we set the motor to a position less than 1rad, the motor would not move. Instead, it worked perfectly as stated above. I don’t know why it did that, but it did so I wasn’t going to question it at the time. I get the feeling that I’m incorrect about what position tolerance is.

EDIT: After looking at the docs for PIDController, it looks as if .setTolerance() only affects the .atSetpoint() method. Would be be effective to run something like this?

m_pidController.setTolerance(...);

if (!m_pidController.atSetpoint()) {
  m_motor.set(m_pidController.calculate(...));
} else {
  m_motor.set(0);
}
15

yeah, that would work, and you’re right that the controller itself doesn’t listen to the tolerance. it’s true that deadbanding around the goal is a normal way to prevent chatter, but you’d want the deadband to be quite small, like 0.02 radians, to prevent tire scrub, so the deadband would probably within the lash of the steering gears.

team100 noticed the same sort of chattering that you’re describing and attributed it to low-level high-frequency noise in the measurement (we use analog steering sensors, we know they’re kinda noisy), so you might look at that too.

16

Did you try other values as well, before settling on 0.5? Finding the best numbers for the PID terms takes systematic trial and error.

It might be helpful for you to graph the error and measurement, and look at how the module moves with different PID terms.

17

This type of thing (hard to tune PID) is why ProfiledPIDController is there…

18

Apologies if these questions are basic, but I’ve never used a ProfiledPIDController before.
To measure the motor’s max velocity, can we run the motor at 100% power, graph the velocity, and find the highest point? Which encoder should we use, the built-in motor encoder or the CANCoder? (I would assume the built-in encoder) Should this be measured with load or without?
To measure the max acceleration, you said to find the motor’s theoretical max acceleration. I don’t see that anywhere on the motor’s data sheet. Is there a place to find that or is it another thing we have to measure?

19

For turning, you want to use the absolute sensor for everything. The ProfiledPIDController is going to work on the sensed steer angle and the desired steer angle. So, you want to measure the maximum velocity as observed by that sensor. You can just pretend the motor doesn’t have a built-in sensor, as you won’t use it at all.

Yes, give the steering motor 100% power and then see what the maximum consistently achievable velocity value turns out to be. This might be slightly different in one direction than another, and it might be lower when the robot is on the ground, on carpet. You are just looking for the maximum velocity to use in the planned rotational motion of the steering angle. If it is low, the steering won’t snap into position as quickly as it could. If it is high, the actual motion won’t be able to keep up with the planned motion, and you might get overshoot/oscillation.

You can measure the max. acceleration, or just time how long it takes for the motor to reach max. velocity from a full stop. That’s probably easier than deriving it, since you need to know about the mechanics of the load the motor is driving to do this. We found that we really don’t want to use full power acceleration for our steering motors, so we measured the max. acceleration and then cut the value we actually plug in way back, after some experimentation.

Here’s a snip of C++ code that has the values all plugged in. C++ has units – Java doesn’t have this, so you have to be really careful with the units. If you just use consistent units, things should cancel out though.

    constexpr units::angular_velocity::degrees_per_second_t kTurningPositionMaxVelocity = 1250.0_deg_per_s;
    constexpr units::angular_acceleration::degrees_per_second_squared_t kTurningPositionMaxAcceleration = 12500.0_deg_per_s_sq;
    constexpr double kTurningPositionP = 0.005;
    constexpr double kTurningPositionI = 0.0;
    constexpr double kTurningPositionD = 0.0;

frc::ProfiledPIDController<units::angle::degrees>(
        pidf::kTurningPositionP,
        pidf::kTurningPositionI,
        pidf::kTurningPositionD,
        std::move(frc::TrapezoidProfile<units::angle::degrees>::Constraints{
            pidf::kTurningPositionMaxVelocity,
            pidf::kTurningPositionMaxAcceleration}));
20

Note that P is lower than you might guess, because the angles are in degrees. So, the angle error is going to appear larger than it would in radians. This holds for the other constants as well.

What is going on here is that the controller keeps track of the actual position of the steering angle, using the sensor data (you feed this in every cycle of the robot main loop – 50 times a second). But, it also keeps track of the current velocity. You will feed in the desired steering angle at least any time this changes but, most likely you will just also do this every cycle of the robot main loop. This means that the controller can compute the steering angle error on every cycle of the robot main loop.

Given the current position and velocity of the steering angle, as well as the desired end position (the end velocity is zero), the controller figures out how quickly it can get to the desired end state. When you graph this plan out with time on the x-axis, it forms a trapezoid. So, this is sometimes called trapezoidal motion profiling.

You can probably at least start by just plugging in these same numbers, if you feed in your angles in degrees. This was for a nearly full-weight robot with four SDS Mk4i modules powered by NEOs. But, I think we might have just copied these constants from another robot that used the regular Mk4 modules.