HELP Swerve Simulation Rotation spinning crazy

Boyu · July 4, 2025, 1:59pm

Recently i have been testing out simulation and the IO structure with advantage kit and advantage scope and I am met with this problem where my swerve can move normally in the simulation until i have an input on the right x axis which controls the rotation of the entire chassis. After there is an input on the right x axis the swerve starts to spin no matter what I input, if i drive input to drive forward it will just spin and start going faster and faster. I feel like the problem is with the yaw of my pose estimator not updating properly but I am not sure. Below is the code attached

github.com

boyu0802/IO-structure-offseason-testing/blob/main/src/main/java/frc/robot/RobotState.java

package frc.robot;

import java.util.Optional;


import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;

import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.util.Units;
import frc.robot.Subsystems.Drivetrain.DrivetrainConstants;

This file has been truncated. show original

trixydevs · July 4, 2025, 5:14pm

Check that you’re not crossing radians and degrees. It can be tricky!

txrnqt · July 4, 2025, 8:19pm

I had a thing like this happen to me but the issue was I was using the wrong controller if your using the command xbox controller for you controller input in code and use a playstation controller to try to drive it will cause the robot to spin and presumably vice versa

dabeycorn · July 4, 2025, 8:53pm

I had a similar problem like this when writing our team’s swerve code. I did something similar to this to fix it:

if(!record.gyroYaw.isPresent()){ // gyro not available in sim
            Twist2d twist = kinematics.toTwist2d(lastModulePosition,record.modulePositions());
            Logger.recordOutput("RobotSTate/Odometry/lastModulePosition", lastModulePosition);
            Logger.recordOutput("RobotSTate/Odometry/record Module Pos", record.modulePositions());
            Logger.recordOutput("RobotSTate/Odometry/Twist", twist);

            double angularRate = twist.dtheta;
            lastGyroYaw = lastGyroYaw.rotateBy(Rotation2d.fromRadians(
                !Double.isNaN(angularRate)
                    ? angularRate * 0.02 // multiply rate by period 
                    : 0);
} else{
    lastGyroYaw = record.gyroYaw.get();
}

Boyu · July 5, 2025, 1:48am

I tried this but there it still spins like crazy. The gyro yaw changes normally when i rotate the swerve, but after that if I try to move the swerve forward it also changes the gyro yaw.

Boyu · July 5, 2025, 1:49am

I use xbox controller for both the code and to drive it.

Boyu · July 5, 2025, 1:53am

is it possible that it is something like the swerve modules don’t point forward when I drive forward but instead stay in their current orientation. so when I try to drive forward it will move the wheels but the angle of the wheels don’t change.