Basic auto with VTS

Mechanical-Advantage · Feb 12, 2024 · ce023f4 · ce023f4
1 parent 903ef58
commit ce023f4
Show file tree

Hide file tree

Showing 11 changed files with 31 additions and 11 deletions.
diff --git a/src/main/deploy/trajectories/.gitkeep b/src/main/deploy/trajectories/.gitkeep
diff --git a/src/main/deploy/trajectories/driveStraight.pathblob b/src/main/deploy/trajectories/driveStraight.pathblob
diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java
@@ -39,6 +39,6 @@ private Command reset(String path) {
   }
 
   public Command driveStraight() {
-    return null; // reset("driveStraight").andThen(path("driveStraight"));
+    return reset("driveStraight").andThen(path("driveStraight"));
   }
 }
diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java
@@ -179,8 +179,8 @@ public void periodic() {
               wheelPositions.positions[j].angle.minus(lastPositions.positions[j].angle).getRadians()
                   / dt;
 
-          if (Math.abs(omega) > currentModuleLimits.maxSteeringVelocity() * 10.0
-              || Math.abs(velocity) > currentModuleLimits.maxDriveVelocity() * 10.0) {
+          if (Math.abs(omega) > currentModuleLimits.maxSteeringVelocity() * 100.0
+              || Math.abs(velocity) > currentModuleLimits.maxDriveVelocity() * 100.0) {
             includeMeasurement = false;
             break;
           }

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java
@@ -143,7 +143,7 @@ public static class KrakenDriveConstants {
             new TrajectoryConstants(
                 4.0,
                 0.0,
-                2.0,
+                4.0,
                 0.0,
                 Units.inchesToMeters(4.0),
                 Units.degreesToRadians(5.0),

diff --git a/...java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TrajectoryController.java b/...java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TrajectoryController.java
@@ -57,7 +57,7 @@ public ChassisSpeeds update() {
         trajectoryLinearkP,
         trajectoryLinearkD,
         trajectoryThetakP,
-        trajectoryThetakP);
+        trajectoryThetakD);
 
     // Run trajectory
     Pose2d currentPose = RobotState.getInstance().getEstimatedPose();

diff --git a/...ain/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java b/...ain/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java
@@ -4,6 +4,7 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
 import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
@@ -18,9 +19,22 @@ public class DriveTrajectories {
         "driveStraight",
         List.of(
             PathSegment.newBuilder()
-                .addPoseWaypoint(new Pose2d())
-                .addPoseWaypoint(new Pose2d(2.0, 0.0, new Rotation2d()), 100)
-                .addPoseWaypoint(new Pose2d(4.0, 2.0, new Rotation2d(Math.PI)))
+                .addPoseWaypoint(
+                    new Pose2d(
+                        Units.inchesToMeters(60.112),
+                        Units.inchesToMeters(161.638),
+                        new Rotation2d()))
+                .addPoseWaypoint(
+                    new Pose2d(
+                        Units.inchesToMeters(130), Units.inchesToMeters(70), new Rotation2d()))
+                .addPoseWaypoint(
+                    new Pose2d(
+                        Units.inchesToMeters(318.640),
+                        Units.inchesToMeters(29.003),
+                        new Rotation2d()))
+                .addPoseWaypoint(
+                    new Pose2d(
+                        Units.inchesToMeters(130), Units.inchesToMeters(70), new Rotation2d()))
                 .build()));
   }
 }
diff --git a/.../java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java b/.../java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java
@@ -73,6 +73,7 @@ public static void main(String[] args) {
           PathRequest.newBuilder().setModel(model).addAllSegments(entry.getValue()).build();
 
       TrajectoryResponse response = service.generateTrajectory(request);
+      System.out.println(response.getError());
 
       String responseHashCode = getHashCode(model, entry.getValue());
 

diff --git a/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java b/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java
@@ -67,7 +67,7 @@ public static VehicleState apply(VehicleState state) {
           .setTheta(apply(new Rotation2d(state.getTheta())).getRadians())
           .setVx(-state.getVx())
           .setVy(state.getVy())
-          .setOmega(state.getOmega())
+          .setOmega(-state.getOmega())
           .build();
     } else {
       return state;

diff --git a/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicDriveController.java b/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicDriveController.java
@@ -2,6 +2,7 @@
 
 import static org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceOuterClass.*;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -63,7 +64,8 @@ public ChassisSpeeds calculate(VehicleState currentState, VehicleState setpointS
     double yFeedback = linearFeedback * currentToStateAngle.getSin();
     double thetaFeedback =
         thetaController.calculate(
-            currentPose.getRotation().getRadians(), setpointPose.getRotation().getRadians());
+            MathUtil.angleModulus(currentPose.getRotation().getRadians()),
+            MathUtil.angleModulus(setpointPose.getRotation().getRadians()));
 
     // Return next output.
     return ChassisSpeeds.fromFieldRelativeSpeeds(

diff --git a/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicTrajectory.java b/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicTrajectory.java
@@ -5,6 +5,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Filesystem;
 import java.io.File;
 import java.io.FileInputStream;
 import java.io.IOException;
@@ -16,7 +17,9 @@ public class HolonomicTrajectory {
   private final Trajectory trajectory;
 
   public HolonomicTrajectory(String name) {
-    File file = Path.of("src", "main", "deploy", "trajectories", name + ".pathblob").toFile();
+    File file =
+        Path.of(Filesystem.getDeployDirectory().getPath(), "trajectories", name + ".pathblob")
+            .toFile();
     try {
       InputStream fileStream = new FileInputStream(file);
       trajectory = Trajectory.parseFrom(fileStream);