From bc53ef3b38fdbeee623ce0b188dbc7097b3cb405 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Mon, 26 Jan 2026 22:26:21 -0500 Subject: [PATCH] Hopeful fixes from the first round of tests --- .../SwerveDrive/SwerveBuilder.java | 25 ++++++- .../SwerveDrive/SwerveDriveSubsystem.java | 66 +++++++++++++++++-- .../SwerveDrive/SwerveModule.java | 57 +++++++++++++--- 3 files changed, 132 insertions(+), 16 deletions(-) diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java index b47f429..4b5781a 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java @@ -9,6 +9,9 @@ public class SwerveBuilder { ArrayList modules = new ArrayList(); String canBusName = "rio"; + private double maxAngularVelRadiansSecond = 1.0; + private double maxLinearVelMetersSecond = Math.PI; + private int pigeonId; public SwerveBuilder(){ @@ -30,8 +33,28 @@ public class SwerveBuilder { return this; } + /** + * Set the max linear velocity in meters per second + * @param maxVel Max linear velocity in meters per second + * @return The swerve builder + */ + public SwerveBuilder withMaxLinearVelocity(double maxVel){ + this.maxLinearVelMetersSecond = maxVel; + return this; + } + + /** + * Set the max angular velocity in radians per second + * @param maxVel Max angular velocity in radians per second + * @return The swerve builder + */ + public SwerveBuilder withMaxAngularVelocity(double maxVel){ + this.maxAngularVelRadiansSecond = maxVel; + return this; + } + public SwerveDriveSubsystem build(){ - return new SwerveDriveSubsystem(pigeonId, modules.toArray(size -> new SwerveModule[size])); + return new SwerveDriveSubsystem(pigeonId, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size])); } public class SwerveModuleBuilder { diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java index aa7bbd5..13e196d 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java @@ -2,6 +2,7 @@ package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive; import java.util.Arrays; +import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d; @@ -26,12 +27,17 @@ public class SwerveDriveSubsystem extends SubsystemBase{ private SwerveModule[] modules; - private final double MAX_LINEAR_SPEED = 3.9; - private final double MAX_ANGULAR_SPEED = Math.PI * 2; + private double MAX_LINEAR_SPEED = 3.9; + private double MAX_ANGULAR_SPEED = Math.PI * 2; private Pigeon2 gyro; - public SwerveDriveSubsystem(int pigeonId, SwerveModule... modules){ + private double mLastSimTime = 0; + + public SwerveDriveSubsystem(int pigeonId, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){ + + MAX_LINEAR_SPEED = maxLinearVelMetersSecond; + MAX_ANGULAR_SPEED = maxAngularVelRadiansSecond; this.modules = modules; @@ -47,19 +53,34 @@ public class SwerveDriveSubsystem extends SubsystemBase{ @Override public void periodic() { + super.periodic(); SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation); for(int i=0; i m.getModuleLocation()).toArray(Translation2d[]::new); } @@ -68,6 +89,14 @@ public class SwerveDriveSubsystem extends SubsystemBase{ return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleState()).toArray(SwerveModuleState[]::new); } + public SwerveModule[] getModules(){ + return Arrays.stream(modules).toArray(SwerveModule[]::new); + } + + public int getNumberOfModules(){ + return modules.length; + } + public Angle getCurrentYaw(){ return gyro.getYaw().refresh().getValue(); } @@ -81,7 +110,13 @@ public class SwerveDriveSubsystem extends SubsystemBase{ this.centerOfRotation = centerOfRotation; } - public void setChassisSpeedsFromJoystick(double x, double y, double rot){ + /** + * Set robot speed relative to field + * @param x Forward robot speed in meters per second + * @param y Left robot speed in meters per second + * @param rot CCW robot speed in radians per second + */ + public void setChassisSpeedsFromJoystickFieldRelative(double x, double y, double rot){ this.setChassisSpeed(ChassisSpeeds.fromFieldRelativeSpeeds( MAX_LINEAR_SPEED * x, MAX_LINEAR_SPEED * y, @@ -90,6 +125,25 @@ public class SwerveDriveSubsystem extends SubsystemBase{ )); } + /** + * Set robot speed relative to robot + * @param x Forward robot speed in meters per second + * @param y Left robot speed in meters per second + * @param rot CCW robot speed in radians per second + */ + public void setChassisSpeedsFromJoystickRobotRelative(double x, double y, double rot){ + this.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds( + MAX_LINEAR_SPEED * x, + MAX_LINEAR_SPEED * y, + MAX_ANGULAR_SPEED * rot, + Rotation2d.fromDegrees(getCurrentYaw().magnitude()) + )); + } + + public Pose3d getRobotPose(){ + return poseEstimator.getEstimatedPosition(); + } + private SwerveModulePosition[] getModulePositions(){ return Arrays.stream(modules).map((SwerveModule m) -> m.getModulePosition()).toArray(SwerveModulePosition[]::new); } diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java index 3f9ec49..420b4c9 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java @@ -1,6 +1,8 @@ package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -8,6 +10,7 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.sun.jdi.request.StepRequest; import edu.wpi.first.math.geometry.Rotation2d; @@ -57,11 +60,11 @@ public class SwerveModule { driveMotor = new TalonFX(driveMotorId, canBusName); driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY); driveMotor.optimizeBusUtilization(); - driveMotor.getConfigurator().apply(generateTalonConfiguration(driveKP, driveKI, driveKD)); + driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD)); steerMotor = new TalonFX(steerMotorId, canBusName); steerMotor.optimizeBusUtilization(); - steerMotor.getConfigurator().apply(generateTalonConfiguration(steerKP, steerKI, steerKD)); + steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD)); steerEncoder = new CANcoder(steerEncoderId, canBusName); steerEncoder.getPosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY); @@ -80,19 +83,33 @@ public class SwerveModule { steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations)); } - public Rotation2d getCurrentSteerRotations(){ + public Rotation2d getSteerRotations(){ return new Rotation2d(steerEncoder.getPosition().refresh().getValueAsDouble()); } public void setDriveSpeed(double speed){ + // Convert wheel speed to rotor speed + // Circumfrence of wheel / speed * gear ratio double rotorVelocity = ((Math.PI * wheelDiameterInMeters) / speed) * gearRatio; - //driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity)); + driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity)); } - public double getDriveSpeed(){ + /** + * Get drive motor speed in rotations per second + * @return + */ + public double getDriveMotorSpeed(){ return driveMotor.getVelocity().refresh().getValueAsDouble(); } + /** + * Get drive wheel speed in meters per second + * @return + */ + public double getDriveWheelSpeed(){ + return (driveMotor.getVelocity().refresh().getValueAsDouble() / gearRatio) * (Math.PI * wheelDiameterInMeters); + } + public Translation2d getModuleLocation(){ return moduleLocation; } @@ -108,16 +125,38 @@ public class SwerveModule { } public SwerveModulePosition getModulePosition(){ - return new SwerveModulePosition(this.getModuleDistance(), getCurrentSteerRotations()); + return new SwerveModulePosition(this.getModuleDistance(), getSteerRotations()); + } + + public String getModuleName(){ + return moduleName; } private double getModuleDistance(){ - double distance = (this.driveMotor.getPosition(true).getValueAsDouble()) / gearRatio / (Math.PI * wheelDiameterInMeters); + double distance = (this.driveMotor.getPosition(true).getValueAsDouble()) / gearRatio * (Math.PI * wheelDiameterInMeters); return distance; } - private TalonFXConfiguration generateTalonConfiguration(double kP, double kI, double kD){ - return new TalonFXConfiguration().withSlot0(new Slot0Configs().withKP(kP).withKI(kI).withKD(kD)); + private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD){ + return new TalonFXConfiguration().withSlot0( + new Slot0Configs() + .withKP(kP) + .withKI(kI) + .withKD(kD) + ); + } + + private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD){ + return new TalonFXConfiguration().withSlot0( + new Slot0Configs() + .withKP(kP) + .withKI(kI) + .withKD(kD) + ).withFeedback( + new FeedbackConfigs() + .withFeedbackRemoteSensorID(steerEncoder.getDeviceID()) + .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder) + ); } private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){