Hi all,
Having a strange issue with our swerve drive. The encoder readings are seemingly correct. When given input to go in one direction (just +x or just +y for example) the wheels spin in the correct direction, but for some reason, they seem to rotate randomly. I’ve confirmed that the angles outputted by SwerveDriveKinematics are seemingly correct, but the wheels don’t seem to at all turn in the direction that SwerveDriveKinematics is outputting. I’ve played with the PID values all the way from .001 to 1 in the turning loop and all it does is increase the jitteriness. Any help would be appreciated. Code is here (https://github.com/SouthRobotics/Team-6969-2022-Code)
Video of what the robot does, P is turned waaaay down in this example to try to eliminate jitter, but its still evident that the wheels are not pointing in the right direction.
https://drive.google.com/file/d/18T7733HaE2rDTTSlXT2Nx49uWBgTfOqi/view?usp=sharing
1 Like
I don’t see anything obviously wrong with your code, but I’ll offer you the same advice that I offer my programming team when they are trying to solve a problem like this: don’t try to debug the entirety of your program all together. There is enough complexity there that you’ll have a very hard tome narrowing down the issue. Instead, start small and confirm that the basics work, and then build your way up. For example:
-Make a test program that tells just one wheel to move to a set angle and validate that it does that correctly
-Expand that to be for each wheel
-Expand that to take a joystick input turn to that angle
-Expand that to use the swerve kinematics, but don’t use the gyro or any rotation
-Expand that and add rotation
-Expand that and add gyro
At one of these steps you’ll see something behave in an unexpected manner. It’ll then be much easier to debug that issue because you’ll already have a foundation of simpler things that you know work. And once you get to the end of those steps, you’ll have a functioning swerve drive
1 Like
I think your left and right are backwards on your kinematics. Positive to center is the left.
We are both using the 0 state as front left in our code.
we have
private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
// Front left
new Translation2d(DrivetrainGeometry.TRACKWIDTH_METERS / 2.0, DrivetrainGeometry.WHEELBASE_METERS / 2.0),
// Front right
new Translation2d(DrivetrainGeometry.TRACKWIDTH_METERS / 2.0, -DrivetrainGeometry.WHEELBASE_METERS / 2.0),
// Back left
new Translation2d(-DrivetrainGeometry.TRACKWIDTH_METERS / 2.0, DrivetrainGeometry.WHEELBASE_METERS / 2.0),
// Back right
new Translation2d(-DrivetrainGeometry.TRACKWIDTH_METERS / 2.0, -DrivetrainGeometry.WHEELBASE_METERS / 2.0));you have
public static final SwerveDriveKinematics const_DriveKinematics = new SwerveDriveKinematics(
new Translation2d(const_WheelBase / 2, -const_TrackWidth / 2),
new Translation2d(const_WheelBase / 2, const_TrackWidth / 2),
new Translation2d(-const_WheelBase / 2, -const_TrackWidth / 2),
new Translation2d(-const_WheelBase / 2, const_TrackWidth / 2));
1 Like
A quick way to know if you are left and right reversed (which it seems to me you are) is to turn the bot on its side and rotate hard one way or the other. If the wheels make a diamond you are doing it correctly, if they make an X you have left and right reversed.
Thanks, not sure how I messed that up. Don’t have access to the robot until Monday, so hopefully that’s the only issue and it works tommorow! I do suspect that there are still issues with the pid loop or maybe some motor directions reversed or something just based on how weirdly it was acting, but i’m hoping for the best!
1 Like
Update. The directions now seem to be correct, but there are still issues. If the p values are below about .7 or so, some of the motors don’t move at all, while some of the others have the drive motors oscillating. Anything above that and both turning and drive motors to oscillate fairly violently or turn continuously with a slight slowdown near the right angle.
Video of the issue: https://drive.google.com/file/d/1HHFqV7Q3YJNg6JabsTyAGi6XA1nEujpP/view?usp=sharing
When the robot is off, does it take roughly the same amount of force to turn each one by hand? If yes, the issue is most likely a software or electrical issue. If no, the issue is a mechanical issue. Give that a shot as the starting point for diagnosing your issue
1 Like
I’ve gone through and checked. All the modules have very similar resistance when turned by hand. All the wiring is correct, and all the motors that need to be reversed in code are reversed correctly. The issue still seems to be with the PID loop. The data (despite it being swapped left and right initially) coming from SwerveDriveKinematics.toSwerveModuleStates() still appears to be fine, so my thinking is there is an issue with the PID loop since that’s the step in the chain that seems to produce problems, but I have no clue what it could be. The robot performs the same on the ground and in the air, so it’s not an issue with a lack of dampening. I tried adding some D today to try and possibly smooth out the ramp-up to prevent the oscillation, but it had no effect. Could it be possibly related to this issue, even though it seems like a solution has already been pushed for it, so I’m not sure how it could be? Swerve Rotation PID Problems - #19 by calcmogul. Anyways, I’m quite lost on what else I could do at this point, so any ideas, even long shots would be appreciated.
You might want to double-check that your port numbers are all assigned correctly. Wacky things will happen to PID controllers if you swap two encoders.
Also, I strongly recommend that all of your code always lists the four corners in the same order. In some places you have (FL, FR, BL, BR) and in others (FL, BL, FR, BR). That way madness lies.
As a next check, I would that each turn motor and encoder are working in the way that you expect. To do this, do the following one wheel at a time:
-Send a small positive voltage (~2V) to the motor in your code
-Observe if the wheel is spinning the direction you expect it to.
-Send the raw encoder values to the SmartDashboard or your logging tool of choice. Observe if the encoder is returning values that you expect. In particular, observe if they are going up or down as the wheel turns, if they “wrap” around 360 degrees back to zero, and is generally working predictably.
-Send your “adjusted” encoder values to smartdashboard and check the same things (I see that you are dividing by 5V, moving into Radians space, backing out an offset, etc.)
-Measure how long it takes to make 10 full rotations and validate that that is the same across all 4 wheels.
Hopefully that will show you that one one of the motors or one of the encoders is behaving unexpectedly and then you can go from there.
Lastly, can you describe the setup you have for your turn encoders? I.e., what encoder you are using, what is connected to, etc? A picture would help as well.
1 Like
Hi, sorry for the delay, I’ve checked all the ports and IDs and everything appears correct, we are running andymark swerve and steers (pg turn and neo drive) with the ma3 absolute encoders. All the motors turn like expected and the encoder values all make sense and wrap correctly. I’ve been troubleshooting for days and am really unsure of what it could be at this point. The closest I’ve gotten is removing the optimize function from the code which seems to marginally decrease the oscillation, but it’s still there and the wheels still seem to be rotating strangely.
Here’s an image if it helps. Also, some of the commits I made have some changed port numbers, that’s just from some re-wiring we did, so shouldn’t have affected anything.
One thing I notice in your code is that you’re using SlewRateLimiter and configuring it in physical units (const_TeleDriveMaxAccelerationUnitsPerSecond, const_TeleDriveMaxAngularAccelerationUnitsPerSecond), but you’re passing it joystick units:
xSpeed = xLimiter.calculate(xSpeed) * DriveConstants.const_TeleDriveMaxSpeedMetersPerSecond;
ySpeed = yLimiter.calculate(ySpeed) * DriveConstants.const_TeleDriveMaxSpeedMetersPerSecond;
turningSpeed = turningLimiter.calculate(turningSpeed) * DriveConstants.const_TeleDriveMaxAngularSpeedRadiansPerSecond;
Should that multiplication be inside the calculate?
I also notice that your turning PID controller has only a P term. For position control, P alone will tend to induce oscillation because of control lag, and a D term can reduce this. I’m not sure this explains all of what you’re seeing, though.
Nice picture. This is completely irrelevant to the control problem you’re having, but a Robot Inspector would ding you for:
- Uninsulated battery terminals (R607)
- Incorrect wire color supplying power to VRM (R624)
This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.