From e15775e31d2cfc44fd8027fa2e2b59ef2771f6ad Mon Sep 17 00:00:00 2001 From: Rango813 Date: Thu, 25 Jan 2024 19:08:23 -0600 Subject: [PATCH] Works,need to clean up --- .../frc/lib/util/swerve/SwerveModule.java | 25 +++++++++++++------ .../frc/lib/util/swerve/SwerveModuleIO.java | 20 +-------------- .../frc/lib/util/swerve/SwerveModuleReal.java | 22 ++++------------ src/main/java/frc/robot/Constants.java | 5 ++-- .../frc/robot/subsystems/swerve/Swerve.java | 9 ++++++- 5 files changed, 35 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index bdb67ce..1c7f6d3 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -1,5 +1,6 @@ package frc.lib.util.swerve; +import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -43,14 +44,20 @@ public class SwerveModule { public SwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID, Rotation2d angleOffset, SwerveModuleIO io) { this.io = io; - io.updateInputs(inputs); + this.moduleNumber = moduleNumber; this.angleOffset = angleOffset; // lastAngle = getState().angle.getDegrees(); + io.updateInputs(inputs); resetToAbsolute(); + Logger.processInputs("SwerveModule" + moduleNumber, inputs); + } + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("SwerveModule" + moduleNumber, inputs); } /** @@ -75,12 +82,14 @@ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { if (isOpenLoop) { driveDutyCycle.Output = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed; io.setDriveMotor(driveDutyCycle); + inputs.driveMotorSelectedSensorVelocity = driveDutyCycle.Output; } else { driveVelocity.Velocity = Conversions.MPSToRPS(desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference); driveVelocity.FeedForward = driveFeedForward.calculate(desiredState.speedMetersPerSecond); - io.setDriveMotor(driveDutyCycle); + io.setDriveMotor(driveVelocity); + inputs.driveMotorSelectedSensorVelocity = driveDutyCycle.Output; } } @@ -90,7 +99,7 @@ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { * @return The rotation of the CANCoder in {@link Rotation2d} */ public Rotation2d getCANcoder() { - return Rotation2d.fromRotations(io.getAbsolutePositionAngleEncoder()); + return Rotation2d.fromRotations(inputs.absolutePositionAngleEncoder); } /** @@ -99,6 +108,7 @@ public Rotation2d getCANcoder() { public void resetToAbsolute() { double absolutePosition = getCANcoder().getRotations() - angleOffset.getRotations(); io.setPositionAngleMotor(absolutePosition); + inputs.angleMotorSelectedPosition = absolutePosition; } /** @@ -108,8 +118,9 @@ public void resetToAbsolute() { */ public SwerveModuleState getState() { return new SwerveModuleState( - Conversions.RPSToMPS(io.getVelocityDriveMotor(), Constants.Swerve.wheelCircumference), - Rotation2d.fromRotations(io.getPositionAngleMotor())); + Conversions.RPSToMPS(inputs.driveMotorSelectedSensorVelocity, + Constants.Swerve.wheelCircumference), + Rotation2d.fromRotations(inputs.angleMotorSelectedPosition)); } /** @@ -119,9 +130,9 @@ public SwerveModuleState getState() { */ public SwerveModulePosition getPosition() { return new SwerveModulePosition( - Conversions.rotationsToMeters(io.getPositionDriveMotor(), + Conversions.rotationsToMeters(inputs.driveMotorSelectedPosition, Constants.Swerve.wheelCircumference), - Rotation2d.fromRotations(io.getPositionAngleMotor())); + Rotation2d.fromRotations(inputs.angleMotorSelectedPosition)); } } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java index fcca1de..282d03b 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java @@ -9,12 +9,10 @@ public interface SwerveModuleIO { /** Inputs Class for SwerveModule */ @AutoLog public static class SwerveModuleInputs { - public double angleEncoderValue; public double driveMotorSelectedPosition; public double driveMotorSelectedSensorVelocity; public double angleMotorSelectedPosition; - public double driveMotorTemperature; - public double angleMotorTemperature; + public double absolutePositionAngleEncoder; } public default void updateInputs(SwerveModuleInputs inputs) {} @@ -25,22 +23,6 @@ public default void setAngleMotor(ControlRequest request) {} public default void setAngleSelectedSensorPosition(double angle) {} - public default double getAbsolutePositionAngleEncoder() { - return 0.0; - } - public default void setPositionAngleMotor(double absolutePosition) {} - public default double getVelocityDriveMotor() { - return 0.0; - } - - public default double getPositionAngleMotor() { - return 0.0; - } - - public default double getPositionDriveMotor() { - return 0.0; - } - } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java index 7c11e24..b9133f3 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java @@ -110,8 +110,11 @@ public void setDriveMotor(ControlRequest request) { } @Override - public double getAbsolutePositionAngleEncoder() { - return angleEncoder.getAbsolutePosition().getValueAsDouble(); + public void updateInputs(SwerveModuleInputs inputs) { + inputs.driveMotorSelectedPosition = mDriveMotor.getPosition().getValueAsDouble(); + inputs.driveMotorSelectedSensorVelocity = mDriveMotor.getVelocity().getValueAsDouble(); + inputs.angleMotorSelectedPosition = mAngleMotor.getPosition().getValueAsDouble(); + inputs.absolutePositionAngleEncoder = angleEncoder.getAbsolutePosition().getValueAsDouble(); } @Override @@ -119,19 +122,4 @@ public void setPositionAngleMotor(double absolutePosition) { mAngleMotor.setPosition(absolutePosition); } - @Override - public double getVelocityDriveMotor() { - return mDriveMotor.getVelocity().getValueAsDouble(); - } - - @Override - public double getPositionAngleMotor() { - return mAngleMotor.getPosition().getValueAsDouble(); - } - - @Override - public double getPositionDriveMotor() { - return mDriveMotor.getPosition().getValueAsDouble(); - } - } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 25072a9..b5f0bfc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -48,10 +48,11 @@ public static final class Swerve { /* Module Gear Ratios */ public static final double driveGearRatio = (8.14 / 1.0); // MK4i L1 - public static final double angleGearRatio = ((150.0 / 7.0) / 1.0); // (150 / 7) : 1 + public static final double angleGearRatio = 12.8 / 1.0; /* Motor Inverts */ - public static final InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; + public static final InvertedValue angleMotorInvert = + InvertedValue.CounterClockwise_Positive; public static final InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index f01c98a..285261e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.swerve; import java.util.Optional; +import org.littletonrobotics.junction.Logger; import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -26,11 +27,15 @@ public class Swerve extends SubsystemBase { public SwerveModule[] swerveMods; public AHRS gyro = new AHRS(Constants.Swerve.navXID); private double fieldOffset = gyro.getYaw(); + private SwerveInputsAutoLogged inputs = new SwerveInputsAutoLogged(); + private SwerveIO swerveIO; /** * Swerve Subsystem */ public Swerve(SwerveIO swerveIO) { + this.swerveIO = swerveIO; + swerveIO.updateInputs(inputs); swerveMods = new SwerveModule[] { swerveIO.createSwerveModule(0, Constants.Swerve.Mod0.DRIVE_MOTOR_ID, Constants.Swerve.Mod0.ANGLE_MOTOR_ID, Constants.Swerve.Mod0.CAN_CODER_ID, @@ -198,8 +203,10 @@ public void resetModulesToAbsolute() { @Override public void periodic() { swerveOdometry.update(getGyroYaw(), getModulePositions()); - + swerveIO.updateInputs(inputs); + Logger.processInputs("Swerve", inputs); for (SwerveModule mod : swerveMods) { + mod.periodic(); SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle",