From becf467173853642b986cf3a5fe50ad67833828e Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:12:06 -0500 Subject: [PATCH 01/14] Add drivetrain teleop command - Weston J --- src/main/deploy/config.toml | 9 ++ src/main/java/frc/robot/Constants.java | 31 ++++- src/main/java/frc/robot/Robot.java | 37 +++++- .../DriveTeleopCommand.java | 120 ++++++++++++++++++ 4 files changed, 190 insertions(+), 7 deletions(-) create mode 100644 src/main/deploy/config.toml create mode 100644 src/main/java/frc/robot/commands/driveTeleopCommand/DriveTeleopCommand.java diff --git a/src/main/deploy/config.toml b/src/main/deploy/config.toml new file mode 100644 index 0000000..6960dbf --- /dev/null +++ b/src/main/deploy/config.toml @@ -0,0 +1,9 @@ +[drivetrain] +frontLeftAbsEncoderOffset = 38.1 +frontRightAbsEncoderOffset = -131.0 +backLeftAbsEncoderOffset = -96.4 +backRightAbsEncoderOffset = -30.6 + +swerve.turn.kP = 2.0 +swerve.turn.kI = 0.01 +swerve.turn.kD = 3.0 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 80e00ed..e09ae10 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,10 +1,31 @@ package frc.robot; -public class Constants { - public class Drive { +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; - public static final double kTrunWheelPerMotorRatio = 0.0; - - public static final double kMaxDriveSpeedMetersPerSecond = 4.0; + +public final class Constants { + + public final static LinearVelocity kMaxDriveSpeed = Units.MetersPerSecond.of(4.0); + + public final static LinearVelocity kNormalDriveSpeed = Units.MetersPerSecond.of(1.5); + public final static LinearVelocity kSlowDriveSpeed = kNormalDriveSpeed.times(1.0 - 0.75); // 75% slower + public final static LinearVelocity kFastDriveSpeed = kMaxDriveSpeed; + + public final static AngularVelocity kMaxTurnSpeed = Units.RadiansPerSecond.of(Math.PI * 2.0); + + public final static AngularVelocity kNormalTurnSpeed = Units.RadiansPerSecond.of(Math.PI); + public final static AngularVelocity kSlowTurnSpeed = kNormalTurnSpeed.times(1.0 - 0.5); //50% slower + public final static AngularVelocity kFastTurnSpeed = kMaxTurnSpeed; + + public final static class Drive { + public final static int kNumberOfSwerverModules = 4; + public final static Distance kWheelDiamter = Units.Inches.of(3.75); + public final static double kTurnWheelPerMotorRatio = 21.4285571; + public final static double kDriveWheelPerMotorRatio = 0.164062; + public final static double kDriveRpmPerVolt = 78.061404; //FIXME: IDK THE TYPE OF THIS??? + public final static double kDriveRadPerSecPerVolt = -1.0; //FIXME: WHAT ARE THESE TYPES??? } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 715e6ca..cb0ce8c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,9 +4,21 @@ package frc.robot; +import java.util.Optional; + +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +import frc.robot.commands.driveTeleopCommand.DriveTeleopCommand; +import frc.robot.lib.config.ConfigLoader; +import frc.robot.lib.config.ConfigTable; +import frc.robot.lib.config.TomlConfigLoader; +import frc.robot.subsystems.drive.Drivetrain; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -19,6 +31,8 @@ public class Robot extends TimedRobot { private static final String kCustomAuto = "My Auto"; private String m_autoSelected; private final SendableChooser m_chooser = new SendableChooser<>(); + + private Drivetrain kDrivetrain = null; /** * This function is run when the robot is first started up and should be used for any @@ -26,6 +40,21 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { + + String path = Filesystem.getDeployDirectory().getAbsolutePath(); + + ConfigLoader config = new TomlConfigLoader(path + "/config.toml"); + + config.openConfig(); + + Optional table = config.getTable("drivetrain"); + + if(table.isEmpty()){ + throw new Error("Unable to find config table 'drivetrain' "); + } + + kDrivetrain = new Drivetrain(table.get()); + m_chooser.setDefaultOption("Default Auto", kDefaultAuto); m_chooser.addOption("My Auto", kCustomAuto); SmartDashboard.putData("Auto choices", m_chooser); @@ -39,7 +68,9 @@ public void robotInit() { * SmartDashboard integrated updating. */ @Override - public void robotPeriodic() {} + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + } /** * This autonomous (along with the chooser code above) shows how to select between different @@ -74,7 +105,9 @@ public void autonomousPeriodic() { /** This function is called once when teleop is enabled. */ @Override - public void teleopInit() {} + public void teleopInit() { + CommandScheduler.getInstance().schedule(new DriveTeleopCommand(kDrivetrain, new XboxController(0), Units.Milliseconds.of(20.0))); + } /** This function is called periodically during operator control. */ @Override diff --git a/src/main/java/frc/robot/commands/driveTeleopCommand/DriveTeleopCommand.java b/src/main/java/frc/robot/commands/driveTeleopCommand/DriveTeleopCommand.java new file mode 100644 index 0000000..f5f7ab5 --- /dev/null +++ b/src/main/java/frc/robot/commands/driveTeleopCommand/DriveTeleopCommand.java @@ -0,0 +1,120 @@ +package frc.robot.commands.driveTeleopCommand; + +import static edu.wpi.first.math.MathUtil.applyDeadband; +import static edu.wpi.first.units.Units.DegreesPerSecond; + +import frc.robot.subsystems.drive.*; +import frc.robot.Constants; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj2.command.Command; + + +public class DriveTeleopCommand extends Command { + + private Time mRobotPeriod; + + private Drivetrain mDrivetrain; + + private XboxController mDriveController; + + private SlewRateLimiter mForwardSpeedLimiter = new SlewRateLimiter(3.0); + private SlewRateLimiter mStrafeSpeedLimiter = new SlewRateLimiter(3.0); + private SlewRateLimiter mTurnSpeedLimiter = new SlewRateLimiter(3.0); + + private final double JOYSTICK_DEADZONE = 0.2; + + public DriveTeleopCommand( + Drivetrain drivetrain, + XboxController driverController, + Time period + ) { + addRequirements(drivetrain); + + mDriveController = driverController; + mDrivetrain = drivetrain; + mRobotPeriod = period; + + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + mDrivetrain.updateOdometry(); + //compresses the range of the driving speed to be within the max speed and + //the minimum. but have the normal speed be the default if no trigger is + //being pressed (so both register 0) + // + //NOTE no trigger takes priority of the other so if both pressed they will cancel each other + + double leftTrigger = mDriveController.getLeftTriggerAxis(); // Range [0.0..1.0] + double rightTrigger = mDriveController.getRightTriggerAxis(); // Range [0.0..1.0] + + LinearVelocity driveReduce = Constants.kNormalDriveSpeed.minus(Constants.kSlowDriveSpeed).times(leftTrigger); + + LinearVelocity driveGain = Constants.kFastDriveSpeed.minus(Constants.kNormalDriveSpeed).times(rightTrigger); + + LinearVelocity driveSpeedFactor = Constants.kNormalDriveSpeed.plus(driveGain).minus(driveReduce); + + AngularVelocity turnReduce = (Constants.kNormalTurnSpeed.minus(Constants.kSlowTurnSpeed)).times(leftTrigger); + AngularVelocity turnGain = (Constants.kFastTurnSpeed.minus(Constants.kNormalTurnSpeed)).times(rightTrigger); + + AngularVelocity turnSpeedFactor = Constants.kNormalTurnSpeed.plus(turnGain).minus(turnReduce); + + // the rotation limit is there in case the driver does not want to spin as fast while driving + final LinearVelocity forwardSpeed = driveSpeedFactor.times( + mForwardSpeedLimiter.calculate(applyDeadband( + -mDriveController.getLeftY(), + JOYSTICK_DEADZONE)) + ); + + final LinearVelocity strafeSpeed = driveSpeedFactor.times( + mStrafeSpeedLimiter.calculate(applyDeadband( + -mDriveController.getLeftX(), + JOYSTICK_DEADZONE)) + ); + + final AngularVelocity turnSpeed = turnSpeedFactor.times( + mTurnSpeedLimiter.calculate(applyDeadband( + -mDriveController.getRightX(), + JOYSTICK_DEADZONE)) + ); + + mDrivetrain.Drive( + forwardSpeed, + strafeSpeed, + turnSpeed, + mDrivetrain.IsFieldOriented(), + mRobotPeriod + ); + + } + + @Override + public void end(boolean interrupted) { + // Stop the drive motors! + mDrivetrain.Drive( + Units.MetersPerSecond.of(0.0), + Units.MetersPerSecond.of(0.0), + Units.RadiansPerSecond.of(0.0), + true, + mRobotPeriod + ); + } + + @Override + public boolean isFinished() { + return false; //dont end because then we wont be able to drive + } + +} From cc38f1568561b01c5e74a925072182ed602525ad Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:14:56 -0500 Subject: [PATCH 02/14] Throw error when opening config.toml fails - Weston J --- .../frc/robot/lib/config/TomlConfigLoader.java | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/lib/config/TomlConfigLoader.java b/src/main/java/frc/robot/lib/config/TomlConfigLoader.java index 846038b..3bd71b3 100644 --- a/src/main/java/frc/robot/lib/config/TomlConfigLoader.java +++ b/src/main/java/frc/robot/lib/config/TomlConfigLoader.java @@ -1,5 +1,10 @@ package frc.robot.lib.config; +import java.io.IOError; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; import java.util.Optional; import org.tomlj.Toml; import org.tomlj.TomlParseResult; @@ -16,9 +21,15 @@ public TomlConfigLoader(String configFile) { @Override public void openConfig() { - TomlParseResult result = Toml.parse(mConfigFile); - result.errors().forEach(error -> System.err.println(error.toString())); - mToml = result; + try { + String file = Files.readString(Path.of(mConfigFile)); + TomlParseResult result = Toml.parse(file); + result.errors().forEach(error -> System.err.println(error.toString())); + mToml = result; + } catch (Exception ex) { + System.err.println("Error: unable to read config.toml at " + mConfigFile); + System.exit(1); + } } @Override From 719addeb41258c903bdc4a92627dbee95cef3d11 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:18:00 -0500 Subject: [PATCH 03/14] Fix variable typo for drivetrain swerve front right - Dominic A --- src/main/java/frc/robot/subsystems/drive/Drivetrain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index 62207ad..9853f2c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -135,7 +135,7 @@ public Drivetrain(ConfigTable table) { throw new Error("error"); } - mBackLeft = new SwerveModule( + mFrontRight = new SwerveModule( Interface.Drive.kFrontRightDrive, Interface.Drive.kFrontRightTurn, Interface.Drive.kFrontRightEncoder, From 202b998fec74cff60adcd76ce73bf63870720f5c Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:18:37 -0500 Subject: [PATCH 04/14] Fix typo on config file key names - Dominic A --- src/main/java/frc/robot/subsystems/drive/Drivetrain.java | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index 9853f2c..14b9f9e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -143,12 +143,10 @@ public Drivetrain(ConfigTable table) { turnPidConfig, "front-right" ); - - } { - Optional backLeftAbsEncoderOffset = table.getDouble("backLeftAbsEncoder"); + Optional backLeftAbsEncoderOffset = table.getDouble("backLeftAbsEncoderOffset"); if (backLeftAbsEncoderOffset.isEmpty()) { System.err.println("Error: drivetrain cannot find toml property backLeftAbsEncoderOffset"); @@ -167,7 +165,7 @@ public Drivetrain(ConfigTable table) { { - Optional backRightAbsEncoderOffset = table.getDouble("backRightAbsEncoder"); + Optional backRightAbsEncoderOffset = table.getDouble("backRightAbsEncoderOffset"); if (backRightAbsEncoderOffset.isEmpty()) { System.err.println("Error: drivetrain cannot find toml property backRightAbsEncoderOffset"); From ded6d5ffba497bd91d8f9d7ba1fb60b300a6c921 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:19:16 -0500 Subject: [PATCH 05/14] Reformat - Dominic A --- .../robot/subsystems/drive/Drivetrain.java | 20 +++++++------------ 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index 14b9f9e..527b311 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -29,23 +29,23 @@ import frc.robot.subsystems.drive.SwerveModule.PidConfig; public class Drivetrain extends SubsystemBase { - - private Translation2d mFrontLeftLocation = new Translation2d(Units.Meters.of(+0.287), Units.Meters.of(+0.287)); - private Translation2d mFrontRightLocation = new Translation2d(Units.Meters.of(+0.287), Units.Meters.of(-0.287)); - private Translation2d mBackLeftLocation = new Translation2d(Units.Meters.of(-0.287), Units.Meters.of(+0.287)); - private Translation2d mBackRightLocation = new Translation2d(Units.Meters.of(-0.287), Units.Meters.of(-0.287)); private SwerveModule mFrontLeft = null; private SwerveModule mFrontRight = null; private SwerveModule mBackLeft = null; private SwerveModule mBackRight = null; + private Translation2d mFrontLeftLocation = new Translation2d(Units.Meters.of(+0.287), Units.Meters.of(+0.287)); + private Translation2d mFrontRightLocation = new Translation2d(Units.Meters.of(+0.287), Units.Meters.of(-0.287)); + private Translation2d mBackLeftLocation = new Translation2d(Units.Meters.of(-0.287), Units.Meters.of(+0.287)); + private Translation2d mBackRightLocation = new Translation2d(Units.Meters.of(-0.287), Units.Meters.of(-0.287)); + private SwerveDriveKinematics mKinematics = new SwerveDriveKinematics( mFrontLeftLocation, mFrontRightLocation, mBackLeftLocation, mBackRightLocation - ); + ); private AHRS mGyro = new AHRS(AHRS.NavXComType.kMXP_SPI); private Angle mGyroOffset = Degrees.of(0.0); @@ -69,7 +69,6 @@ public Drivetrain(ConfigTable table) { double turnD = 0.0; { - Optional kP = table.getDouble("swerve.turn.kP"); if (kP.isEmpty()) { @@ -82,7 +81,6 @@ public Drivetrain(ConfigTable table) { } { - Optional kI = table.getDouble("swerve.turn.kI"); if (kI.isEmpty()) { @@ -107,8 +105,7 @@ public Drivetrain(ConfigTable table) { PidConfig turnPidConfig = new PidConfig(turnP, turnI, turnD); - { - + { Optional frontLeftAbsEncoderOffset = table.getDouble("frontLeftAbsEncoderOffset"); if (frontLeftAbsEncoderOffset.isEmpty()) { @@ -205,9 +202,6 @@ public Drivetrain(ConfigTable table) { mBackRight.GetPosition() } ); - - - } //this bracket ends config table public void Drive( From 7a6976dae620200d2d68e2c3672d0c3169c62d3f Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:20:23 -0500 Subject: [PATCH 06/14] Initialize swerve drive odometry in constructor - Dominic A --- .../java/frc/robot/subsystems/drive/Drivetrain.java | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index 527b311..8a795fd 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -52,15 +52,7 @@ public class Drivetrain extends SubsystemBase { private boolean mIsFieldOriented = true; - private SwerveDriveOdometry mOdometry = new SwerveDriveOdometry( - mKinematics, - mGyro.getRotation2d(), - new SwerveModulePosition[] { - getModulePosition(mFrontLeftLocation), - getModulePosition(mFrontRightLocation), - getModulePosition(mBackLeftLocation), - getModulePosition(mBackRightLocation)}, - new Pose2d()); + private SwerveDriveOdometry mOdometry; public Drivetrain(ConfigTable table) { From b4d1effa612e83007112242133fcf599644d4d97 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:21:12 -0500 Subject: [PATCH 07/14] Import units - Dominic A --- src/main/java/frc/robot/subsystems/drive/Drivetrain.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index 8a795fd..b63827a 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Seconds; import java.util.Optional; From dade4cb3be190c1a733ca8c12f4f9e0e1b2a6c5c Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:21:40 -0500 Subject: [PATCH 08/14] Rename resetGyro to zeroGyro Why? Because we have other reset methods that reboot the NavX while this function only updates the zero offset. - Dominic A --- src/main/java/frc/robot/subsystems/drive/Drivetrain.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index b63827a..a8ee907 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -183,7 +183,7 @@ public Drivetrain(ConfigTable table) { } } - resetGyro(); + zeroGyro(); mOdometry = new SwerveDriveOdometry( mKinematics, @@ -280,7 +280,7 @@ public void toggleFieldOriented() { mIsFieldOriented = !mIsFieldOriented; } - public void resetGyro() { + public void zeroGyro() { mGyroOffset = Degrees.of(mGyro.getYaw()).unaryMinus(); } From fdce59533c623d3f9bf51f78b18219cedb7ab863 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:22:41 -0500 Subject: [PATCH 09/14] Fix swerve discretization period in seconds Otherwise units are unknown, and value is arbitrary and wrong. - Dominic A --- src/main/java/frc/robot/subsystems/drive/Drivetrain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index a8ee907..8d21034 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -231,7 +231,7 @@ public void Drive( : new ChassisSpeeds(forwardSpeed, strafeSpeed, turnSpeed); SwerveModuleState[] states = mKinematics.toSwerveModuleStates( - ChassisSpeeds.discretize(chassisSpeeds, period.magnitude()) + ChassisSpeeds.discretize(chassisSpeeds, period.in(Seconds)) ); SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.Drive.kMaxDriveSpeedMetersPerSecond); From 8cce2b489cf85f90c85d556913ba572ba868da24 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:23:52 -0500 Subject: [PATCH 10/14] Fix typo in renamed constant - Dominic A --- src/main/java/frc/robot/subsystems/drive/Drivetrain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index 8d21034..9298d68 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -234,7 +234,7 @@ public void Drive( ChassisSpeeds.discretize(chassisSpeeds, period.in(Seconds)) ); - SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.Drive.kMaxDriveSpeedMetersPerSecond); + SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.kMaxDriveSpeed); SwerveModuleState fl = states[0]; SwerveModuleState fr = states[1]; From 0694d6ab61b7a9076b246e0a023189da19fd5bf3 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:29:10 -0500 Subject: [PATCH 11/14] Cleanup - Justin C --- .../robot/subsystems/drive/SwerveModule.java | 170 +++++++----------- 1 file changed, 69 insertions(+), 101 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java index 9dd198a..f8cc48c 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -1,22 +1,17 @@ package frc.robot.subsystems.drive; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Radians; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.Unit; import edu.wpi.first.units.Units; -import edu.wpi.first.units.Units.*; -import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.geometry.Rotation2d; - -import static edu.wpi.first.units.Units.Degrees; - import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -62,9 +57,7 @@ public SwerveModule( Angle absEncoderOffset, PidConfig turnConfig, String name - ) - - { + ) { mName = name; mDriveMotor = new SparkMax(driveMotorCan, MotorType.kBrushless); @@ -84,11 +77,6 @@ public SwerveModule( mDriveMotor.configure(driveMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); mTurnMotor = new SparkMax(turnMotorCan, MotorType.kBrushless); - mTurnPid = mTurnMotor.getClosedLoopController(); - mTurnEncoder = mTurnMotor.getEncoder(); - - mTurnAbsEncoder = new CANcoder(turnAbsEncoderCan); - mTurnAbsPositionSignal = mTurnAbsEncoder.getAbsolutePosition(); SparkMaxConfig turnMotorConfig = new SparkMaxConfig(); // names are diffrent ill try my best to copy! -Weston Justice @@ -121,6 +109,12 @@ public SwerveModule( mTurnMotor.configure(turnMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + mTurnPid = mTurnMotor.getClosedLoopController(); + mTurnEncoder = mTurnMotor.getEncoder(); + + mTurnAbsEncoder = new CANcoder(turnAbsEncoderCan); + mTurnAbsPositionSignal = mTurnAbsEncoder.getAbsolutePosition(); + CANcoderConfiguration configCanCoder = new CANcoderConfiguration(); MagnetSensorConfigs configMagnetSensor = new MagnetSensorConfigs(); configMagnetSensor @@ -130,48 +124,32 @@ public SwerveModule( configCanCoder.withMagnetSensor(configMagnetSensor); - while (true){ - com.ctre.phoenix6.StatusCode sc = mTurnAbsEncoder.getConfigurator().apply(configMagnetSensor, 5.0); + while (true) { + com.ctre.phoenix6.StatusCode sc = mTurnAbsEncoder.getConfigurator().apply( + configCanCoder, + 5.0 + ); - if(sc.isOK()){ + if (sc.isOK()) { break; - } - else{ + } else { System.err.println( - String.format("!!! ERROR !!! swerve module: failed to apply config to abs encoder for module %s: status code %s: description %s", mName, sc.getName(), sc.getDescription())); + String.format( + "!!! ERROR !!! swerve module: failed to apply config to abs encoder for module %s: status code %s: description %s", + mName, + sc.getName(), + sc.getDescription() + ) + ); } - try{ + + try { Thread.sleep(50); - } catch(Exception e){ + } catch (Exception e) { // do nothing. } - } - /** FIXME: Constants have not been set up yet - Commented out to see if new code works - - mTurnEncoder.setPositionConversionFactor( - 2.0 * Math.PI - / Constants.Drive.kTrunWheelPerMotorRatio - ); - - mTurnEncoder.setVelocityConversionFactor( - (2.0 * Math.PI / 1.0) - / Constants.Drive.kTrunWheelPerMotorRatio - * (1.0 / 60.0) - ); - */ - - /** drive pid parameters || Commented out to see if new code works - mDrivePid.setP(1.0); - mDrivePid.setI(0.0); - mDrivePid.setIZone(0.0); - mDrivePid.setD(0.0); - - mDrivePid.setFF(0.0); - */ - mDriveEncoder.setPosition(0.0); while (true) { @@ -179,12 +157,16 @@ public SwerveModule( com.ctre.phoenix6.StatusCode sc = mTurnAbsPositionSignal.getStatus(); - if(sc.isOK()){ + if (sc.isOK()) { break; - } - else{ + } else { System.err.println( - String.format("!!! ERROR !!! swerve module: failed to update measurement from abs encoder for module {}: status code {}: description {}", mName, sc.getName(), sc.getDescription()) + String.format( + "!!! ERROR !!! swerve module: failed to update measurement from abs encoder for module {}: status code {}: description {}", + mName, + sc.getName(), + sc.getDescription() + ) ); } @@ -204,44 +186,19 @@ public void Periodic(){ mTurnAbsPositionSignal.refresh(); } - - public Angle GetTurnPosition() { - return Units.Radians.of( mTurnEncoder.getPosition() ); - } - - public Angle GetTurnAbsPosition(){ - Angle position = Degrees.of(Math.abs( - fmod( - mTurnAbsPositionSignal.getValue().plus(mAbsEncoderOffset).plus(Units.Degrees.of(180)).in(Degrees), - 360.0 - ) - ) - 180.0); - return position; - } - - public Angle GetTurnAbsPositionRaw(){ - Angle position = mTurnAbsPositionSignal.getValue(); - return position; - } - - public SwerveModulePosition GetPosition(){ - return new SwerveModulePosition( - mDriveEncoder.getPosition(), - Rotation2d.fromRadians(GetTurnPosition().in(Units.Radians)) - ); - } - public SwerveModuleState GetState() { return new SwerveModuleState( mDriveEncoder.getVelocity(), Rotation2d.fromRadians(GetTurnPosition().in(Units.Radians)) ); } - - public void UpdateDashboard() { - // TODO: - } + public SwerveModulePosition GetPosition() { + return new SwerveModulePosition( + mDriveEncoder.getPosition(), + Rotation2d.fromRadians(GetTurnPosition().in(Units.Radians)) + ); + } public void SetDesiredState( SwerveModuleState desiredState @@ -256,32 +213,47 @@ public void SetDesiredState( // perpendicular to the desired direction of travel that can occur when // modules change directions. this results in smoother driving - // FIXME: - // I do not know how rotation2d works - - desiredState.speedMetersPerSecond *= (encoderRotation.minus(desiredState.angle)).getCos(); + desiredState.speedMetersPerSecond *= (desiredState.angle.minus(encoderRotation)).getCos(); // Set drive speed percent. - // FIXME: - double drivePercent = clamp( - (desiredState.speedMetersPerSecond / Constants.Drive.kMaxDriveSpeedMetersPerSecond), - -1.0, 1.0 + (desiredState.speedMetersPerSecond / Constants.kMaxDriveSpeed.in(Units.MetersPerSecond)), + -1.0, + 1.0 ); mDriveMotor.set(drivePercent); - // this has been marked for removel - mTurnPid.setReference( desiredState.angle.getRadians(), com.revrobotics.spark.SparkBase.ControlType.kPosition ); } - //SetTrunBreak might be a typo but it might break everything if I fix it - public void SetTrunBreak(Boolean isEnabled, SparkMaxConfig turnMotorConfig){ + + public void ResetTurnPosition() { + mTurnEncoder.setPosition(GetTurnAbsPosition().in(Radians)); + } + + public Angle GetTurnPosition() { + return Units.Radians.of( mTurnEncoder.getPosition() ); + } + + public Angle GetTurnAbsPosition() { + double absA = GetTurnAbsPositionRaw().plus(mAbsEncoderOffset).plus(Degrees.of(180.0)).in(Degrees); + double absB = absA % 360.0; + double absC = absB - 180.0; + + return Degrees.of(absC); + } + + public Angle GetTurnAbsPositionRaw() { + Angle position = mTurnAbsPositionSignal.getValue(); + return position; + } + + public void SetTurnBreak(Boolean isEnabled, SparkMaxConfig turnMotorConfig){ if(isEnabled){ turnMotorConfig.idleMode(IdleMode.kBrake); } @@ -290,10 +262,6 @@ public void SetTrunBreak(Boolean isEnabled, SparkMaxConfig turnMotorConfig){ } } - public void ResetTurnPosition() { - mTurnEncoder.setPosition(GetTurnAbsPosition().in(Degrees)); - } - public static final class PidConfig { public double kP = 0.0; public double kI = 0.0; @@ -306,7 +274,7 @@ public PidConfig(double p, double i, double d) { } } - public double fmod(double a, double b) { + private double fmod(double a, double b) { b = Math.abs(b); double quotient = a/b; double remainder = quotient - Math.floor(quotient); From 0b74b27cf7f3da45e547daaa89766065967435fe Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:33:42 -0500 Subject: [PATCH 12/14] Fix abs encoder zero offset - Dominic A and Weston J --- src/main/java/frc/robot/subsystems/drive/SwerveModule.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java index f8cc48c..8852a3d 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -32,7 +32,7 @@ public class SwerveModule { String mName; - Angle mAbsEncoderOffset; + Angle mAbsEncoderOffset = Degrees.zero(); SparkMax mDriveMotor; SparkClosedLoopController mDrivePid; @@ -60,6 +60,8 @@ public SwerveModule( ) { mName = name; + mAbsEncoderOffset = absEncoderOffset; + mDriveMotor = new SparkMax(driveMotorCan, MotorType.kBrushless); mDrivePid = mDriveMotor.getClosedLoopController(); mDriveEncoder = mDriveMotor.getEncoder(); From 9be8d7ea7a530b13ab55fd6faac6d5bc416bf069 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:34:07 -0500 Subject: [PATCH 13/14] Fix typo - Weston J --- src/main/java/frc/robot/subsystems/drive/SwerveModule.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java index 8852a3d..5d5763e 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -91,11 +91,11 @@ public SwerveModule( turnMotorConfig.encoder .positionConversionFactor( 2.0 * Math.PI - / Constants.Drive.kTrunWheelPerMotorRatio + / Constants.Drive.kTurnWheelPerMotorRatio ) .velocityConversionFactor( (2.0 * Math.PI / 1.0) - / Constants.Drive.kTrunWheelPerMotorRatio + / Constants.Drive.kTurnWheelPerMotorRatio * (1.0 / 60.0) ); From ef1a1425d6efded8cad09300c3aa68c5351fb3ff Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 6 Feb 2025 20:34:23 -0500 Subject: [PATCH 14/14] Turn on turn motor position wrapping and output limits - Domonic A and Weston J --- src/main/java/frc/robot/subsystems/drive/SwerveModule.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java index 5d5763e..187ce3d 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -100,13 +100,16 @@ public SwerveModule( ); turnMotorConfig.closedLoop - .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .outputRange(-1.0, 1.0) + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pidf( turnConfig.kP, //SetP turnConfig.kI, //SetI turnConfig.kD, //SetD 0.0 //SetFF - ); + ) + .positionWrappingEnabled(true) + .positionWrappingInputRange(-Math.PI, Math.PI); mTurnMotor.configure(turnMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);