# SDS MK3 swerve base code

SDS publishes the free speed of their swerve modules, which is what I based the estimate of 4 m/s on.

No, our drive gear ratio is 8.16. (The calculations are in the Constants file).

1023 is what CTRE uses in its PID loops as “full output,” so yeah, if you’re controlling onboard the motors this would be the maximum output.

This one’s kind of arbitrary. I started it low and kept increasing it until I was happy with the performance. I might have done similar calculations, though, so I’ll update this once I’m back in the shop and can look at the math.

I also forgot to respond on voltage compensation, basically this adjusts output voltage so it’s consistent across different battery voltages.

Thanks that, l see that you have used kp ,kd and kf in your turning motor, how do you calculate these three parameters? And if l want to test which `kModuleMaxSpeedTurningRadiansPerSecond` fits l best, l will start from 1 Math.pi and increase until l am happy with the performance. But what is the best performance, is there any creteria for it?

Let’s start with kF. There’s a guide on the CTRE docs for finding kF, we did so with a regular output of 40% (this was somewhat arbitrary to be honest). For kP, we basically did the same thing we did for the drive motor. Maximum error with optimized swerve headings is pi/4 radians, which is about 3000 pulses. To get 1023 (full output) kP would be about 0.3. We tuned it down to 0.2, though. We introduced kD when we had a bit of overshoot, and kinda tuned it arbitrarily. Not sure if there’s a great way for benchmarking kD, if anyone more versed than me wants to chime in that would be cool.

Thanks that . l have tried some parameters today and have an effect below.

Is that the good performance you say before?

That looks pretty good (you’re getting between setpoints fast), but it’s hard to judge because I don’t know exactly what you’re doing and I haven’t extensively used Phoenix for tuning. It looks like you don’t have motion profiling on, or that the velocity limit is a little too high. You’re ultimately the judge of performance, though.

How to do the motion profiling?

Well, you would set a cruise velocity and acceleration and then set the motor in Motion Magic mode.

You can somewhat do this by specifying a custom center of rotation: https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java#L87

As @vargoose84 mentioned above, you will need accurate odometry throughout the match for this because you will need the center of rotation (e.g. hub) relative to the robot center.

1 Like

Oh right!!! That’s pretty sweet. It’s kind of different than what I had in mind (basically your x and y would be replaced by radius and angle) but still works. Another goal is to use the dpad to let drivers turn about any wheel.

l have set cruise velocity and cruise accel in motion magic . Do also need to set them in motion profile?

Can you explain this in depth?

Motion magic is CTRE’s implementation of a trapezoidal motion profile.

Basically the normal strafe movement (left x stick) would rotate the robot around the hub, and the normal forward/back movement would get closer or farther from the hub.

Ether’s papers from 11 years ago derive most of everything you want to know about swerve, including what he coined as the moon, rotary and dosado maneuvers.

1 Like

How do you tune your thetacontroller in the auto?(l have some trouble when tuning this controller these days)

I think I started at 1, then increased to 2, then 5, then 10. I had some oscillation at 10 so I left it at 5. It’s just like tuning a P term.

Yeah, l also tune in this way . Howevre ,with the same thetacontroller, every time l enable the autonomous mode ,its action is different, l just can’t judge how the thetacontroller works well. Is there something wrong? Besides , how to set the max speed and the max accel of the profiled pid controller?

Hmm. I’m not quite sure, but I would try to graph angle as you set it to a specific angle and find whatever works “well enough”. If there’s another problem it’s hard for me to figure that out remotely, but I would make sure everything works sufficiently in teleop.

For setting max angular velocity and acceleration, the method used in my main branch of just putting numbers is non-ideal. If you look at the constants file in the feat/localization branch, it calculates angular velocity by dividing max velocity by the radius, and angular acceleration similarly. Also, it calculates max linear acceleration based on the maximum voltage (12V), the maximum velocity (which can be set wherever) and the kS, kV, and kA terms. It’s based on the motor equations in this paper.

How to graph it?

Yes, everything works well in the teleop . And l don’t use the thetacontroller in the teleop

Thanks that , l will dig into this.

I would graph the gyro angle on Shuffleboard (although I do think maybe the issue could be resetting the heading like someone said in another thread.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.