made pathSequenceAuto compatible with drive abstraction

Team766 · Nov 21, 2024 · 0f5da0f · 0f5da0f
1 parent eab5972
commit 0f5da0f
Show file tree

Hide file tree

Showing 13 changed files with 60 additions and 33 deletions.
diff --git a/src/main/java/com/team766/robot/common/DriverOI.java b/src/main/java/com/team766/robot/common/DriverOI.java
@@ -52,7 +52,7 @@ public void handleOI(Context context) {
                         * ControlConstants.MAX_ROTATIONAL_VELOCITY; // For steer
 
         if (leftJoystick.getButtonPressed(InputConstants.BUTTON_RESET_GYRO)) {
-            drive.resetGyro();
+            drive.resetHeading();
         }
 
         if (leftJoystick.getButtonPressed(InputConstants.BUTTON_RESET_POS)) {

diff --git a/src/main/java/com/team766/robot/common/mechanisms/BurroDrive.java b/src/main/java/com/team766/robot/common/mechanisms/BurroDrive.java
@@ -2,7 +2,6 @@
 
 import static com.team766.robot.common.constants.ConfigConstants.*;
 
-import com.team766.hal.GyroReader;
 import com.team766.hal.MotorController;
 import com.team766.hal.MotorController.ControlMode;
 import com.team766.hal.RobotProvider;
@@ -45,10 +44,11 @@ public BurroDrive(double trackWidthMeters) {
                 new DifferentialDriveOdometry(
                         leftMotor,
                         rightMotor,
-                        5 /* todo */,
+                        WHEEL_RADIUS * 2 * Math.PI,
                         DRIVE_GEAR_RATIO,
-                        0 /* todo */,
-                        WHEEL_RADIUS);
+                        1.,
+                        0 // TODO
+                        );
     }
 
     /**
@@ -78,6 +78,20 @@ public void resetCurrentPosition() {
         differentialDriveOdometry.setCurrentPosition(new Pose2d(0, 0, new Rotation2d()));
     }
 
+    public void setCurrentPosition(Pose2d P) {
+        differentialDriveOdometry.setCurrentPosition(P);
+    }
+
+    public void resetHeading(double angle) {
+        Pose2d curPose = getCurrentPosition();
+        differentialDriveOdometry.setCurrentPosition(
+                new Pose2d(curPose.getX(), curPose.getY(), Rotation2d.fromDegrees(angle)));
+    }
+
+    public double getHeading() {
+        return getCurrentPosition().getRotation().getDegrees();
+    }
+
     /*
      * Stops each drive motor
      */
@@ -88,7 +102,10 @@ public void stopDrive() {
     }
 
     public ChassisSpeeds getChassisSpeeds() {
-        return new ChassisSpeeds(); // todo
+        return differentialDriveKinematics.toChassisSpeeds(
+                new DifferentialDriveWheelSpeeds(
+                        leftMotor.getSensorVelocity() / MOTOR_WHEEL_FACTOR_MPS,
+                        rightMotor.getSensorVelocity() / MOTOR_WHEEL_FACTOR_MPS));
     }
 
     public void setCross() {}

diff --git a/src/main/java/com/team766/robot/common/mechanisms/Drive.java b/src/main/java/com/team766/robot/common/mechanisms/Drive.java
@@ -21,7 +21,13 @@ public abstract class Drive extends Mechanism {
 
     public abstract void resetCurrentPosition();
 
+    public abstract double getHeading();
+
+    public abstract void resetHeading(double angle);
+
+    public abstract void setCurrentPosition(Pose2d P);
+
     public abstract ChassisSpeeds getChassisSpeeds();
 
     public abstract void setCross(); // dummy method
-}
+}
diff --git a/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java b/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java
@@ -314,15 +314,15 @@ public void setCross() {
      * Resets gyro to zero degrees relative to the driver
      * Sets to 180 degrees if the driver is on red (facing backwards)
      */
-    public void resetGyro() {
-        resetGyro(alliance.isPresent() && alliance.get() == Alliance.Blue ? 0 : 180);
+    public void resetHeading() {
+        resetHeading(alliance.isPresent() && alliance.get() == Alliance.Blue ? 0 : 180);
     }
 
     /**
      * Sets gyro to value in degrees
      * @param angle in degrees
      */
-    public void resetGyro(double angle) {
+    public void resetHeading(double angle) {
         checkContextOwnership();
         gyro.setAngle(angle);
     }

diff --git a/src/main/java/com/team766/robot/common/procedures/FollowPath.java b/src/main/java/com/team766/robot/common/procedures/FollowPath.java
@@ -112,15 +112,15 @@ public void run(Context context) {
                     "input rotational velocity", targetSpeeds.omegaRadiansPerSecond);
             org.littletonrobotics.junction.Logger.recordOutput(
                     "targetState", targetState.getTargetHolonomicPose());
-            
+
             drive.controlRobotOriented(targetSpeeds);
             context.yield();
         }
 
         if (path.getGoalEndState().getVelocity() < 0.1) {
             drive.stopDrive();
             if (controller.isHolonomic()) {
-                drive.setCross(); 
+                drive.setCross();
             }
         }
     }

diff --git a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java
@@ -1,6 +1,8 @@
 package com.team766.robot.common.procedures;
 
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.controllers.PPLTVController;
+import com.pathplanner.lib.controllers.PathFollowingController;
 import com.pathplanner.lib.util.GeometryUtil;
 import com.pathplanner.lib.util.PIDConstants;
 import com.team766.config.ConfigFileReader;
@@ -9,9 +11,10 @@
 import com.team766.framework.RunnableWithContext;
 import com.team766.robot.common.constants.ConfigConstants;
 import com.team766.robot.common.constants.PathPlannerConstants;
+import com.team766.robot.common.mechanisms.BurroDrive;
+import com.team766.robot.common.mechanisms.Drive;
 import com.team766.robot.common.mechanisms.SwerveDrive;
 import com.team766.robot.reva.Robot;
-import com.team766.robot.reva.VisionUtil.VisionSpeakerHelper;
 import com.team766.robot.reva.procedures.MoveClimbersToBottom;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -22,25 +25,27 @@
 public class PathSequenceAuto extends Procedure {
 
     private final LinkedList<RunnableWithContext> pathItems;
-    private final SwerveDrive drive;
+    private final Drive drive;
     private final Pose2d initialPosition;
-    private final PPHolonomicDriveController controller;
-    private VisionSpeakerHelper visionSpeakerHelper;
+    private final PathFollowingController controller;
 
     /**
      * Sequencer for using path following with other procedures
      * @param drive The instantiation of drive for the robot (pass in Robot.drive)
      * @param initialPosition Starting position on Blue Alliance in meters (gets flipped when on red)
      */
-    public PathSequenceAuto(SwerveDrive drive, Pose2d initialPosition) {
+    public PathSequenceAuto(Drive drive, Pose2d initialPosition) {
         pathItems = new LinkedList<RunnableWithContext>();
         this.drive = drive;
-        this.controller = createDriveController(drive);
+        if (drive instanceof SwerveDrive) {
+            this.controller = createHolonomicDriveController((SwerveDrive) drive);
+        } else {
+            this.controller = new PPLTVController(0.02);
+        }
         this.initialPosition = initialPosition;
-        visionSpeakerHelper = new VisionSpeakerHelper(drive);
     }
 
-    private PPHolonomicDriveController createDriveController(SwerveDrive drive) {
+    private PPHolonomicDriveController createHolonomicDriveController(SwerveDrive drive) {
         double maxSpeed =
                 ConfigFileReader.getInstance()
                         .getDouble(ConfigConstants.PATH_FOLLOWING_MAX_MODULE_SPEED_MPS)
@@ -105,12 +110,11 @@ public final void run(Context context) {
 
         context.startAsync(new MoveClimbersToBottom());
         context.takeOwnership(drive);
-        // if (!visionSpeakerHelper.updateTarget(context)) {
         drive.setCurrentPosition(
                 shouldFlipAuton ? GeometryUtil.flipFieldPose(initialPosition) : initialPosition);
-        // }
+
         // context.takeOwnership(drive);
-        drive.resetGyro(
+        drive.resetHeading(
                 (shouldFlipAuton
                                 ? GeometryUtil.flipFieldRotation(initialPosition.getRotation())
                                 : initialPosition.getRotation())
@@ -128,7 +132,7 @@ public final void run(Context context) {
         // TODO: We should figure out why after EBR but for now we can just reset the gyro to 180 of
         // current angle
         context.takeOwnership(drive);
-        drive.resetGyro(180 + drive.getHeading());
+        drive.resetHeading(180 + drive.getHeading());
         context.releaseOwnership(drive);
     }
 }
diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OPECHelper.java b/src/main/java/com/team766/robot/gatorade/procedures/OPECHelper.java
@@ -14,7 +14,7 @@ public void run(Context context) {
         // context.takeOwnership(Robot.intake);
         double startY = Robot.drive.getCurrentPosition().getY();
         // robot gyro is offset 90º from how we want, so we reset it to 90º to account for this
-        Robot.drive.resetGyro();
+        Robot.drive.resetHeading();
         // context.runSync(new IntakeRelease());
         Robot.drive.controlFieldOriented(0, -FollowPointsInputConstants.SPEED, 0);
         context.waitFor(() -> Math.abs(Robot.drive.getCurrentPosition().getY() - startY) > DIST);

diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceBalance.java
@@ -21,7 +21,7 @@ public void run(Context context) {
         context.takeOwnership(Robot.drive);
         // TODO: how do we reset the gyro at 180 degrees?
         // Robot.drive.resetGyro180();
-        Robot.drive.resetGyro();
+        Robot.drive.resetHeading();
 
         Optional<Alliance> alliance = DriverStation.getAlliance();
         if (alliance.isPresent()) {

diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunity.java
@@ -20,7 +20,7 @@ public OnePieceExitCommunity(GamePieceType type) {
     public void run(Context context) {
         context.takeOwnership(Robot.drive);
         // context.takeOwnership(Robot.intake);
-        Robot.drive.resetGyro();
+        Robot.drive.resetHeading();
 
         Optional<Alliance> alliance = DriverStation.getAlliance();
 

diff --git a/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/OnePieceExitCommunityBalance.java
@@ -20,7 +20,7 @@ public OnePieceExitCommunityBalance(GamePieceType type) {
     public void run(Context context) {
         context.takeOwnership(Robot.drive);
         // context.takeOwnership(Robot.intake);
-        Robot.drive.resetGyro();
+        Robot.drive.resetHeading();
 
         Optional<Alliance> alliance = DriverStation.getAlliance();
 

diff --git a/src/main/java/com/team766/robot/reva/DriverOI.java b/src/main/java/com/team766/robot/reva/DriverOI.java
@@ -80,7 +80,7 @@ protected void handlePre() {
     protected void handleOI(Context context) {
 
         if (leftJoystick.getButtonPressed(InputConstants.BUTTON_RESET_GYRO)) {
-            drive.resetGyro();
+            drive.resetHeading();
         }
 
         if (leftJoystick.getButtonPressed(InputConstants.BUTTON_RESET_POS)) {

diff --git a/src/main/java/com/team766/robot/swerveandshoot/OI.java b/src/main/java/com/team766/robot/swerveandshoot/OI.java
@@ -38,7 +38,7 @@ public void run(final Context context) {
             // General drive util
 
             if (joystick0.getButtonPressed(2)) {
-                Robot.drive.resetGyro();
+                Robot.drive.resetHeading();
             }
             if (Math.abs(joystick0.getAxis(0))
                             + Math.abs(joystick0.getAxis(1))

diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2024.1.6",
+    "version": "2024.2.8",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2024",
     "mavenUrls": [
@@ -12,15 +12,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2024.1.6"
+            "version": "2024.2.8"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.1.6",
+            "version": "2024.2.8",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,