Skip to content

Commit

Permalink
Rearrange function order to be more consistent
Browse files Browse the repository at this point in the history
  • Loading branch information
@Emma03L
Emma03L committed Jan 17, 2024
1 parent 7962380 commit 42f1069
Show file tree
Hide file tree
Showing 5 changed files with 68 additions and 68 deletions.
27 changes: 11 additions & 16 deletions src/main/java/frc/robot/swerve/ModuleIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,21 @@
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import lib.webconstants.LoggedTunableNumber;

public interface ModuleIO {
void updateInputs(SwerveModuleInputs inputs);

void updatePID();

default boolean hasPIDChanged(LoggedTunableNumber[] PIDValues) {
boolean hasChanged = false;
for (LoggedTunableNumber value : PIDValues) {
if (value.hasChanged(hashCode())) hasChanged = true;
}
return hasChanged;
}

Rotation2d getAngle();

void setAngle(Rotation2d angle);
Expand All @@ -18,31 +27,17 @@ public interface ModuleIO {

void setVelocity(double velocity);

void setAngleVelocity(double velocity);

SwerveModuleState getModuleState();

SwerveModulePosition getModulePosition();

void stop();

void updatePID();

default boolean hasPIDChanged(LoggedTunableNumber[] PIDValues) {
boolean hasChanged = false;
for (LoggedTunableNumber value : PIDValues) {
if (value.hasChanged(hashCode())) hasChanged = true;
}
return hasChanged;
}
Command checkModule();

default void updateOffset(Rotation2d offset) {}

default boolean encoderConnected() {
return true;
}

default Command checkModule() {
return Commands.none();
}
}
9 changes: 5 additions & 4 deletions src/main/java/frc/robot/swerve/ModuleIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,18 +7,19 @@
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import lib.math.AngleUtil;
import lib.motors.TalonFXSim;
import lib.units.Units;

public class ModuleIOSim implements ModuleIO {
private final TalonFXSim driveMotor;
private final TalonFXSim angleMotor;

private final PIDController velocityController;
private final PIDController angleController;
private VelocityVoltage driveControlRequest = new VelocityVoltage(0).withEnableFOC(true);
private PositionVoltage angleControlRequest = new PositionVoltage(0).withEnableFOC(true);

private final PIDController angleController;
private final PIDController velocityController;
private double currentVelocity = 0;
private double velocitySetpoint = 0;
private Rotation2d currentAngle = new Rotation2d();
Expand Down
53 changes: 30 additions & 23 deletions src/main/java/frc/robot/swerve/ModuleIOSparkMax.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,11 +37,7 @@ public class ModuleIOSparkMax implements ModuleIO {
private double moduleDistance;
private double driveMotorSetpoint;

private SimpleMotorFeedforward feedforward =
new SimpleMotorFeedforward(
SwerveConstants.DRIVE_KS.get(),
SwerveConstants.DRIVE_KV.get(),
SwerveConstants.DRIVE_KA.get());
private SimpleMotorFeedforward feedforward;

public ModuleIOSparkMax(
int driveMotorID,
Expand All @@ -56,14 +52,15 @@ public ModuleIOSparkMax(

this.encoder = new DutyCycleEncoder(encoderID);

updatePID();

driveMotor.restoreFactoryDefaults();
drivePIDController = driveMotor.getPIDController();
driveEncoder = driveMotor.getEncoder();

driveMotor.enableVoltageCompensation(SwerveConstants.VOLT_COMP_SATURATION);
driveMotor.setIdleMode(CANSparkMax.IdleMode.kBrake);
driveMotor.setSmartCurrentLimit(
(int) SwerveConstants.NEO_CURRENT_LIMIT);
driveMotor.setSmartCurrentLimit((int) SwerveConstants.NEO_CURRENT_LIMIT);
driveMotor.setInverted(driveInverted);
driveEncoder.setPositionConversionFactor(SwerveConstants.DRIVE_REDUCTION);
driveEncoder.setVelocityConversionFactor(SwerveConstants.DRIVE_REDUCTION);
Expand All @@ -75,13 +72,8 @@ public ModuleIOSparkMax(

angleMotor.enableVoltageCompensation(SwerveConstants.VOLT_COMP_SATURATION);
angleMotor.setIdleMode(CANSparkMax.IdleMode.kBrake);
angleMotor.setSmartCurrentLimit(
(int) SwerveConstants.NEO_550_CURRENT_LIMIT);
angleMotor.setSmartCurrentLimit((int) SwerveConstants.NEO_550_CURRENT_LIMIT);
angleMotor.setInverted(angleInverted);
anglePIDController.setP(motionMagicConfigs[0]);
anglePIDController.setI(motionMagicConfigs[1]);
anglePIDController.setD(motionMagicConfigs[2]);
anglePIDController.setFF(motionMagicConfigs[3]);
angleEncoder.setPositionConversionFactor(SwerveConstants.ANGLE_REDUCTION);
angleEncoder.setVelocityConversionFactor(SwerveConstants.ANGLE_REDUCTION);
angleMotor.burnFlash();
Expand Down Expand Up @@ -121,6 +113,21 @@ public void updateInputs(SwerveModuleInputs inputs) {
inputs.moduleDistance =
inputs.driveMotorPosition * SwerveConstants.WHEEL_DIAMETER * Math.PI;
moduleDistance = inputs.moduleDistance;

if (hasPIDChanged(SwerveConstants.PID_VALUES)) updatePID();
}

@Override
public void updatePID() {
feedforward =
new SimpleMotorFeedforward(
SwerveConstants.DRIVE_KS.get(),
SwerveConstants.DRIVE_KV.get(),
SwerveConstants.DRIVE_KA.get());
anglePIDController.setP(SwerveConstants.ANGLE_KP.get());
anglePIDController.setI(SwerveConstants.ANGLE_KI.get());
anglePIDController.setD(SwerveConstants.ANGLE_KD.get());
anglePIDController.setFF(SwerveConstants.ANGLE_KS.get());
}

@Override
Expand Down Expand Up @@ -161,22 +168,12 @@ public SwerveModulePosition getModulePosition() {
return new SwerveModulePosition(moduleDistance, getAngle());
}

@Override
public void updateOffset(Rotation2d offset) {
angleEncoder.setPosition(getEncoderAngle() - offset.getRotations());
}

@Override
public void stop() {
driveMotor.stopMotor();
angleMotor.stopMotor();
}

@Override
public boolean encoderConnected() {
return encoder.isConnected();
}

@Override
public Command checkModule() {
return Commands.run(
Expand All @@ -186,6 +183,16 @@ public Command checkModule() {
});
}

@Override
public void updateOffset(Rotation2d offset) {
angleEncoder.setPosition(getEncoderAngle() - offset.getRotations());
}

@Override
public boolean encoderConnected() {
return encoder.isConnected();
}

private double getEncoderAngle() {
return 1.0 - encoder.getAbsolutePosition();
}
Expand Down
44 changes: 21 additions & 23 deletions src/main/java/frc/robot/swerve/ModuleIOTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,13 +13,14 @@
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import java.util.List;
import java.util.Queue;
import lib.PhoenixOdometryThread;
import lib.math.AngleUtil;
import lib.math.differential.Integral;
import lib.units.Units;

import java.util.List;
import java.util.Queue;

public class ModuleIOTalonFX implements ModuleIO {

private final TalonFX driveMotor;
Expand All @@ -37,11 +38,11 @@ public class ModuleIOTalonFX implements ModuleIO {
new PositionVoltage(0).withEnableFOC(true).withSlot(0);
private final VelocityVoltage velocityControlRequest =
new VelocityVoltage(0).withEnableFOC(true);
private final Queue<Double> distanceQueue;
private final Queue<Double> angleQueue;
private Rotation2d angleSetpoint = new Rotation2d();
private Rotation2d currentAngle = new Rotation2d();
private double driveMotorVelocitySetpoint = 0;
private final Queue<Double> distanceQueue;
private final Queue<Double> angleQueue;

public ModuleIOTalonFX(
int driveMotorID,
Expand Down Expand Up @@ -92,15 +93,6 @@ public ModuleIOTalonFX(
angleVelocitySignal);
}

@Override
public Command checkModule() {
return Commands.run(
() -> {
driveMotor.set(0.8);
angleMotor.set(0.2);
});
}

@Override
public void updateInputs(SwerveModuleInputs inputs) {
inputs.absolutePosition = encoder.getAbsolutePosition();
Expand Down Expand Up @@ -201,9 +193,7 @@ public void setVelocity(double velocity) {
driveMotorVelocitySetpoint = velocity;

velocityControlRequest
.withVelocity(
Units.metersToRotations(
velocity, SwerveConstants.WHEEL_DIAMETER / 2))
.withVelocity(Units.metersToRotations(velocity, SwerveConstants.WHEEL_DIAMETER / 2))
.withEnableFOC(true);
driveMotor.setControl(velocityControlRequest);
}
Expand All @@ -217,22 +207,30 @@ public SwerveModuleState getModuleState() {
public SwerveModulePosition getModulePosition() {
return new SwerveModulePosition(
Units.rpsToMetersPerSecond(
driveMotor.getPosition().getValue(),
SwerveConstants.WHEEL_DIAMETER / 2),
driveMotor.getPosition().getValue(), SwerveConstants.WHEEL_DIAMETER / 2),
getAngle());
}

@Override
public void updateOffset(Rotation2d offset) {
angleMotor.setPosition(encoder.getAbsolutePosition() - offset.getRotations());
}

@Override
public void stop() {
driveMotor.stopMotor();
angleMotor.stopMotor();
}

@Override
public Command checkModule() {
return Commands.run(
() -> {
driveMotor.set(0.8);
angleMotor.set(0.2);
});
}

@Override
public void updateOffset(Rotation2d offset) {
angleMotor.setPosition(encoder.getAbsolutePosition() - offset.getRotations());
}

@Override
public boolean encoderConnected() {
return encoder.isConnected();
Expand Down
3 changes: 1 addition & 2 deletions src/main/java/lib/PhoenixOdometryThread.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,8 +82,7 @@ public void run() {
signalsLock.lock();
try {
if (isCANFD) {
BaseStatusSignal.waitForAll(
2.0 / SwerveConstants.ODOMETRY_FREQUENCY, allArr);
BaseStatusSignal.waitForAll(2.0 / SwerveConstants.ODOMETRY_FREQUENCY, allArr);
} else {
// "waitForAll" does not support blocking on multiple
// signals with a bus that is not CAN FD, regardless
Expand Down

0 comments on commit 42f1069

Please sign in to comment.