Having trouble implementing Swerve Drive

Code has been posted to GitHub - JMDeschenes13/2023SwerveDrive

Drive System - Our Team bought the last of the AndyMark swerve and steer drive system - am-3009_CPMA
Running Falcon 500 to drive the wheels, and an RS775-5 for steering controlled with a Victor SPX. We have the MA3 analog encoder with it (at least we think it is the analog version) to monitor steering. Ended up attaching the encoder to a custom gear follower for the steering which is a small pinion at 4:1 of the main steering gear.
Plugged the encoder into the analog ports, and it appears to recognize the encoders, but the code isn’t working. During trouble shooting, we set up the encoder output on the dash board, and if we wiggle the steering back and forth by hand, the output floats around. For the same relative motion it will be positive over the full the range of motion, then all of the sudden go negative for the same range of motion. We pulled the code off of the FRC 0 to Autonomous video and changed it slightly to make it work for our parts. Not sure where to start or what to look for, but fear we have some basic problems communicating properly with the encoder. Also we are currently testing out the system with a 2019 RoboRio.

Any thoughts, help would be appreciated. Thank you.

Can you be more specific about what isn’t working?

Have you confirmed that the gear ratios in your constants are correct? They do not look right to me. What’s the actual gear ratio of your steering gearmotor? What’s the actual gear ratio to the drive wheel? That 0 to autonomous code is based off of SDS modules, you will need to use the correct gearing to get any correct values from your encoders.

Did you use this for reference? https://www.andymark.com/products/swerve-and-steer?via=Z2lkOi8vYW5keW1hcmsvV29ya2FyZWE6Ok5hdmlnYXRpb246OlNlYXJjaFJlc3VsdHMvJTdCJTIyYnV0dG9uJTIyJTNBJTIyc2VhcmNoJTIyJTJDJTIycSUyMiUzQSUyMnN3ZXJ2ZSthbmQrc3RlZXIlMjIlMkMlMjJ1dGY4JTIyJTNBJTIyJUUyJTlDJTkzJTIyJTdE&Assembled%20or%20Kit=Assembled&Drive%20Motor=CIM&Rotation%20Encoder=MA3%20Encoder&Steer%20Motor=PG

It sounds a little like you haven’t attached your steer encoder at 1:1 ratio to the steer motor. Possibly you are seeing the -180 to 0 to +180 which is supposed to be one revolution of the motor but is incorrectly multiple revolutions of the motor.

When we move the joystick, the wheels will start spinning and the modules will turn, but their movement seems to be completely random.

I guess those variable names might not be completely correct. What is called the TurningMotoGearRatio is the ratio of the turning encoder rotations to the actual rotations of the wheel. I was thinking that the actual gear ratio from the turning motor to the wheel wouldn’t matter because I thought the PIDController would just drive the motor until the encoder was in the correct spot. Is that not the case. I believe the gear ratio from the motor to the module is 77:1 I don’t know if it is 77:1 for module rotations or if there is some reductions in the module itself. The gear ratio that I have for the drive motor is the encoder counts per revolution of the wheel. There are 2048 encoder counts per motor revolution and the ratio is 6.67:1 for the swerve module.

SLAB-Mr.Thomas, We do not have our steer encoder at a 1:1 ratio to the steer motor. The only swerve and steer modules we could get didn’t come with encoders attached and we couldn’t get the correct mounting hardware. So, we have them currently mounted at a 4:1 with the wheel. I know this is not ideal and I tried to compensate for it in my code.

When our team meets today, I am going to try and use the WPILIb Swerve example with the relative encoders that came attached to the steering motors and see if I can make that work instead.