Swerve module randomly inverting

Technical Programming
, ,
1

Hello, we have a very weird behaviour with our swerve drive that doesnt seem that it cant be easily replicated nor seems to happen to anyone else.
When a power cycle or a code deployment happens, sometimes, one of our modules inverts their movement, it follow the modules speeds and rotation but dirves the other way, it happens randomly and sometimes the module changes

public class SwerveModule {

    public final int moduleNumber;

    private final CANSparkMax driveMotor;
    private final CANSparkMax turnMotor;

    private final RelativeEncoder driveEncoder;

    private final AnalogEncoder absoluteEncoder;

    private final SimpleMotorFeedforward feedForward;

    private final SparkPIDController driveController;
    private final PIDController turnController;
 
    private final double absoluteEncoderOffset;

    private Rotation2d lastAngle;
    
    public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants){

        this.moduleNumber = moduleNumber;

        driveMotor = new CANSparkMax(moduleConstants.driveMotorID, MotorType.kBrushless);
        turnMotor = new CANSparkMax(moduleConstants.turnMotorID, MotorType.kBrushless);

        absoluteEncoderOffset = moduleConstants.angleOffset;
        absoluteEncoder = new AnalogEncoder(moduleConstants.absoluteEncoderID);

        driveMotor.restoreFactoryDefaults();
        turnMotor.restoreFactoryDefaults();

        driveMotor.setInverted(moduleConstants.driveReversed);
        turnMotor.setInverted(moduleConstants.turnReversed);

        driveMotor.setIdleMode(IdleMode.kBrake);
        turnMotor.setIdleMode(IdleMode.kCoast);

        driveMotor.setSmartCurrentLimit(40);
        turnMotor.setSmartCurrentLimit(40);

        driveEncoder = driveMotor.getEncoder();

        driveEncoder.setPositionConversionFactor(ModuleConstants.kDriveEncoderRot2Meter);
        driveEncoder.setVelocityConversionFactor(ModuleConstants.kDriveEncoderRPM2MeterPerSec);

        feedForward = new 
            SimpleMotorFeedforward(ModuleConstants.kS, ModuleConstants.kV, ModuleConstants.kA);

        driveController = driveMotor.getPIDController();

        driveController.setP(ModuleConstants.kP);
        driveController.setI(ModuleConstants.kI);
        driveController.setD(ModuleConstants.kD);
        driveController.setFF(ModuleConstants.kFF);

        turnController = new PIDController(ModuleConstants.kPTurning, 0, 0);
        turnController.enableContinuousInput(-Math.PI, Math.PI);

        Timer.delay(1.0);
        resetEncoders();

        lastAngle = getState().angle;
    }

    public double getDrivePosition(){
        return driveEncoder.getPosition();
    }

    public double getDriveVelocity(){
        return driveEncoder.getVelocity();
    }

    public double getRawAbsoluteVolts(){
        return absoluteEncoder.getAbsolutePosition();
    }

    public double getAngleDeegrees(){
        double rawAngle = 
                (absoluteEncoder.getAbsolutePosition() * 360 - absoluteEncoderOffset) % 360;
        double angle;
        if(rawAngle > 180.0 && rawAngle < 360.0){
            angle = -180 + rawAngle % 180.0;
        } else {
            angle = rawAngle;
        }

        return angle;
    }

    public double turningDeegreesToRadians(){
        return Units.degreesToRadians(getAngleDeegrees());
    }

    public void resetEncoders(){
        driveEncoder.setPosition(0);
        absoluteEncoder.reset();
    }

    public SwerveModuleState getState(){
        return new SwerveModuleState(getDriveVelocity(), 
        Rotation2d.fromDegrees(getAngleDeegrees()));
    }

    public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop){
        desiredState = 
            CANModuleOptimizer.optimize(desiredState, getState().angle);

        setAngle(desiredState);
        setSpeed(desiredState, isOpenLoop);  

        //SmartDashboard.putString("Swerve [" + moduleNumber + "] state", desiredState.toString());
    }

    public void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop){
        if(isOpenLoop){
            double percentOutput = 
                desiredState.speedMetersPerSecond / DriveConstants.kPhysicalMaxSpeedMetersPerSecond;
            driveMotor.set(percentOutput);
        } else {
            driveController.setReference(-desiredState.speedMetersPerSecond, ControlType.kVelocity, 
            0, feedForward.calculate(-desiredState.speedMetersPerSecond));
        }
    }

    public void setAngle(SwerveModuleState desiredState){
        Rotation2d angle = (Math.abs(desiredState.speedMetersPerSecond) <= 
        (DriveConstants.kPhysicalMaxSpeedMetersPerSecond * 0.01)) ? lastAngle : desiredState.angle;

        turnMotor
        .set(turnController.calculate(turningDeegreesToRadians(), desiredState.angle.getRadians()));
        lastAngle = angle;
    }

    public void lockModule(){
        double targetAngle = -45;
        turnMotor.set(turnController.calculate(targetAngle));
    }

    //Invert if needed for odometry correction
    public SwerveModulePosition getPosition(){
        return new SwerveModulePosition(
            -getDrivePosition(), 
            Rotation2d.fromDegrees(getAngleDeegrees()));
    }

    public void stop(){
        driveMotor.set(0);
        turnMotor.set(0);
    }

This is the code of our swerve module, if anyone can help us, itll be great appreciated.
Ill try to get a video later of the behaviour of the swerve

2

Have you done a test where you tell each module to go to 0º, and apply forward voltage? We saw this behavior when one of our wheels was zeroed 180º from what it should have been.

3

We had this exact problem and the only fix was changing code bases. For us, the inversion was permanent even after factory defaulting the motor. Not sure what caused the problem but it only happened while we were driving quite aggressively. To fix our issue, we migrated to different swerve code. I’m not sure if you are using BaseFalconSwerve but that was our issue. If I see this issue happening in CTRE swerve gen I will update here.

4

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.