In your SwerveModule, your turning ProfiledPIDController doesn’t look right to me.
Your turning constraints seem awfully high. Can your robot rotate at 200 revolutions per second?
You might want to check your units. The turning controller’s calculate method should be returning a velocity in radians per second. However, you are adding it with the feedforward voltage, and calling motor.setVoltage().
I totally missed that you put them in the original post, oops.
Because you tuned for a straight path, you might be getting a bunch of error in more complex paths. It is pretty easy for the robot to follow a straight path really well with 0 P constants since the FF will get you pretty close. It becomes a lot harder when there’s curves, momentum, module rotations, etc. to deal with.
A little example to show how little a P value of 0.5 for X and Y is actually doing:
Say you have an error of 0.1 meters and have a current target velocity of 2 m/s. Your output velocity will be targetVel + (P * error), so 2 + (0.5 * 0.1), or 2.05 m/s. If the output stayed constant it would take 2 seconds to correct for that error.
A P value I usually recommend as a starting point would be ~5. In that same example, the output would be 2.5 m/s. So it will be much more capable of correcting for error quickly.
There may be some other issues somewhere else, but I’d try giving it a go with a higher translation P and see if that helps.
I gotchya, that makes sense. I’ll adjust my P value and see what happens.
The issue I have doing that is when we had a P value that high, the robot would drive insanely off course. I had it drive a 1x1 m box and it drove across the room. I moved it back down and it was fine.
If you got your feed forward values from SysID they are most likely in rotations, but it depends what you selected, radians is an option in SysID. If it’s rotations, you have to convert from your target in radians to rotations before calculating. When I try that, I get feedforward of 8.93 volts.
I expected the turning controller to be in radians, and the fact that your constraints had Pi in them seemed to confirm that. But if you’re working in voltage, your constraints will also be voltage. That means your velocity constraint would be voltage, and your acceleration would be voltage/second. The constraint values would be no more than 12 in this case.
I’d probably recommend using a normal position PID for your module rotations instead of a profiled PID controller. The motion will be smoother if its profiled, but it will reduce your rotation response time which can play a part in causing the oscillations at higher X/Y P values. We are using a position PID on our Mk4s and the module can do a 90 degree turn nearly instantly.
Further note on constraints (this applies to both your module constraints and the path constraints), you will want to constrain the max velocity to be a bit below the actual maximum of the system. This will give it some headroom to actually correct for error.
I switched the turning PID from profiled to normal. The P value is gonna change. I am up to 8, I believe right now and was wondering what yours looks like. I had to pause working on Path Planner to working on this (which I suppose I used by PP).
I also printed out the feedforward and PID calculations and am seeing much more reasonable voltage outputs.
The PID for our module rotation? It won’t really be too useful to you since it is for a falcon running phoenix pro, not a spark max. But, we have P = 6 and D = 3. That is in terms of motor rotations to output volts
I cannot get a regular PID to work. I posted this on discord:
If I use this:
final var angularPIDOutput = m_turningPIDController.calculate(m_moduleAngleRadians, optimizedState.angle.getRadians());
final var angularFFOutput = turnFeedForward.calculate(m_turningPIDController.getSetpoint());
final var turnOutput = angularPIDOutput + angularFFOutput;
the module is off by about 8 degrees at 90 degreee setpoint. It is off by about 14 degrees at the 180 degree setpoint. If I remove the angularFFOutput it wil go to its position. Obviously this is not ideal. What am I doing wrong with the FF?
If I use a profiled PID, I have to use HUGE velocity and acceleration values, which apparently makes my Path Planner not work.
If I use a regular PID, the feedforward screws up the angle. At 180 the FF sends -1.5V and the PID sends 1.5V.