Problems with MK4i / NEO swerve drive in teleop and auto

We’d like to avoid tank drive this year but have been having some code trouble with both our mecanum and our swerve drives. Code we’re using for the swerve is based on code from GitHub - frc3512/SwerveBot-2022: The source code for the 2022 FRC postseason swerve drive robot. and GitHub - Team364/BaseFalconSwerve.

We have two main issues at this point:

  • In teleop, doing repeated back and forth runs tends to rotate and eventually move diagonally (video, don’t mind the parts where the driver corrects with sideways movements)
  • In auto, the robot sometimes does this weird but subtle stairstep diagonal movement (video 1, video 2). Also, the auto is meant to travel 1 meter but instead goes more like 2 meters (the blue tape squares on the carpet are left over from 2021 and are spaced 30 inches from each other). When I print out the estimated pose I also have gotten numbers all over the place like 0.9, 0.7, or I think even -0.3 (yes, negative 0.3) with it still going about 2 meters.

Code: GitHub - SuperiorRoboworksTeam857/2023SwerveTest: 2023 test of swerve code
Robot is NEOs with Spark MAXes over CAN, with SDS MK4i L2, CANCoders, ADIS16470 IMU board

We’ve been working on this chassis for over a month and have had a hard time with code, especially since the first thing that mostly worked was the (what we now know is unmaintained) SDS example code.

any chance that with the tele-op you are running into torque steer? I don’t know much about the Mk4i, but you see that issue a lot with some high torque front wheel drive cars like the Mazdaspeed 3.

other things I would check if you guys haven’t already, would be frame rigidity, we had that issue with the snowbots X-Drive a few years back. Also double check tolerances on the modules, there’s a very slight chance a bearing is a smidge loose causing a wheel to drift.

In auto, it seems to step to the other direction than the drift in tele-op. If you are using a gyro, it might be over correcting for the drift. (I haven’t looked at the code at all, so I’m not too sure).

Check your steering offsets. If one wheel is not pointed exactly the right direction, this will happen. We had this happen when the mechanical team disassembled a module to dislodged some carpet debris, and programming didn’t know to reset the offsets.

As far as the distance it travels in auto, start by verifying your odometry in teleop. Put the estimated pose on the dashboard, drive around a little and measure if the estimated pose is reasonable. If it is not, you probably have something wrong in your conversion using the drive encoder counts, gear ratio, and wheel diameter. Double check all of these parameters, and the math.

With your issue number 1, as @gdefender mentioned, check your offsets in Shuffleboard first. But, if that all seems fine, then try enabling tele-op, but staying stationary for ~10 seconds. SDS has code that retrieves the absolute encoder positions if you aren’t moving, which you can see on line 128 here.

With your second issue, if your pose seems accurate in teleop but not auton, you’re probably double-updating your odometer-- once from your periodic and once from your trajectory follower command. The periodic one passes in 0’s for chassis speeds since you aren’t touching the controller, while the auton one updates it as intended. Rather then updating the odometer in your periodic every time, try adding a condition that checks if you’re in teleop.

Update from tonight’s meeting: pose in teleop is all over the place. There are times that the pose barely changes, and there are times when driving the robot in a straight line that the pose changes roughly sinusoidally up and down about 2 meters amplitude when it should just increase linearly.

P.S. printing the “distanceMeters” field of each SwerveModulePosition in getPositions() in teleop appears to give the correct numbers (i.e. I drive 7 meters and each number for each module seems to change by 7 meters)

If the distance is correct then perhaps the angle is wrong. I don’t think a units issue would show up as sinusoidal… but if the angles are messed up then you could see something like that. I see where you are printing out the angles from the encoders… Add printing out the angle from GetPositions call for each module. If the robot is going straight they should all be the same-ish. (They could be 180 off if the velocity is flipped.).

Update: found the problem. In getPosition() we were constructing a Rotation2d that expected radians but we passed in degrees. After fixing that the pose looks correct and auto seems to be behaving itself.