Having MK4i Swerve Modules go to correct angle


I’m the programming lead for team 599. For our swerve, we’re using Falcon500s for both drive and turn motors with a CANCoder for each module. I’m trying to have the wheels go to the correct angle when I make a joystick input. The angle readouts from all of the turn motors all match the angle of the wheel, but, when I make any joystick input, the wheels are moving to the desired angle relative to the starting angle. For example, if I initialize the robot with a wheel at 35 deg and move the joystick right, the wheel will move to 125 deg instead of 90 deg (even though the program reads the desired angle as 90 deg). I think the problem is either in the SwerveModule.java file or the subsystem_DriveTrain.java file in the DriveTrain branch. I’ve linked our repo below. I’d greatly appreciate help with this!

1 Like

Sounds like an angle optimization issue?

Are you referring to how we’re finding the optimal angle or how it’s being processed (e.g. whether the PID controller is reading it correctly)?

How’s being processed from the encoder into the PID controller. Could be possible your custom angle optimization could be messing with it somehow. Have you tried referencing BaseFalconFXSwerve: GitHub - dirtbikerxz/BaseTalonFXSwerve? I can see your commented parts of your code derivated from there.

When configuring your turn motor in HardwareConfig.java, you need to set swerveAngleFXConfig.Feedback.SensorToMechanismRatio to use the gear ratio for your swerve drive so that the turn motor will move the correct rotations. Additionally, you can set swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true so that you can use the WPILIb SwerveModuleState.optimize() function instead of relying on your own.

I ended up solving it by setting the motor position to 0 and then having the wheels move to ‘m_AngleOffset - getCANCoder()’ when I initialize the bot. Thank you all for your help!

1 Like