My team (9445) is a rookie team this year using the Swerve Drive Specialties mk4 modules. We have assembled and wired a drivetrain (29" square) and have moved to programming it. I am the team’s programmer, and, with some difficulty, I have gotten the robot to translate effectively (forward/backward, left/right), however, when trying to rotate the robot, we have no success.
I have attached a Github link which contains the robot code. The SwerveModule, Drivetrain, and DriveCommand (.cpp/.h) files are where most of the logic happens and are probably where the problem resides.
Github code
I greatly appreciate any help that can be provided!
How are you trying to control your robot? My team (and many others I believe) use the x axis of the right stick for theta. It seems like you’re doing something interesting.
There are two systems for turning. There is the typical way, which can be found around line 95 of RobotContainer.cpp, and there is a custom system which is designed to make turning field oriented (lines 45-85). The system used for turning is controlled by a constant (OperatorConstants::usingFieldOrientedTurn), which currently evaluates to false (use the simple turning system)
The issue is resolved. I have eliminated the use of the FIRST Swerve drive kinematics and custom written the math in the code. I am certain that I was using the library incorrectly in some way, but I was unable to find the problem. Thanks for the assistance!