Swerve Module Example

Here is an example implementation of a swerve module using Nova. The turn controller is configured with an external absolute encoder. You would create a swerve module by calling the constructor as follows.

// Assume module1 is defined in class scope
module1 = new NovaSwerveModule(1, 4, 77); // drive ID, turn ID, encoder offset.

Below is the swerve class definition.

public class NovaSwerveModule {
  // Constants for drive and turn motors (use your own)
  private static final double MAX_VELOCITY = 4.0; // max velocity
  private static final double GEAR_RATIO = 6.75; // gear ratio drive motor to wheel
  private static final double WHEEL_CIRCUM = .1; // wheel circumference
  
  // Thrify Novas driving NEO
  private ThriftyNova drive;
  private ThriftyNova turn;

  // Unit convertors for drive velocity, drive position, and turn
  private Conversion driveConverter; 
  private Conversion distanceConverter; 
  private Conversion turnConverter; 
  
  // Store the turn encoder offset
  private double turnEncoderOffset;
  
  // Construct swerve module
  private NovaSwerveModule(int driveId, int turnId, double turnEncoderOffset) {
    this.turnEncoderOffset = turnEncoderOffset;
    initializeDriveMotor(driveId);
    initializeTurnMotor(turnId);
  }

  // Construct and configure turn motor
  private void initializeDriveMotor(int driveId) {

    // For drive we need to convert between RPM and internal NEO encoder units
    driveConverter = new Conversion(VelcotiyUnit.ROTATIONS_PER_MIN, EncoderType.INTERNAL);
    distanceConverter = new Conversion(PositionUnit.ROTATIONS, EncoderType.INTERNAL);

    drive = new ThriftyNova(driveId) // create with correct CAN ID
      .setBrakeMode(true) // brake mode
      .setInverted(false) // not inverted 
      .setRampUp(0.1)    // small ramp up used during auto 
      .setRampDown(0.1)  // same for ramp down
      .setMaxOutput(1, 1)  // full power because drive motor
      .setMaxCurrent(CurrentType.SUPPLY, 80) // set a 80amp current limit
                                             // on supply side
      .useEncoderType(EncoderType.INTERNAL) // use internal NEO encoder
      .usePIDSlot(PIDSlot.SLOT0)            // use the first PID slot
      
      // Configure the first PID slot (examples, please tune your own values)
      .pid0.setP(0.5) 
      .pid0.setI(0)
      .pid0.setD(0)
      .pid0.setFF(1.2);
      
    // Iterate through errors and check them
    for (var err : drive.getErrors()) {
    // The user can handle the errors
      System.err.println("Drive Motor Config Error: " + err.toString());
    }
    // Clear errors here
    drive.clearErrors();
  }

  // Construct and configure turn motor
  private void initializeTurnMotor(int turnId) {

    // For drive we need to convert between radians and external absolute encoder units
    turnConverter = new Conversion(PositionUnit.RADIANS, EncoderType.ABS);

    turn = new ThriftyNova(turnId) // create with correct CAN ID
      .setBrakeMode(true) // brake mode
      .setInverted(false) // not inverted 
      .setMaxOutput(1, 1)  // full power because we sometimes want to
                           // change direction quickly
      .setMaxCurrent(CurrentType.SUPPLY, 35) // set a 35amp current limit
                                             // on supply side
      .useEncoderType(EncoderType.ABS)      // use external absolute encoder
      .usePIDSlot(PIDSlot.SLOT0)            // use the first PID slot
      
      // Configure the first PID slot (examples, please tune your own values)
      .pid0.setP(0.5) 
      .pid0.setI(0)
      .pid0.setD(0)
      .pid0.setFF(1.2);
      
    // Iterate through errors and check them
    for (var err : turn.getErrors()) {
    // The user can handle the errors
      System.err.println("Drive Motor Config Error: " + err.toString());
    }
    // Clear errors here
    turn.clearErrors();
  }
  
  
  // This method is called periodically
  public void getState() {
    // Set the position based on the setpoint
    pivot.setPosition(convertor.toMotor(setPoint.pos / inchesPerRad));
    
    // Get the real position from the encoder
    double realPosition = convertor.fromMotor(pivot.getPosition()) * inchesPerRad;
    
    // Log the real position and the current draw
    SmartDashboard.putNumber("Elevator Position", realPosition); 
    SmartDashboard.putNumber("Elevator Current", pivot.getCurrent()); 
  }

  // Get position, required for swerve
  public SwerveModulePosition getPosition() {
    // Get the position of the drive motor and use the distance convertor to get the real position
    double distance = distanceConverter.fromMotor((drive.getPosition() * WHEEL_CIRCUM) / GEAR_RATIO);
    // Return the position object using the distance and rotation
    return new SwerveModulePosition(distance, getRotation());
  }

  // Get rotation, required for swerve
  public Rotation2d getRotation() {
    // Return the current rotation of the swerve rotation
    return Rotation2d.fromRadians(turnConverter.fromMotor(turn.getPosition()) - turnEncoderOffset);
  }

  // Set desired state, required for swerve
  public void setDesiredState(SwerveModuleState state) {
    // Optimize the state
    final SwerveModuleState optimized = SwerveModuleState.optimize(state, getRotation());

    // Output the desired velocity on the drive motor
    if (DriverStation.isAutonomous()) {
      // Calculate the motor velocity input
      double driveVelocity = driveConvertor.toMotor((optimized.speedMetersPerSecond * GEAR_RATIO) / WHEEL_CIRCUM);
      drive.setVelocity(driveVelocity);
    } else {
      // Calculate the motor power input
      drive.setPercent(optimized.speedMetersPerSecond / MAX_VELOCITY);
    }

    // Calculate the turn position input
    double turnPosition = turnConvertor.toMotor(optimized.angle.getRadians());
    // Output the desired angle on the turn motor
    turn.setPosition(turnPosition);
  }
}