Added linked waypoints

Team2338 · Feb 28, 2025 · b928ca7 · b928ca7
1 parent 4a83ed7
commit b928ca7
Show file tree

Hide file tree

Showing 13 changed files with 112 additions and 89 deletions.
diff --git a/src/main/deploy/pathplanner/autos/L-J4-L4.auto b/src/main/deploy/pathplanner/autos/L-J4-L4.auto
@@ -54,12 +54,6 @@
             ]
           }
         },
-        {
-          "type": "named",
-          "data": {
-            "name": "Print Me"
-          }
-        },
         {
           "type": "named",
           "data": {

diff --git a/src/main/deploy/pathplanner/paths/coral-a4.path b/src/main/deploy/pathplanner/paths/coral-a4.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.589,
+        "y": 7.33
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.1724224275690256,
+        "y": 6.60509553280222
+      },
+      "isLocked": true,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.1887295081967215,
+        "y": 3.9890368852459024
+      },
+      "prevControl": {
+        "x": 3.1053290580005295,
+        "y": 4.31664510801174
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "AutonElevatorL4",
+      "waypointRelativePos": 0.686868686868689,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "AutonElevatorL4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -55.0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/deploy/pathplanner/paths/coral-l4.path b/src/main/deploy/pathplanner/paths/coral-l4.path
@@ -3,16 +3,16 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.589,
-        "y": 7.33
+        "x": 1.58925,
+        "y": 7.3302499999999995
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.9956274530055549,
-        "y": 6.904312341126338
+        "x": 1.995877453005555,
+        "y": 6.904562341126337
       },
       "isLocked": true,
-      "linkedName": null
+      "linkedName": "Coral Left"
     },
     {
       "anchor": {
@@ -25,7 +25,7 @@
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "L Branch"
     }
   ],
   "rotationTargets": [],

diff --git a/src/main/deploy/pathplanner/paths/j-coral.path b/src/main/deploy/pathplanner/paths/j-coral.path
@@ -3,47 +3,35 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 4.963,
-        "y": 5.116
+        "x": 5.058811475409836,
+        "y": 5.05594262295082
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.169062603458528,
-        "y": 5.598460073639451
+        "x": 4.255971727872007,
+        "y": 5.503157799248037
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "J Branch"
     },
     {
       "anchor": {
         "x": 1.58925,
         "y": 7.3302499999999995
       },
       "prevControl": {
-        "x": 2.137493718110874,
-        "y": 6.894665148750694
+        "x": 2.069016228179425,
+        "y": 6.937346410238761
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Coral Left"
     }
   ],
   "rotationTargets": [],
   "constraintZones": [],
   "pointTowardsZones": [],
-  "eventMarkers": [
-    {
-      "name": "ElevatorL0",
-      "waypointRelativePos": 0.0,
-      "endWaypointRelativePos": null,
-      "command": {
-        "type": "named",
-        "data": {
-          "name": "AutonElevatorL0"
-        }
-      }
-    }
-  ],
+  "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 2.5,

diff --git a/src/main/deploy/pathplanner/paths/l-coral.path b/src/main/deploy/pathplanner/paths/l-coral.path
@@ -3,47 +3,35 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.68,
-        "y": 5.044
+        "x": 3.6802254098360656,
+        "y": 5.043954918032787
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.036867284207851,
-        "y": 4.667062582778861
+        "x": 4.037092694043917,
+        "y": 4.667017500811649
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "L Branch"
     },
     {
       "anchor": {
-        "x": 1.589,
-        "y": 7.33
+        "x": 1.58925,
+        "y": 7.3302499999999995
       },
       "prevControl": {
-        "x": 1.4080663971863612,
-        "y": 7.512871326349569
+        "x": 1.4083163971863613,
+        "y": 7.513121326349569
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Coral Left"
     }
   ],
   "rotationTargets": [],
   "constraintZones": [],
   "pointTowardsZones": [],
-  "eventMarkers": [
-    {
-      "name": "AutonElevatorL0",
-      "waypointRelativePos": 0,
-      "endWaypointRelativePos": null,
-      "command": {
-        "type": "named",
-        "data": {
-          "name": "AutonElevatorL0"
-        }
-      }
-    }
-  ],
+  "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 2.5,

diff --git a/src/main/deploy/pathplanner/paths/l-j4.path b/src/main/deploy/pathplanner/paths/l-j4.path
@@ -8,24 +8,24 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.382505716821357,
-        "y": 6.910923122592814
+        "x": 6.5735589411869535,
+        "y": 6.736568065392607
       },
       "isLocked": true,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.963,
-        "y": 5.115881147540984
+        "x": 5.058811475409836,
+        "y": 5.05594262295082
       },
       "prevControl": {
-        "x": 5.4441774183939255,
-        "y": 5.946924135947702
+        "x": 5.520785029650235,
+        "y": 5.888626091056476
       },
       "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
+      "isLocked": true,
+      "linkedName": "J Branch"
     }
   ],
   "rotationTargets": [],

diff --git a/src/main/java/team/gif/robot/OI.java b/src/main/java/team/gif/robot/OI.java
@@ -3,7 +3,6 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.RepeatCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import team.gif.robot.commands.climber.ClimberDeploy;

diff --git a/src/main/java/team/gif/robot/Robot.java b/src/main/java/team/gif/robot/Robot.java
@@ -229,15 +229,4 @@ static public void enableRobotModeManual() {
     static public void enableRobotModeStandardOp() {
         robotMode = RobotMode.STANDARD_OP;
     }
-
-    /**
-     * returns true if robot is in manual mode
-     * Needed for dashboard functionality
-     *
-     * @return true if robot is in manual mode, false if in StandardOp mode
-     */
-    public static boolean getRobotModeManual() {
-        return robotMode == RobotMode.MANUAL;
-    }
-
 }
diff --git a/src/main/java/team/gif/robot/UiSmartDashboard.java b/src/main/java/team/gif/robot/UiSmartDashboard.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import team.gif.lib.RobotMode;
 import team.gif.lib.delay;
 import team.gif.robot.commands.drivetrain.Reset180;
 
@@ -60,7 +61,7 @@ public void updateUI() {
 
         SmartDashboard.putNumber("Left", Robot.shooter.getLeftD());
         SmartDashboard.putNumber("Right", Robot.shooter.getRightD());
-        SmartDashboard.putBoolean("Mode(Std)", Robot.getRobotModeManual());
+        SmartDashboard.putBoolean("Mode(Std)", Robot.getRobotMode() == RobotMode.STANDARD_OP);
         SmartDashboard.putString("Piston", Robot.climber.getPistonStateAsString());
 
         // Update Diagnostics tab

diff --git a/src/main/java/team/gif/robot/commands/drivetrain/MoveModulesIn.java b/src/main/java/team/gif/robot/commands/drivetrain/MoveModulesIn.java
@@ -1,6 +1,5 @@
 package team.gif.robot.commands.drivetrain;
 import edu.wpi.first.wpilibj2.command.Command;
-import team.gif.robot.Robot;
 
 public class MoveModulesIn extends Command {
     public MoveModulesIn() {}

diff --git a/src/main/java/team/gif/robot/commands/drivetrain/ShortDriveAway.java b/src/main/java/team/gif/robot/commands/drivetrain/ShortDriveAway.java
@@ -2,7 +2,6 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import team.gif.lib.drivePace;
-import team.gif.robot.Constants;
 import team.gif.robot.Robot;
 
 /**

diff --git a/src/main/java/team/gif/robot/commands/shooter/AutonShootReset.java b/src/main/java/team/gif/robot/commands/shooter/AutonShootReset.java
@@ -14,11 +14,11 @@ public AutonShootReset() {
                 new ParallelRaceGroup( // ends when any command ends // if robot does not have a target, don't shoot
                         new AutonHasNoTarget(),
                         new Shoot()
+                ),
+                new ParallelCommandGroup(
+                        new ShortDriveAway(),
+                        new SetElevatorPosition(0)
                 )
-//                new ParallelCommandGroup(
-//                        new ShortDriveAway(),
-//                        new SetElevatorPosition(0)
-//                )
         );
     }
 }
diff --git a/src/main/java/team/gif/robot/commands/shooter/AutonStrafeToTarget.java b/src/main/java/team/gif/robot/commands/shooter/AutonStrafeToTarget.java
@@ -8,7 +8,6 @@
 public class AutonStrafeToTarget extends Command {
 
     private boolean hasTarget;
-    private int counter;
 
     public AutonStrafeToTarget() {
         super();
@@ -20,14 +19,16 @@ public AutonStrafeToTarget() {
     public void initialize() {
         hasTarget = false;
         Robot.swerveDrive.setDrivePace(drivePace.COAST_RR);
-        counter = 0;
     }
 
     // Called every time the scheduler runs (~20ms) while the command is scheduled
     @Override
     public void execute() {
-        if (Robot.elevator.getPosition() < Constants.Elevator.LEVEL_4_POSITION - 1 ) {
-            System.out.println("counter " + counter++);
+        // Ensure elevator is at level 4 before checking reef sensors to move
+        // This makes sure the sensors don't trip if the robot lined up
+        // correctly and is still raising the elevator
+        // Since this only runs in auto, safe to just chcek height of level 4
+        if (Robot.elevator.getPosition() < Constants.Elevator.LEVEL_4_POSITION - 0.3 ) {
             return;
         }
 
@@ -67,5 +68,4 @@ public void end(boolean interrupted) {
         Robot.swerveDrive.drive(0.0, 0.0, 0.0);
         Robot.swerveDrive.setDrivePace(drivePace.COAST_FR);
     }
-}
-
+}