From 3476f890bdfd2da682cc8900fc59b4d6df9079df Mon Sep 17 00:00:00 2001 From: Merridew1 Date: Mon, 9 Dec 2024 20:04:21 -0600 Subject: [PATCH 1/2] Differential odometry completed* --- src/main/java/frc/robot/subsystems/drive/Drivetrain.java | 4 ++++ src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java | 2 ++ src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java | 4 ++++ 3 files changed, 10 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index 811addc..5ebc570 100644 --- 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) { diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java index b6aa6e1..88a983f 100644 --- 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 index 04d70ae..6a6b74e 100644 --- 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 rightVelocity = right1.getVelocity(); StatusSignal leftVelocity = left1.getVelocity(); + StatusSignal leftDistance = left1.getPosition(); + StatusSignal 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(); } From 886863a5cc6a00b880cf15c3d4b682db447b635f Mon Sep 17 00:00:00 2001 From: Merridew1 Date: Mon, 9 Dec 2024 21:22:14 -0600 Subject: [PATCH 2/2] Space --- 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 5ebc570..554f5e4 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -46,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())); }