Potential Choreo - might get reverted

wildstang · Jan 6, 2024 · 0385854 · 0385854
1 parent 41aaf84
commit 0385854
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 4 deletions.
diff --git a/src/main/java/org/wildstang/framework/auto/steps/SwervePathFollowerStep.java b/src/main/java/org/wildstang/framework/auto/steps/SwervePathFollowerStep.java
@@ -9,11 +9,14 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
+import com.choreo.lib.*;
+
 public class SwervePathFollowerStep extends AutoStep {
 
     private static final double mToIn = 39.3701;
     private SwerveDriveTemplate m_drive;
     private PathPlannerTrajectory pathData;
+    private ChoreoTrajectory pathtraj;
     private boolean isBlue;
 
     private double xOffset, yOffset;
@@ -27,9 +30,10 @@ public class SwervePathFollowerStep extends AutoStep {
      * @param drive the swerveDrive subsystem
      * @param isBlue whether the robot is on the blue alliance
      */
-    public SwervePathFollowerStep(PathPlannerTrajectory pathData, SwerveDriveTemplate drive, boolean isBlue) {
+    public SwervePathFollowerStep(PathPlannerTrajectory pathData, ChoreoTrajectory traj, SwerveDriveTemplate drive, boolean isBlue) {
         this.pathData = pathData;
         m_drive = drive;
+        pathtraj = traj;
 
         this.isBlue = isBlue;
         timer = new Timer();
@@ -69,11 +73,13 @@ public String toString() {
     }
 
     public double getVelocity(){
-        return pathData.sample(timer.get()).velocityMetersPerSecond * mToIn;
+        //return pathData.sample(timer.get()).velocityMetersPerSecond * mToIn;
+        return Math.hypot(pathtraj.sample(timer.get()).velocityX, pathtraj.sample(timer.get()).velocityY);
     }
     public double getHeading(){
-        if (isBlue) return (-pathData.sample(timer.get()).poseMeters.getRotation().getDegrees() + 360)%360;
-        else return (pathData.sample(timer.get()).poseMeters.getRotation().getDegrees()+360)%360;
+        //if (isBlue) return (-pathData.sample(timer.get()).poseMeters.getRotation().getDegrees() + 360)%360;
+        //else return (pathData.sample(timer.get()).poseMeters.getRotation().getDegrees()+360)%360;
+        return pathtraj.sample(timer.get()).heading*180/Math.PI;
     }
     public double getAccel(){
         return pathData.sample(timer.get()).accelerationMetersPerSecondSq * mToIn * mToIn;

diff --git a/vendordeps/ChoreoLib.json b/vendordeps/ChoreoLib.json
@@ -0,0 +1,25 @@
+{
+    "fileName": "ChoreoLib.json",
+    "name": "ChoreoLib",
+    "version": "2024.0.4",
+    "uuid": "287cff6e-1b60-4412-8059-f6834fb30e30",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://SleipnirGroup.github.io/ChoreoLib/dep"
+    ],
+    "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.choreo.lib",
+            "artifactId": "ChoreoLib",
+            "version": "2024.0.4"
+        },
+        {
+            "groupId": "com.google.code.gson",
+            "artifactId": "gson",
+            "version": "2.10.1"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": []
+}