From 42f10690f8e9ec613a818bfaddad08bc271a8d90 Mon Sep 17 00:00:00 2001 From: Emma Date: Wed, 17 Jan 2024 21:59:33 +0200 Subject: [PATCH] Rearrange function order to be more consistent --- src/main/java/frc/robot/swerve/ModuleIO.java | 27 ++++------ .../java/frc/robot/swerve/ModuleIOSim.java | 9 ++-- .../frc/robot/swerve/ModuleIOSparkMax.java | 53 +++++++++++-------- .../frc/robot/swerve/ModuleIOTalonFX.java | 44 ++++++++------- src/main/java/lib/PhoenixOdometryThread.java | 3 +- 5 files changed, 68 insertions(+), 68 deletions(-) diff --git a/src/main/java/frc/robot/swerve/ModuleIO.java b/src/main/java/frc/robot/swerve/ModuleIO.java index 5f956ef2..eb5ce008 100644 --- a/src/main/java/frc/robot/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/swerve/ModuleIO.java @@ -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); @@ -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(); - } } diff --git a/src/main/java/frc/robot/swerve/ModuleIOSim.java b/src/main/java/frc/robot/swerve/ModuleIOSim.java index 66dfb102..8a2199e5 100644 --- a/src/main/java/frc/robot/swerve/ModuleIOSim.java +++ b/src/main/java/frc/robot/swerve/ModuleIOSim.java @@ -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(); diff --git a/src/main/java/frc/robot/swerve/ModuleIOSparkMax.java b/src/main/java/frc/robot/swerve/ModuleIOSparkMax.java index f89085d5..c981c2c2 100644 --- a/src/main/java/frc/robot/swerve/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/swerve/ModuleIOSparkMax.java @@ -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, @@ -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); @@ -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(); @@ -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 @@ -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( @@ -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(); } diff --git a/src/main/java/frc/robot/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/swerve/ModuleIOTalonFX.java index 3ea77c40..29679ab4 100644 --- a/src/main/java/frc/robot/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/swerve/ModuleIOTalonFX.java @@ -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; @@ -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 distanceQueue; + private final Queue angleQueue; private Rotation2d angleSetpoint = new Rotation2d(); private Rotation2d currentAngle = new Rotation2d(); private double driveMotorVelocitySetpoint = 0; - private final Queue distanceQueue; - private final Queue angleQueue; public ModuleIOTalonFX( int driveMotorID, @@ -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(); @@ -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); } @@ -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(); diff --git a/src/main/java/lib/PhoenixOdometryThread.java b/src/main/java/lib/PhoenixOdometryThread.java index b1fc98ff..0a30959c 100644 --- a/src/main/java/lib/PhoenixOdometryThread.java +++ b/src/main/java/lib/PhoenixOdometryThread.java @@ -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