Help with Swerve Drive programming (Java)

Our team bought the AndyMark Swerve and Steer modules, however our program only causes the swivel motors to spin continuously when a joystick is moved. I am not the “chief” programmer for our team but we would like to get the chassis going in the off season.

We are still using the hall effect encoders on the rear of the motor in stead of the absolute encoder they recommend for it.

Here is a link to our code:

Thanks for any help!

It’s probably an issue with your steer control loops. First of all I would double check your in/out polarity- a positive output from the PID controller should reduce a positive error.

Then comes tuning. I would set all your gains (kP, kI, kD) to 0, then start with a small kP and slowly increase it. You also probably want just a PD loop (0 kI) as you want fast rise time with little overshoot.

Last of all an absolute encoder is almost essential for swerve. You’ll need either that or a limit switch/hall effect sensor to set zero on your modules. Otherwise your quadrature encoder will only measure changes in angle while the robot is on, and not absolute angle.

1 Like

Definitely start with making sure your angles are being read properly by all encoders. Positive/increasing angle should be in the same direction as when the motors are given positive control effort.

A couple “code review” items to look into:

This code handles bad encoders. Do all calls to getAngle() check if -1 is returned and do something appropriate, or is -1 ever used as an actual angle later on?

*( Is it ever possible to get angle = -370 as input to this function? If so, the wrap logic may not work as expected. I haven’t dug deep enough to determine if this is true, just something to double check.

The encoder appears configured to return 0.875 degrees per pulse. Can you confirm this is accurate?

To the absolute encoder logic: What physical state is defined as “0 degrees of module rotation” on the robot? What logic in software (or mechanism in hardware?) guarntees that during at least one point of operation, the robot will know you’re in the zero degree state, and reset the encoder readings to 0?

Check the “desired” angles coming in from the controller too. Print out all four angles passed into your swerve modules objects. Are all of these angles reasonable, based on driver inputs?

Same comment as above - once you’ve verified encoders are reading in the correct direction, try to re-tune. I’d start by setting your I/D gains to zero, P very small. Double P until you get oscillations in steady state. Back it off slowly until the oscillations cease. Then pull in some I to overcome steady state error. Historically, I’ve avoided D for cases like this. Then again, I’ve historically been dealing with high-stiction systems with low controllability. YMMV.

You may also want to add logic to prevent integrator windup. Perhaps only “accumulate” your I term when your actual is within a few degrees of setpoint? There’s a couple other ways to accomplish this…*

1 Like

I went ahead and bought the absolute encoders from AndyMark and the 40 tooth gear. It seems like the encoders are working. They give values between 0 and 359.

The wheels do turn to a degree of rotation, but sometimes they will be over 80 or 90 degrees off. I’ve tried to PID tune but it seems when I increase the ‘I’ value the wheels will start to spin out of control. Plus I may not be PID tuning the correct way.

Here’s the updated code :

So, I is usually the last thing I touch when tuning. In fact, I usually completely ignore it. In fact, for 323’s steering we completely ignored D as well and just used a lot of P. The robot weight and friction will do a decent job damping the system. Or something like that, it worked.

We had a similar issue where the angle seemed to be off by a variable amount, turns out our field centric code was using a different coordinate system than the rest of the bot (CW+ instead of the CCW+ the rest of the system used) so that’s definitely somewhere to check as well.

If you’re still struggling, have you given John a call?

Thanks for all the help. I finally got it to tune right and I runs fairly good.

This is interesting to know about the dampening from wheels on carpet, I’m seeing a similar experience with tuning now. I am currently just using P, but did wonder what you meant by “a lot”. Were you seeing some overshoot? I’m only doing front/back crab steering and the front does overshoot a little with a lower P gain than the back (which doesn’t overshoot). The overshoot settles within a couple periods. Another question I had was about synchronizing the speeds between more than one module. I do notice the module with less gain is getting to the setpoint just a little slower than the other. I’m thinking of reducing the output range of the faster module slightly, but wondered if some better tweak exists.

Any chance your weight balance is biased one direction? That would explain one end overshooting and the other not. Think about the load on the system for different weights on the wheels.

For synchronizing, we used motion magic for steering which let us set max velocities for everything. But, with all 4 wheels independently steered each wheel ends up traveling different amounts any way so I never really worried about it.

Is there any way you can post the final project of the working swerve code? Thanks!

Here it is:

It was very basic and crude. There was a lot of room for improvement.