Merge pull request #19 from Merridew1/Odometry

Odometry
Merridew1 · Dec 10, 2024 · 7bb5aa6 · 7bb5aa6
2 parents 27ec070 + 886863a
commit 7bb5aa6
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java
@@ -3,6 +3,7 @@
 import org.littletonrobotics.junction.Logger;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -19,6 +20,8 @@ public class Drivetrain extends SubsystemBase {
         Constants.PID.DriveTrain.I, Constants.PID.DriveTrain.D);
     PIDController driveTrainLeftPIDController = new PIDController(0, 0, 0);
     SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0, 0);
+    private DifferentialDriveOdometry odometry =
+        new DifferentialDriveOdometry(inputs.gyroYaw, inputs.leftDistance, inputs.rightDistance);
 
     /**
      * Create Wrist Intake Subsystem
@@ -31,6 +34,7 @@ public Drivetrain(DrivetrainIO io) {
     public void periodic() {
         io.updateInputs(inputs);
         Logger.processInputs("Drivetrain", inputs);
+        odometry.update(inputs.gyroYaw, inputs.leftDistance, inputs.rightDistance);
     }
 
     public void drive(Double lPower, double rPower) {
@@ -42,7 +46,7 @@ public void drive(Double lPower, double rPower) {
         Logger.recordOutput("Drivetrain/leftPower", lPower);
         Logger.recordOutput("Drivetrain/rightPower", rPower);
     }
-
+ 
     public Command driveCommand(CommandXboxController controller) {
         return run(() -> drive(controller.getLeftY(), controller.getRightY()));
     }

diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java
@@ -15,6 +15,8 @@ public static class DrivetrainIOInputs {
         public Rotation2d gyroYaw = new Rotation2d();
         public double rightVelocity;
         public double leftVelocity;
+        public double rightDistance;
+        public double leftDistance;
     }
 
     /** Updates the set of loggable inputs. */

diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java
@@ -17,6 +17,8 @@ public class DrivetrainReal implements DrivetrainIO {
     TalonFX right2 = new TalonFX(Constants.Motors.DriveTrain.BACK_RIGHT_MOTOR_ID, "CANivore");
     StatusSignal<Double> rightVelocity = right1.getVelocity();
     StatusSignal<Double> leftVelocity = left1.getVelocity();
+    StatusSignal<Double> leftDistance = left1.getPosition();
+    StatusSignal<Double> rightDistance = right1.getPosition();
 
     /**
      * Drivetrain Real
@@ -31,6 +33,8 @@ public void updateInputs(DrivetrainIOInputs inputs) {
         inputs.gyroYaw = Rotation2d.fromDegrees(0);
         inputs.leftVelocity = leftVelocity.getValueAsDouble();
         inputs.rightVelocity = rightVelocity.getValueAsDouble();
+        inputs.leftDistance = leftDistance.getValueAsDouble();
+        inputs.rightDistance = rightDistance.getValueAsDouble();
 
     }