Swerve

src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java

@@ -39,6 +39,10 @@ public FlyWheel(Motor motor, double maxAcceleration, double maxJerk, Gains gains
         this.m_FF_CONTROLLER = new SimpleMotorFeedforward(gains.ks, gains.kv, gains.ka);
     }
 
+    public FlyWheel(Motor motor, FlyWheel other) {
+        this(motor, other.maxAcceleration, other.maxJerk, other.m_gains);
+    }
+
     /**
      * @param velocitySupplier a dynamic velocity setpoint.
      * @return a command that controls the FlyWheels velocity with high precision

src/main/java/frc/excalib/mechanisms/turret/Turret.java

@@ -15,11 +15,16 @@
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
+import static java.lang.Double.NEGATIVE_INFINITY;
+import static java.lang.Double.POSITIVE_INFINITY;
+
 /**
  * A class representing a Turret Mechanism
  */
 public final class Turret extends Mechanism {
     private final ContinuousSoftLimit m_rotationLimit;
+    private final Gains m_gains;
+    private final double m_PIDtolerance;
     private final PIDController m_anglePIDcontroller;
     private final SimpleMotorFeedforward m_angleFFcontroller;
     private final DoubleSupplier m_POSITION_SUPPLIER;
@@ -35,6 +40,8 @@ public Turret(Motor motor, ContinuousSoftLimit rotationLimit, Gains angleGains,
         super(motor);
         m_rotationLimit = rotationLimit;
 
+        m_gains = angleGains;
+        m_PIDtolerance = PIDtolerance;
         m_anglePIDcontroller = new PIDController(angleGains.kp, angleGains.ki, angleGains.kd);
         m_angleFFcontroller = new SimpleMotorFeedforward(angleGains.ks, angleGains.kv, angleGains.ka);
 
@@ -44,6 +51,14 @@ public Turret(Motor motor, ContinuousSoftLimit rotationLimit, Gains angleGains,
         m_POSITION_SUPPLIER = positionSupplier;
     }
 
+    public Turret(Motor motor, DoubleSupplier positionSupplier, Turret other) {
+        this(motor, other.m_rotationLimit, other.m_gains, other.m_PIDtolerance, positionSupplier);
+    }
+
+    public Turret(Motor motor, Gains angleGains, double PIDtolerance, DoubleSupplier positionSupplier) {
+        this(motor, new ContinuousSoftLimit(() -> NEGATIVE_INFINITY, () -> POSITIVE_INFINITY), angleGains, PIDtolerance, positionSupplier);
+    }
+
     /**
      * @param wantedPosition a Rotation2d dynamic setpoint
      * @return a Command that moves the turret tho the given setpoint

src/main/java/frc/excalib/slam/mapper/Odometry.java → src/main/java/frc/excalib/slam/mapper/odometry/Odometry.java

@@ -1,4 +1,4 @@
-package frc.excalib.slam.mapper;
+package frc.excalib.slam.mapper.odometry;
 
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;

src/main/java/frc/excalib/slam/mapper/odometry/VisionMeasurement.java

@@ -0,0 +1,5 @@
+package frc.excalib.slam.mapper.odometry;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public record VisionMeasurement(Pose2d estimatedRobotPose, double timestampSeconds) {}

src/main/java/frc/excalib/swerve/ModulesHolder.java

@@ -11,9 +11,6 @@
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
-import static frc.robot.SwerveConstants.MAX_VEL;
-import static monologue.Annotations.Log;
-
 public class ModulesHolder implements Logged {
     public final SwerveModule m_frontLeft;
     public final SwerveModule m_frontRight;
@@ -42,7 +39,6 @@ public ModulesHolder(
         this.m_backLeft = backLeft;
         this.m_backRight = backRight;
 
-        // Initialize SwerveDriveKinematics once since module locations are constant
         this.m_swerveDriveKinematics = new SwerveDriveKinematics(
                 frontLeft.m_MODULE_LOCATION,
                 frontRight.m_MODULE_LOCATION,
@@ -59,56 +55,10 @@ public ModulesHolder(
     }
 
     /**
-     * Stops all swerve modules.
-     */
-    public void stop() {
-        m_frontLeft.stopModule();
-        m_frontRight.stopModule();
-        m_backLeft.stopModule();
-        m_backRight.stopModule();
-    }
-
-    /**
-     * Gets the robot's average velocity based on the velocities of all modules.
-     *
-     * @return a Vector2D representing the robot's velocity.
-     */
-    public Vector2D getVelocity() {
-        // Sum the velocities of all modules
-        double totalX = m_frontLeft.getVelocity().getX()
-                + m_frontRight.getVelocity().getX()
-                + m_backLeft.getVelocity().getX()
-                + m_backRight.getVelocity().getX();
-
-        double totalY = m_frontLeft.getVelocity().getY()
-                + m_frontRight.getVelocity().getY()
-                + m_backLeft.getVelocity().getY()
-                + m_backRight.getVelocity().getY();
-
-        // Compute the average velocity
-        return new Vector2D(totalX * 0.25, totalY * 0.25);
-    }
-
-    @Log.NT(key = "angular vel")
-    public double getOmegaRadPerSec() {
-        return new SwerveDriveKinematics(
-                m_frontLeft.m_MODULE_LOCATION,
-                m_frontRight.m_MODULE_LOCATION,
-                m_backLeft.m_MODULE_LOCATION,
-                m_backRight.m_MODULE_LOCATION
-        ).toChassisSpeeds(logStates()).omegaRadiansPerSecond;
-    }
-
-    @Log.NT(key = "swerve velocity")
-    public double getVelocityDistance() {
-        return getVelocity().getDistance();
-    }
-
-    /**
-     * Calculates the minimum velocity ratio limit among all modules.
+     * A method that calculates the minimum velocity ratio limit among all modules.
      *
-     * @param translationVelocity The desired translation velocity.
-     * @param omegaRadPerSec      The desired rotation rate in radians per second.
+     * @param translationVelocity The desired translation velocity of the robot.
+     * @param omegaRadPerSec      The desired rotation rate of the robot in radians per second.
      * @return The velocity ratio limit.
      */
     private double calcVelocityRatioLimit(Vector2D translationVelocity, double omegaRadPerSec) {
@@ -122,13 +72,13 @@ private double calcVelocityRatioLimit(Vector2D translationVelocity, double omega
     }
 
     /**
-     * Sets the velocities of all modules based on the desired translation and rotation velocities.
+     * A method that sets the velocities of all modules based on the desired translation and rotation velocities of the robot.
      *
      * @param omega            The desired rotation rate supplier.
      * @param translationalVel The desired translation velocity supplier.
-     * @return A command to set the velocities.
+     * @return A command that sets the velocities.
      */
-    public Command setVelocitiesCommand(Supplier<Vector2D> translationalVel, DoubleSupplier omega) {
+    Command setVelocitiesCommand(Supplier<Vector2D> translationalVel, DoubleSupplier omega) {
         return new ParallelCommandGroup(
                 m_frontLeft.setVelocityCommand(
                         translationalVel,
@@ -153,7 +103,20 @@ public Command setVelocitiesCommand(Supplier<Vector2D> translationalVel, DoubleS
         );
     }
 
-    public Command coastCommand() {
+    /**
+     * A method that stops all swerve modules.
+     */
+    void stop() {
+        m_frontLeft.stopModule();
+        m_frontRight.stopModule();
+        m_backLeft.stopModule();
+        m_backRight.stopModule();
+    }
+
+    /**
+     * @return A Command that sets the idle state of all modules to coast.
+     */
+    Command coastCommand() {
         return new ParallelCommandGroup(
                 m_frontLeft.coastCommand(),
                 m_frontRight.coastCommand(),
@@ -162,36 +125,80 @@ public Command coastCommand() {
         );
     }
 
-    public void setModulesStates(SwerveModuleState[] states) {
-        SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_VEL);
+    /**
+     * A method that sets the states of the modules to the desired states.
+     *
+     * @param states an array represents the wanted states of the modules.
+     */
+    void setModulesStates(SwerveModuleState[] states) {
         m_frontLeft.setDesiredState(states[0]);
         m_frontRight.setDesiredState(states[1]);
         m_backLeft.setDesiredState(states[2]);
         m_backRight.setDesiredState(states[3]);
     }
 
     /**
-     * Logs the states of all modules.
+     * A method to get the robot's average velocity based on the velocities of all modules.
+     *
+     * @return a Vector2D represents the robot's velocity.
+     */
+    Vector2D getVelocity() {
+        // Sum the velocities of all modules
+        double totalX = m_frontLeft.getVelocity().getX()
+                + m_frontRight.getVelocity().getX()
+                + m_backLeft.getVelocity().getX()
+                + m_backRight.getVelocity().getX();
+
+        double totalY = m_frontLeft.getVelocity().getY()
+                + m_frontRight.getVelocity().getY()
+                + m_backLeft.getVelocity().getY()
+                + m_backRight.getVelocity().getY();
+
+        // Compute the average velocity
+        return new Vector2D(totalX * 0.25, totalY * 0.25);
+    }
+
+    /**
+     * A method to get the linear velocity of the robot.
+     *
+     * @return the linear velocity of the robot.
+     */
+    double getVelocityDistance() {
+        return new Vector2D(
+                m_swerveDriveKinematics.toChassisSpeeds(getStates()).vxMetersPerSecond,
+                m_swerveDriveKinematics.toChassisSpeeds(getStates()).vyMetersPerSecond
+        ).getDistance();
+    }
+
+    /**
+     * A method to get the angular velocity of the robot.
+     *
+     * @return the angular velocity of the robot.
+     */
+    double getOmegaRadPerSec() {
+        return m_swerveDriveKinematics.toChassisSpeeds(getStates()).omegaRadiansPerSecond;
+    }
+
+    /**
+     * A method to get the states of all modules.
      *
      * @return An array of SwerveModuleState representing the states of the modules.
      */
-    @Log.NT(key = "Swerve States")
-    public SwerveModuleState[] logStates() {
+    SwerveModuleState[] getStates() {
         return new SwerveModuleState[]{
-                m_frontLeft.logState(),
-                m_frontRight.logState(),
-                m_backLeft.logState(),
-                m_backRight.logState()
+                m_frontLeft.getState(),
+                m_frontRight.getState(),
+                m_backLeft.getState(),
+                m_backRight.getState()
         };
     }
 
-    @Log.NT(key = "SetPoints")
-    public SwerveModuleState[] logSetPointStates() {
+    SwerveModuleState[] getDesiredStates() {
         return new SwerveModuleState[]{
-                m_frontLeft.logSetpointState(),
-                m_frontRight.logSetpointState(),
-                m_backLeft.logSetpointState(),
-                m_backRight.logSetpointState()
+                m_frontLeft.getSetpointState(),
+                m_frontRight.getSetpointState(),
+                m_backLeft.getSetpointState(),
+                m_backRight.getSetpointState()
         };
     }
 
@@ -200,7 +207,7 @@ public SwerveModuleState[] logSetPointStates() {
      *
      * @return The SwerveDriveKinematics instance.
      */
-    public SwerveDriveKinematics getSwerveDriveKinematics() {
+    SwerveDriveKinematics getSwerveDriveKinematics() {
         return m_swerveDriveKinematics;
     }
 
@@ -209,7 +216,7 @@ public SwerveDriveKinematics getSwerveDriveKinematics() {
      *
      * @return An array of SwerveModulePosition representing the positions of the modules.
      */
-    public SwerveModulePosition[] getModulesPositions() {
+    SwerveModulePosition[] getModulesPositions() {
         return m_modulePositions;
     }