diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 4d245065b..7e98905a0 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -17,6 +17,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.Timer; public class Swerve extends SubsystemBase { public SwerveDriveOdometry swerveOdometry; @@ -35,6 +36,14 @@ public Swerve() { new SwerveModule(3, Constants.Swerve.Mod3.constants) }; + /* + * By pausing init for a second before setting module offsets, we avoid a bug + * with inverting motors. + * See https://github.com/Team364/BaseFalconSwerve/issues/8 for more info. + */ + Timer.delay(1.0); + resetModulesToAbsolute(); + swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions()); } @@ -124,4 +133,4 @@ public void periodic(){ SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); } } -} \ No newline at end of file +}