From 7a84cd28b0496910aa6b0359b88b95412a37f3c3 Mon Sep 17 00:00:00 2001 From: GreenTomato5 Date: Mon, 18 Nov 2024 18:23:14 -0600 Subject: [PATCH] Z Willy is the best (on Z Willy) --- src/main/java/frc/robot/Constants.java | 8 ++-- .../SwerveAbsoluteEncoderIOCANcoder.java | 43 ++++++++++--------- .../pioneersLib/bumSwerve/SwerveModule.java | 5 ++- .../bumSwerve/SwerveMotor/SwerveMotorIO.java | 2 +- .../SwerveMotor/SwerveMotorIONeoSim.java | 4 +- .../SwerveMotor/SwerveMotorIOSparkMax.java | 4 +- .../SwerveMotor/SwerveMotorIOTalonFX.java | 5 ++- .../SwerveMotor/SwerveMotorIOTalonFXSim.java | 4 +- 8 files changed, 40 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 759b6c7..64a7469 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -174,22 +174,22 @@ public static final class Real { new SwerveModule( new SwerveMotorIOTalonFX(59, CAN_BUS, DRIVE_BASE.driveGearing), new SwerveMotorIOTalonFX(8, CAN_BUS, DRIVE_BASE.azimuthGearing), - new SwerveAbsoluteEncoderIOCANcoder(3, 0), + new SwerveAbsoluteEncoderIOCANcoder(3, Units.radiansToDegrees(0.206)), "FrontLeft"), new SwerveModule( new SwerveMotorIOTalonFX(56, CAN_BUS, DRIVE_BASE.driveGearing), new SwerveMotorIOTalonFX(4, CAN_BUS, DRIVE_BASE.azimuthGearing), - new SwerveAbsoluteEncoderIOCANcoder(6, 0), + new SwerveAbsoluteEncoderIOCANcoder(6, -Units.radiansToDegrees(2.772)), "FrontRight"), new SwerveModule( new SwerveMotorIOTalonFX(11, CAN_BUS, DRIVE_BASE.driveGearing), new SwerveMotorIOTalonFX(2, CAN_BUS, DRIVE_BASE.azimuthGearing), - new SwerveAbsoluteEncoderIOCANcoder(12, 0), + new SwerveAbsoluteEncoderIOCANcoder(12, Units.radiansToDegrees(2.398)), "BackLeft"), new SwerveModule( new SwerveMotorIOTalonFX(58, CAN_BUS, DRIVE_BASE.driveGearing), new SwerveMotorIOTalonFX(5, CAN_BUS, DRIVE_BASE.azimuthGearing), - new SwerveAbsoluteEncoderIOCANcoder(9, 0), + new SwerveAbsoluteEncoderIOCANcoder(9, -Units.radiansToDegrees(1.934)), "BackRight") }; } diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIOCANcoder.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIOCANcoder.java index 1e6ee65..6e19a0d 100644 --- a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIOCANcoder.java +++ b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveAbsoluteEncoder/SwerveAbsoluteEncoderIOCANcoder.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Unit; import edu.wpi.first.wpilibj.CAN; public class SwerveAbsoluteEncoderIOCANcoder implements SwerveAbsoluteEncoderIO { @@ -20,7 +21,7 @@ public class SwerveAbsoluteEncoderIOCANcoder implements SwerveAbsoluteEncoderIO private double absoluteEncoderOffset; private boolean inverted; - // private StatusSignal turnAbsolutePosition; + private StatusSignal turnAbsolutePosition; private CANcoderConfigurator configurator; private CANcoder CaNcoder; private MagnetSensorConfigs magnetSensorConfiguration; @@ -28,28 +29,28 @@ public class SwerveAbsoluteEncoderIOCANcoder implements SwerveAbsoluteEncoderIO public SwerveAbsoluteEncoderIOCANcoder(int ID, double encoderOffset) { this.absoluteEncoderOffset = encoderOffset; this.inverted = false; + + double offsetRotations = encoderOffset/360; CaNcoder = new CANcoder(ID); - // this.turnAbsolutePosition = CaNcoder.getAbsolutePosition(); - // this.configurator = CaNcoder.getConfigurator(); - - // this.magnetSensorConfiguration = new MagnetSensorConfigs(); + this.turnAbsolutePosition = CaNcoder.getAbsolutePosition(); + this.configurator = CaNcoder.getConfigurator(); - // configurator.apply(new CANcoderConfiguration()); + this.magnetSensorConfiguration = new MagnetSensorConfigs(); - // configurator.refresh(magnetSensorConfiguration); - // configurator.apply(magnetSensorConfiguration - // .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) - // .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) - // ); - // configurator.apply(magnetSensorConfiguration.withMagnetOffset(encoderOffset/360)); - // CaNcoder.getConfigurator().withSensorDirection(getS) - + configurator.apply(new CANcoderConfiguration()); + + configurator.refresh(magnetSensorConfiguration); + configurator.apply(magnetSensorConfiguration + .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) + ); + configurator.apply(magnetSensorConfiguration.withSensorDirection(SensorDirectionValue.CounterClockwise_Positive)); + configurator.apply(magnetSensorConfiguration.withMagnetOffset((offsetRotations))); } @Override public void updateInputs(SwerveAbsoluteEncoderIOInputs inputs) { - // BaseStatusSignal.refreshAll(turnAbsolutePosition); + BaseStatusSignal.refreshAll(turnAbsolutePosition); inputs.absoluteEncoderOffset = absoluteEncoderOffset; inputs.inverted = inverted; @@ -59,17 +60,17 @@ public void updateInputs(SwerveAbsoluteEncoderIOInputs inputs) { @Override public void setInverted(boolean inverted) { this.inverted = inverted; - // configurator.refresh(magnetSensorConfiguration); - // configurator.apply(magnetSensorConfiguration - // .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) - // .withSensorDirection((inverted ? SensorDirectionValue.CounterClockwise_Positive : SensorDirectionValue.Clockwise_Positive)) - // ); + configurator.refresh(magnetSensorConfiguration); + configurator.apply(magnetSensorConfiguration + .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) + .withSensorDirection((inverted ? SensorDirectionValue.CounterClockwise_Positive : SensorDirectionValue.Clockwise_Positive)) + ); } @Override public Rotation2d getTurnAbsolutePosition() { // CHECK: Subtracting offset instead of adding offset - return Rotation2d.fromRotations(CaNcoder.getAbsolutePosition().getValueAsDouble()).plus(Rotation2d.fromDegrees(absoluteEncoderOffset)); + return Rotation2d.fromRotations(CaNcoder.getAbsolutePosition().getValueAsDouble()); // return new Rotation2d(); } diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveModule.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveModule.java index 5d137f9..f14b59d 100644 --- a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveModule.java +++ b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveModule.java @@ -117,7 +117,7 @@ public SwerveModuleState runState(SwerveModuleState state) { // } if(!offsetedTurnEncoder && absoluteEncoder.getTurnAbsolutePosition().getDegrees() != 0) { - offsetedTurnEncoder = turnMotor.setEncoderPosition(absoluteEncoder.getTurnAbsolutePosition().getDegrees()) == StatusCode.OK ? true : false; + offsetedTurnEncoder = turnMotor.setEncoderPosition(absoluteEncoder.getTurnAbsolutePosition()) == StatusCode.OK ? true : false; } //feeds value directly to encoder if it is sim. @@ -135,6 +135,9 @@ public SwerveModuleState runState(SwerveModuleState state) { // Prevents the turn motor from doing unneeded rotations var optimizedState = SwerveModuleState.optimize(state, getAngle()); + System.out.println(angleSetPoint + " " + moduleName); + System.out.println(getAngle() + " " + moduleName); + angleSetPoint = optimizedState.angle.getDegrees(); speedSetPoint = (Math.cos(Units.degreesToRadians(getAngle().getDegrees() - angleSetPoint)) * optimizedState.speedMetersPerSecond) / (SwerveDrive.wheelRadius * Math.PI * 2); diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIO.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIO.java index b49327e..3c41c39 100644 --- a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIO.java +++ b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIO.java @@ -55,7 +55,7 @@ public default double getVelocity() { /** * Sets the motors position */ - public default StatusCode setEncoderPosition(double positionDeg) { + public default StatusCode setEncoderPosition(Rotation2d positionDeg) { return StatusCode.OK; } diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIONeoSim.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIONeoSim.java index 06d4e3a..be1abe3 100644 --- a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIONeoSim.java +++ b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIONeoSim.java @@ -111,8 +111,8 @@ public void setVoltage(double volts) { } @Override - public StatusCode setEncoderPosition(double positionDeg) { - encoder.setPosition(positionDeg / 360); + public StatusCode setEncoderPosition(Rotation2d positionDeg) { + encoder.setPosition(positionDeg.getDegrees()); return StatusCode.OK; } diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOSparkMax.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOSparkMax.java index df401c2..ebe25d7 100644 --- a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOSparkMax.java +++ b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOSparkMax.java @@ -146,8 +146,8 @@ public void setPosition(double setpoint) { } @Override - public StatusCode setEncoderPosition(double positionDeg) { - encoder.setPosition(positionDeg / 360); + public StatusCode setEncoderPosition(Rotation2d positionDeg) { + encoder.setPosition(positionDeg.getRotations() * gearRatio); return StatusCode.OK; } diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java index 184580f..9885df4 100644 --- a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java +++ b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java @@ -73,6 +73,7 @@ public SwerveMotorIOTalonFX(int canID, String canbus, double gearRatio) { driveConfig = new TalonFXConfiguration(); driveConfig.CurrentLimits.SupplyCurrentLimit = CURRENT_LIMIT; driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; configurator.apply(driveConfig); //TODO: Apparently this might not work (idk what it's even supposed to do) configurator.apply(driveConfig.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(VOLTAGE_CLOSED_LOOP_RAMP_PERIOD)); @@ -145,8 +146,8 @@ public double getVelocity() { } @Override - public StatusCode setEncoderPosition(double positionDeg) { - return motor.setPosition(Units.degreesToRotations(positionDeg) * gearRatio); + public StatusCode setEncoderPosition(Rotation2d positionDeg) { + return motor.setPosition(positionDeg.getRotations() * gearRatio); // motor.setPosition((positionDeg/360) * gearRatio); } diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFXSim.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFXSim.java index 02457df..0ecc110 100644 --- a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFXSim.java +++ b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFXSim.java @@ -124,8 +124,8 @@ public void setVoltage(double volts) { } @Override - public StatusCode setEncoderPosition(double positionDeg) { - return dummyTalon.setPosition((positionDeg/360) * gearing); + public StatusCode setEncoderPosition(Rotation2d positionDeg) { + return dummyTalon.setPosition(positionDeg.getRotations() * gearing); } @Override