Swerve help

Technical
, , ,
1

we just tested our swerve drive code but there is an issue with turning. we can go forward backward left right just fine but we can turn. i use advantage kit and below are the datas i got. in advantage scope, its supposed to be but irl the wheels positions are wrong

IMG_6560
IMG_6560638Ă—1381 154 KB

IMG_6559
IMG_65593024Ă—4032 2.96 MB

IMG_6558
IMG_65583024Ă—4032 2.84 MB

2

This looks like you might have a motor or sensor inverted incorrectly. What swerve modules do you have and do you have a link to your code?

1 Like
3

I think your offsets might be wrong.

Make sure to point all of your wheels forward, and look at the offsets recorded in advantagescope.

Then, you should do encoder.setPosition() (Or manually store offsets within IO implementations) in order to make the encoder readings correct.

4

package frc.robot.subsystems.drive;

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.hardware.CANcoder;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;

/**

  • To calibrate the absolute encoder offsets, point the modules straight (such that forward motion
  • on the drive motor will propel the robot forward) and copy the reported values from the absolute
  • encoders using AdvantageScope. These values are logged under
  • “/Drive/ModuleX/TurnAbsolutePositionRad”
    */
    public class ModuleIOSparkMax implements ModuleIO {
    // Gear ratios for SDS MK4i L3, adjust as necessary
    private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0);
    private static final double TURN_GEAR_RATIO = 150 / 7;

private final CANSparkMax driveSparkMax;
private final CANSparkMax turnSparkMax;

private final RelativeEncoder driveEncoder;
private final RelativeEncoder turnRelativeEncoder;
private final CANcoder cancoder;
private final Rotation2d absoluteEncoderOffset;
private final StatusSignal turnAbsolutePosition;

private final boolean isTurnMotorInverted = true;

public ModuleIOSparkMax(int index) {
switch (index) {
case 0:
driveSparkMax = new CANSparkMax(1, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(2, MotorType.kBrushless);
cancoder = new CANcoder(9);
absoluteEncoderOffset = new Rotation2d(2.269); // MUST BE CALIBRATED

    break;
  case 1:
    driveSparkMax = new CANSparkMax(3, MotorType.kBrushless);
    turnSparkMax = new CANSparkMax(4, MotorType.kBrushless);
    cancoder = new CANcoder(10);
    absoluteEncoderOffset = new Rotation2d(1.281); // MUST BE CALIBRATED
    // MUST BE CALIBRATED
    break;
  case 2:
    driveSparkMax = new CANSparkMax(5, MotorType.kBrushless);
    turnSparkMax = new CANSparkMax(6, MotorType.kBrushless);
    cancoder = new CANcoder(11);
    absoluteEncoderOffset = new Rotation2d(1.083); // MUST BE CALIBRATED
    // MUST BE CALIBRATED
    break;
  case 3:
    driveSparkMax = new CANSparkMax(7, MotorType.kBrushless);
    turnSparkMax = new CANSparkMax(8, MotorType.kBrushless);
    cancoder = new CANcoder(12);
    absoluteEncoderOffset = new Rotation2d(0.529); // MUST BE CALIBRATED
    break;
  default:
    throw new RuntimeException("Invalid module index");
}

driveSparkMax.restoreFactoryDefaults();
turnSparkMax.restoreFactoryDefaults();

driveSparkMax.setCANTimeout(250);
turnSparkMax.setCANTimeout(250);

driveEncoder = driveSparkMax.getEncoder();
turnRelativeEncoder = turnSparkMax.getEncoder();
turnAbsolutePosition = cancoder.getAbsolutePosition();

turnSparkMax.setInverted(isTurnMotorInverted);
driveSparkMax.setSmartCurrentLimit(40);
turnSparkMax.setSmartCurrentLimit(30);
driveSparkMax.enableVoltageCompensation(12.0);
turnSparkMax.enableVoltageCompensation(12.0);

driveEncoder.setPosition(0.0);
driveEncoder.setMeasurementPeriod(10);
driveEncoder.setAverageDepth(2);

turnRelativeEncoder.setPosition(0.0);
turnRelativeEncoder.setMeasurementPeriod(10);
turnRelativeEncoder.setAverageDepth(2);

driveSparkMax.setCANTimeout(0);
turnSparkMax.setCANTimeout(0);

driveSparkMax.burnFlash();
turnSparkMax.burnFlash();

cancoder.getConfigurator().apply(new CANcoderConfiguration());

}

@Override
public void updateInputs(ModuleIOInputs inputs) {

BaseStatusSignal.refreshAll(turnAbsolutePosition);

inputs.drivePositionRad =
    Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO;
inputs.driveVelocityRadPerSec =
    Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO;
inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage();
inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()};

inputs.turnAbsolutePosition =
    Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble())
        .minus(absoluteEncoderOffset);

inputs.turnPosition =
    Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO);
inputs.turnVelocityRadPerSec =
    Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity())
        / TURN_GEAR_RATIO;
inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage();
inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()};

}

@Override
public void setDriveVoltage(double volts) {
driveSparkMax.setVoltage(volts);
}

@Override
public void setTurnVoltage(double volts) {
turnSparkMax.setVoltage(volts);
}

@Override
public void setDriveBrakeMode(boolean enable) {
driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast);
}

@Override
public void setTurnBrakeMode(boolean enable) {
turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast);
}
}

5

my thougt aswell but going forward doward left right sideways works just fine

6

also we use sds MK4i modules

7

Two things before I even take a good look through code:

  1. any time you’re posting raw code in CD (or on discord), put “```” before and after the code block to make it easier for everyone else to read
  2. Is there a reason why you’re not storing your code on GitHub? even if you’re the only programmer on your team you still should have some kind of version management software to keep backups of your code.
8

Are your IDs correct and your swerve modules instantiated in the right order? Both Rev and CTRE tuners allow one to check which ID corresponds to which motor controller by a button that briefly changes the status light.

9
10

can you expand on that please i think our ids are correct but what do you mean initiated in right order

11

Not sure about YAGSL but in 364 base the swerve modules are numbered 0-3. 0 is the front left module, 1 is the front right and the other two continue this counterclockwise pattern. Idk just a thought

13

If you view measured states compared to desired states, what’s the difference between the two? You can compare two sets of SwerveModulePositions on AdvantageScope, and the AdvantageKitTemplate that you’re using has both being logged.

14

This also might not entirely fix your issue, but add a Timer.sleep(0.5) in your moduleIO file after your initiate any motors/sensors, but before you do any config.

1 Like
15

will check that tmrw

16

there arent any differences . The second and third pics shows the setpoints optimized and measured

17

Could the CAN IDs for front right and back right just be flipped?

1 Like
18

that might be the issue. I left the workshop today and will check that tomorrow.

19

Double check your module coordinates. Make sure the x and Y measurements and positive and negatives follow what your serve object wants. We had a simmer issue last year and we basically had x, y coordinates and negatives mixed up which was making our front left actually be our back left or something along those lines. I can remember the x and y directions but it wasn’t what we thought. Think x is front to back and y is side to side but don’t quote me

20

fixed the issue our arrangments were wrong.

1 Like