more changes

Team612 · Mar 19, 2024 · 106d1cc · 106d1cc
1 parent 134f000
commit 106d1cc
Show file tree

Hide file tree

Showing 8 changed files with 161 additions and 29 deletions.
diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
@@ -3,12 +3,14 @@
   "robotLength": 0.4631436,
   "holonomicMode": true,
   "pathFolders": [
+    "Left Paths",
     "Middle Paths",
-    "Left Paths"
+    "Right Paths"
   ],
   "autoFolders": [
+    "Left",
     "Middle",
-    "Left"
+    "Right"
   ],
   "defaultMaxVel": 3.0,
   "defaultMaxAccel": 3.0,

diff --git a/src/main/deploy/pathplanner/autos/Left 2 Note Auto.auto b/src/main/deploy/pathplanner/autos/Left 2 Note Auto.auto
@@ -47,9 +47,9 @@
           "data": {
             "commands": [
               {
-                "type": "named",
+                "type": "path",
                 "data": {
-                  "name": "Apriltag Align"
+                  "pathName": "Left-Subwoofer"
                 }
               },
               {

diff --git a/src/main/deploy/pathplanner/autos/Right Two Note.auto b/src/main/deploy/pathplanner/autos/Right Two Note.auto
@@ -0,0 +1,18 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2,
+      "y": 2
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": []
+    }
+  },
+  "folder": "Right",
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/Leave Right.path b/src/main/deploy/pathplanner/paths/Leave Right.path
@@ -0,0 +1,58 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.6699236223008809,
+        "y": 4.543804579763226
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.6699236223008804,
+        "y": 4.543804579763226
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.8481392198162414,
+        "y": 4.098792791023527
+      },
+      "prevControl": {
+        "x": 1.8481392198162414,
+        "y": 4.098792791023527
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.45,
+      "rotationDegrees": 0.0,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Right Paths",
+  "previewStartingState": {
+    "rotation": -57.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/...eploy/pathplanner/paths/Back to Left.path → ...loy/pathplanner/paths/Left-Subwoofer.path b/...eploy/pathplanner/paths/Back to Left.path → ...loy/pathplanner/paths/Left-Subwoofer.path
@@ -28,13 +28,7 @@
       "linkedName": null
     }
   ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 0.25,
-      "rotationDegrees": 0,
-      "rotateFast": false
-    }
-  ],
+  "rotationTargets": [],
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
@@ -45,13 +39,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0,
+    "rotation": 56.0,
     "rotateFast": false
   },
   "reversed": false,
   "folder": "Left Paths",
   "previewStartingState": {
-    "rotation": 57.0,
+    "rotation": 0.0,
     "velocity": 0
   },
   "useDefaultConstraints": false

diff --git a/src/main/deploy/pathplanner/paths/Right-Subwoofer.path b/src/main/deploy/pathplanner/paths/Right-Subwoofer.path
@@ -0,0 +1,58 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.85,
+        "y": 4.1
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8500000000000005,
+        "y": 4.1
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.67,
+        "y": 4.54
+      },
+      "prevControl": {
+        "x": 1.67,
+        "y": 4.54
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.45,
+      "rotationDegrees": -57.0,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -57.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Right Paths",
+  "previewStartingState": {
+    "rotation": 0.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -175,6 +175,7 @@ private void configureButtonBindings() {
     ControlMap.m_driverController.leftBumper().onTrue(new InstantCommand(() -> m_drivetrain.zeroGyro()));
     ControlMap.m_driverController.b().toggleOnTrue(m_defaultDrive);
     ControlMap.m_driverController.a().onTrue(m_alignSpeaker);
+    ControlMap.m_driverController.x().onTrue(m_alignAmp);
 
     // Gunner button bindings
     ControlMap.m_gunnerController.a().whileTrue(m_intakeDown);

diff --git a/src/main/java/frc/robot/commands/TrajectoryCommands/TrajectoryCreation.java b/src/main/java/frc/robot/commands/TrajectoryCommands/TrajectoryCreation.java
@@ -173,25 +173,26 @@ public PathPlannerPath alignAmp(PoseEstimator estimation, Vision vision) {
         double tagX = 0;
         double tagY = 0;
         Rotation2d tagAngle = new Rotation2d();
-        double xChange = 0;
-        double yChange = 0;
-
-        if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) {
-            tagX = vision.return_tag_pose(6).getX();
-            tagY = vision.return_tag_pose(6).getY();; 
-            tagAngle = new Rotation2d(90);
-        } else {
-            tagX = vision.return_tag_pose(5).getX();
-            tagY = vision.return_tag_pose(5).getY();; 
-            tagAngle = new Rotation2d(90);
-        }
+        double xChange = Constants.SwerveConstants.trackWidth/2;
+        double yChange = Constants.SwerveConstants.trackWidth/2;
+
+        tagX = vision.return_tag_pose(6).getX();
+        tagY = vision.return_tag_pose(6).getY();
+        tagAngle = new Rotation2d(3 * Math.PI/2);
 
-        double offset = Constants.SwerveConstants.trackWidth / 2;
+        // double offset = Constants.SwerveConstants.trackWidth / 2;
+
+        // List<Translation2d> bezierPoints = PathPlannerPath.bezierFromPoses(
+        //         new Pose2d(x, y, angle),
+        //         new Pose2d(tagX + xChange, tagY + yChange, tagAngle)
+        //     );
+
+        System.out.println(tagX + xChange);
 
         List<Translation2d> bezierPoints = PathPlannerPath.bezierFromPoses(
-                new Pose2d(x, y, angle),
-                new Pose2d(tagX + xChange, tagY + yChange, tagAngle)
-            );
+            new Pose2d(x, y, angle),
+            new Pose2d(tagX + xChange, tagY - yChange, tagAngle)
+        );
 
         // Create the path using the bezier points created above
         PathPlannerPath path = new PathPlannerPath(