bug fixes for 2023

Author: CoolSpy3

src/main/java/org/carlmontrobotics/lib199/OmniDrive.java

@@ -60,6 +60,16 @@ public interface DifferentialDriveInterface extends DrivetrainInterface {
      */
     public void setOdometry(DifferentialDriveOdometry odometry);
 
+    /**
+     * @return The left encoder position in meters
+     */
+    public double getLeftEncoderPosition();
+
+    /**
+     * @return The right encoder position in meters
+     */
+    public double getRightEncoderPosition();
+
     /**
      * Creates Ramsete Controller
      * 
@@ -105,14 +115,13 @@ public default Command createAutoCommand(Trajectory trajectory, Supplier<Rotatio
     }
 
     /**
-     * Sets odometry based on current gyro angle and pose
+     * Sets odometry to the specified pose
      * 
-     * @param gyroAngle   The angle reported by the gyroscope.
      * @param initialPose The starting position of the robot on the field.
      */
     @Override
-    public default void setOdometry(Rotation2d gyroAngle, Pose2d initialPose) {
-        setOdometry(new DifferentialDriveOdometry(gyroAngle, initialPose));
+    public default void setOdometry(Pose2d initialPose) {
+        setOdometry(new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeadingDeg()), getLeftEncoderPosition(), getRightEncoderPosition(), initialPose));
     }
 
 }

src/main/java/org/carlmontrobotics/lib199/path/DifferentialDriveInterface.java

@@ -50,12 +50,11 @@ public interface DrivetrainInterface extends Subsystem {
     public double getHeadingDeg();
 
     /**
-     * Sets odometry based on current gyro angle and pose
+     * Sets odometry to the specified pose
      * 
-     * @param gyroAngle   The angle reported by the gyroscope.
      * @param initialPose The starting position of the robot on the field.
      */
-    public void setOdometry(Rotation2d gyroAngle, Pose2d initialPose);
+    public void setOdometry(Pose2d initialPose);
 
     /**
      * Stops the drivetrain

src/main/java/org/carlmontrobotics/lib199/path/DrivetrainInterface.java

@@ -15,15 +15,15 @@
 import org.apache.commons.csv.CSVParser;
 import org.apache.commons.csv.CSVRecord;
 
-import edu.wpi.first.wpilibj.Filesystem;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
@@ -107,7 +107,7 @@ public void initializeDrivetrainPosition() {
         if (trajectory == null) {
             generateTrajectory();
         }
-        dt.setOdometry(Rotation2d.fromDegrees(dt.getHeadingDeg()), trajectory.getInitialPose());
+        dt.setOdometry(trajectory.getInitialPose());
     }
 
     /**

src/main/java/org/carlmontrobotics/lib199/path/RobotPath.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
@@ -55,6 +56,11 @@ public interface SwerveDriveInterface extends DrivetrainInterface {
      */
     public void setOdometry(SwerveDriveOdometry odometry);
 
+    /**
+     * @return The current positions of the swerve modules
+     */
+    public SwerveModulePosition[] getModulePositions();
+
     /**
      * Configures the constants for generating a trajectory
      * 
@@ -95,14 +101,13 @@ public default Command createAutoCommand(Trajectory trajectory, Supplier<Rotatio
     }
 
     /**
-     * Sets odometry based on current kinematics, gyro angle, and pose
+     * Sets odometry to the specified pose
      * 
-     * @param gyroAngle   The angle reported by the gyroscope.
      * @param initialPose The starting position of the robot on the field.
      */
     @Override
-    public default void setOdometry(Rotation2d gyroAngle, Pose2d initialPose) {
-        setOdometry(new SwerveDriveOdometry(getKinematics(), gyroAngle, initialPose));
+    public default void setOdometry(Pose2d initialPose) {
+        setOdometry(new SwerveDriveOdometry(getKinematics(), Rotation2d.fromDegrees(getHeadingDeg()), getModulePositions(), initialPose));
     }
 
 }

src/main/java/org/carlmontrobotics/lib199/path/SwerveDriveInterface.java

@@ -1,4 +1,4 @@
-package org.carlmontrobotics.lib199;
+package org.carlmontrobotics.lib199.swerve;
 
 public final class SwerveConfig {
 

…arlmontrobotics/lib199/SwerveConfig.java …robotics/lib199/swerve/SwerveConfig.javasrc/main/java/org/carlmontrobotics/lib199/SwerveConfig.java renamed to src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java src/main/java/org/carlmontrobotics/lib199/SwerveConfig.java renamed to src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java

@@ -1,4 +1,4 @@
-package org.carlmontrobotics.lib199;
+package org.carlmontrobotics.lib199.swerve;
 
 import java.util.function.Supplier;
 
@@ -12,6 +12,7 @@
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Timer;
@@ -242,6 +243,18 @@ public SwerveModuleState getCurrentState() {
         return new SwerveModuleState(getCurrentSpeed(), Rotation2d.fromDegrees(getModuleAngle()));
     }
 
+    /**
+     * Gets the current position (distance and angle) of this module.
+     * @return A SwerveModulePosition object representing the distance and angle of the module.
+     */
+    public SwerveModulePosition getCurrentPosition() {
+        return new SwerveModulePosition(getCurrentDistance(), Rotation2d.fromDegrees(getModuleAngle()));
+    }
+
+    public double getCurrentDistance() {
+        return drive.getEncoder().getPosition();
+    }
+
     public double getCurrentSpeed() {
         return drive.getEncoder().getVelocity();
     }