Adapting 364's base swerve drive code to use NEOs for steering

[Nailuj]

Our team has been working on creating a swerve drive, and we’ve heard that 364’s BaseFalconSwerve is the best option. However, we built our modules with NEOs for steering and Falcons for driving. How can we adapt the code to use NEOs for steer or are we better off sticking with the SDS template code? This is our first time using java; we are typically a LabVIEW team.

[DaMoose3512]

We use NEOs for both steering and driving, but it can also apply here just for steering.

You will just need to create a SparkMaxPIDController object, and register it to your steer motor. Then you can run:
turningController.setReference(angle);

You’ll have to adapt the Falcon steer conversions as NEO rotations are different than Falcon rotations, but I’ve found using setPositionConversionFactor() to work out better.

Code if you’re interested: https://github.com/frc3512/SwerveBot-2022

[tecnicraze]

Hi, I ported the swerve code to all neo motors for our team. Here is our Swerve Module Class. I would recommend staying away from the template as it is known to be buggy.

package frc.robot;

import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModuleState;

import frc.lib.util.ModuleStateOptimizer;
import frc.lib.util.SwerveModuleConstants;

import com.ctre.phoenix.sensors.CANCoder;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxPIDController;
import com.revrobotics.CANSparkMax.ControlType;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;

public class SwerveModule {
public int moduleNumber;
private Rotation2d angleOffset;
private Rotation2d lastAngle = Rotation2d.fromDegrees(0);

private CANSparkMax mAngleMotor;
private CANSparkMax mDriveMotor;
private CANCoder absoluteEncoder;

private RelativeEncoder mDriveEncoder;
private RelativeEncoder mAngleEncoder;

private SparkMaxPIDController mDrivePIDController;
private SparkMaxPIDController mAnglePIDController;

SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA);

/* Sim Caches (basically im lazy and don't want to use the rev physics sim) */
private double simSpeedCache;
private Rotation2d simAngleCache = Rotation2d.fromDegrees(0);

public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants){
    this.moduleNumber = moduleNumber;
    this.angleOffset = moduleConstants.angleOffset;
    
    /* Absolute Encoder */
    absoluteEncoder = new CANCoder(moduleConstants.cancoderID);
    configAngleEncoder();

    /* Angle Motor */
    mAngleMotor = new CANSparkMax(moduleConstants.angleMotorID, MotorType.kBrushless);
    mAngleEncoder = mAngleMotor.getEncoder();
    mAnglePIDController = mAngleMotor.getPIDController();
    configAngleMotor();

    /* Drive motor */
    mDriveMotor = new CANSparkMax(moduleConstants.driveMotorID, MotorType.kBrushless);
    mDriveEncoder = mDriveMotor.getEncoder();
    mDrivePIDController = mDriveMotor.getPIDController();
    configDriveMotor();

    lastAngle = getState().angle;
}

public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop){
    desiredState = ModuleStateOptimizer.optimize(desiredState, getState().angle); //Custom optimize command, since default WPILib optimize assumes continuous controller which CTRE is not

    setAngle(desiredState);
    setSpeed(desiredState, isOpenLoop);
    simSpeedCache = desiredState.speedMetersPerSecond;
    simAngleCache = desiredState.angle;
}

private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop){
    if(isOpenLoop){
        double percentOutput = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed;
        // mDriveMotor_ctre.set(ControlMode.PercentOutput, percentOutput);
        mDriveMotor.set(percentOutput);
    }
    else {
        mDriveMotor.getPIDController().setReference(desiredState.speedMetersPerSecond, ControlType.kVelocity, 0, feedforward.calculate(desiredState.speedMetersPerSecond));
        // mDriveMotor.set(ControlMode.Velocity, desiredState.speedMetersPerSecond, DemandType.ArbitraryFeedForward, feedforward.calculate(desiredState.speedMetersPerSecond));
    }
}

private void setAngle(SwerveModuleState desiredState){
    Rotation2d angle = (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.Swerve.maxSpeed * 0.01)) ? lastAngle : desiredState.angle; //Prevent rotating module if speed is less than 1%. Prevents Jittering.
    // mAngleMotor_ctre.set(ControlMode.Position, Conversions.degreesToFalcon(angle.getDegrees(), Constants.DriveSubsystem.angleGearRatio));
    mAngleMotor.getPIDController().setReference(angle.getDegrees(), ControlType.kPosition);
    lastAngle = angle;
}

private Rotation2d getAngle(){
    if (Robot.isReal()) return Rotation2d.fromDegrees(mAngleEncoder.getPosition());
    return simAngleCache; // If sim.
}

public Rotation2d getAbsoluteAngle(){
    return Rotation2d.fromDegrees(absoluteEncoder.getAbsolutePosition());
}

private void resetToAbsolute(){
    mAngleEncoder.setPosition(getAbsoluteAngle().getDegrees() - angleOffset.getDegrees());
}

private void configAngleEncoder(){        
    absoluteEncoder.configFactoryDefault();
    absoluteEncoder.configAllSettings(Robot.ctreConfigs.swerveCanCoderConfig);
}

private void configAngleMotor() {
    mAngleMotor.restoreFactoryDefaults();
    mAngleMotor.setSmartCurrentLimit(Constants.Swerve.angleSmartCurrentLimit);
    mAngleMotor.setSecondaryCurrentLimit(Constants.Swerve.angleSecondaryCurrentLimit);
    mAngleMotor.setInverted(Constants.Swerve.angleMotorInvert);
    mAngleMotor.setIdleMode(Constants.Swerve.angleNeutralMode);
    
    mAngleEncoder.setPositionConversionFactor((1/Constants.Swerve.chosenModule.angleGearRatio) // We do 1 over the gear ratio because 1 rotation of the motor is < 1 rotation of the module
            * 360); // 1/360 rotations is 1 degree, 1 rotation is 360 degrees.
    resetToAbsolute();

    mAnglePIDController.setP(Constants.Swerve.angleKP);
    mAnglePIDController.setI(Constants.Swerve.angleKI);
    mAnglePIDController.setD(Constants.Swerve.angleKD);
    mAnglePIDController.setFF(Constants.Swerve.angleKF);
}

private void configDriveMotor(){       
    mDriveMotor.restoreFactoryDefaults();
    mDriveMotor.setSmartCurrentLimit(Constants.Swerve.driveSmartCurrentLimit);
    mDriveMotor.setSecondaryCurrentLimit(Constants.Swerve.driveSecondaryCurrentLimit);
    mDriveMotor.setInverted(Constants.Swerve.driveMotorInvert);
    mDriveMotor.setIdleMode(Constants.Swerve.driveNeutralMode);
    mDriveMotor.setOpenLoopRampRate(Constants.Swerve.openLoopRamp);
    mDriveMotor.setClosedLoopRampRate(Constants.Swerve.closedLoopRamp);

    mDriveEncoder.setVelocityConversionFactor(1/Constants.Swerve.chosenModule.driveGearRatio // 1/gear ratio because the wheel spins slower than the motor.
            * Constants.Swerve.chosenModule.wheelCircumference // Multiply by the circumference to get meters per minute
            / 60); // Divide by 60 to get meters per second.
    mDriveEncoder.setPosition(0);

    mDrivePIDController.setP(Constants.Swerve.driveKP);
    mDrivePIDController.setI(Constants.Swerve.driveKI);
    mDrivePIDController.setD(Constants.Swerve.driveKD);
    mDrivePIDController.setFF(Constants.Swerve.driveKF); // Not actually used because we specify our feedforward when we set our speed.

}

public SwerveModuleState getState(){
    return new SwerveModuleState(
        Robot.isReal() ? mDriveEncoder.getVelocity() : simSpeedCache,
        getAngle()
    ); 
}
}

[jimfl014]

Hi! We (Team 5000) bit the bullet and invested in the swerve drive kit through Andymark using Falcons. We found this post and used 364’s code as our base. We understand the code, we understand the process to tune the PID, and have verified functionality of each individual module; however after 3 hours of testing our results are chaotic.

Is there anyone at 364 we could contact to discuss? As we have never done this we have no baseline on how this process should go or how it should operate properly when it is tuned. One of many questions is how do the wheels reset themselves between each match, or enable-disable, without manually turning them?

[tecnicraze]

Hello, I’m not sure witch andymark modules you are using, a link would be nice. For the robot to know which way the modules are facing, you need absolute encoders (something like ctre cancoder) on the modules, these maintain position between robot power cycles.

Could you post here what your results were, it is somewhat difficult to give possible solutions if you don’t give us the problem.

[jimfl014]

We are using the MK4i modules with 8.14:1 drive ratio

Also, we are using CANcoders, on a CANivore with the Falcons and a Pigeon 2.0.

All the wheelbase, and encoder offsets entered into our Constants class along with our test PID values are entered

We start out with the wheel bevel gears facing the same direction. The robot will go back and forth relatively straight. Once we start turning the wheels develop a mind of their own. We adjust the PID values to get to oscillation, increase and try again, (after setting the wheels back straight manually) and the wheels once again go in different directions. We have also noticed that sometimes after we disable and reset the wheels manually then enable, at least two opposing wheels will cant a few degrees.

I’m going to have our Lead Coder reply as well with more details

The logs are clean of any errors.

[MaxMcKelvey]

The alignment of the wheels seems to be sporadic from test to test. Sometimes, the robot moves straight perfectly, and then once it tries to go left or right the alignment of some wheels is entirely off. Other times, the wheels go in opposite directions for seemingly no reason.
It seems to me like the more we increase the PID, the more the wheels fight each other. When we decrease them, however, the alignment of the wheels is completely off regardless of where they start.

[nstrike]

Run a self-test snapshot on your CANCoders and ensure the field is “Ideal” using the Pheonix Tuner. Sounds like a CANCoder problem to me.

[jimfl014]

Thanks! We will do it at our next meeting tomorrow afternoon

[dwwright]

5419 built an Offseason Swerve drive (Mk4i) using the hybrid Neo(steering) Falcon(drive) set up and competed with it at two offseason events.

I don’t know that much about the code but it is here (https://github.com/team5419/frc-2022.git)

I can pass on specific questions to the students if you have any

DW

[tecnicraze]

Could you post a link to your code here?

Make sure all your encoders are set up to absolute in phoenix tuner and post the values of the encoders to shuffleboard to make sure they are as expected.

By saying the wheels go in different directions, are they facing the correct way but fighting in direction, or are they just facing the wrong way?

[tecnicraze]

also, make sure your offsets are correct.

[jimfl014]

Here is the code we are usingRobotContainer.java (2.6 KB) Main.java (775 Bytes) Constants.java (6.2 KB) Robot.java (3.3 KB) CTREConfigs.java (2.9 KB) SwerveModule.java (4.3 KB)

[jimfl014]

Yesterday we verified all the CANcorder settings in Tuner. We also verified that each CANCoder angle changed appropriately when the wheels were manually turned; meaning they all know right from left. We also lifted the robot and turned it while observing the Pigeon 2 yaw angle; once again it know right from left.

[jimfl014]

The robot is back heavy due to the battery placement. We briefly put another battery in the front to even off the weight; at the time we did not invest much time tuning.