backup autos with old structure

blair-robot-project · Jan 20, 2025 · b3b54d8 · b3b54d8
1 parent 77395ac
commit b3b54d8
Show file tree

Hide file tree

Showing 16 changed files with 1,301 additions and 74 deletions.
diff --git a/src/main/deploy/choreo/ThreeL4/1.traj b/src/main/deploy/choreo/ThreeL4/1.traj
diff --git a/src/main/deploy/choreo/ThreeL4/2.traj b/src/main/deploy/choreo/ThreeL4/2.traj
diff --git a/src/main/deploy/choreo/ThreeL4/3.traj b/src/main/deploy/choreo/ThreeL4/3.traj
diff --git a/src/main/deploy/choreo/ThreeL4/4.traj b/src/main/deploy/choreo/ThreeL4/4.traj
diff --git a/src/main/deploy/choreo/ThreeL4/5.traj b/src/main/deploy/choreo/ThreeL4/5.traj
@@ -0,0 +1,64 @@
+{
+ "name":"5",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":1.4775506258010864, "y":0.6930339932441711, "heading":0.9493919061825494, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.1279983520507812, "y":2.452793836593628, "heading":1.0500675813896283, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":true},
+    {"from":"last", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"1.4775506258010864 m", "val":1.4775506258010864}, "y":{"exp":"0.6930339932441711 m", "val":0.6930339932441711}, "heading":{"exp":"0.9493919061825494 rad", "val":0.9493919061825494}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"3.1279983520507812 m", "val":3.1279983520507812}, "y":{"exp":"2.452793836593628 m", "val":2.452793836593628}, "heading":{"exp":"1.0500675813896283 rad", "val":1.0500675813896283}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":true},
+    {"from":"last", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.1026],
+  "samples":[
+    {"t":0.0, "x":1.47755, "y":0.69303, "heading":0.94939, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.28748, "ay":4.57551, "alpha":0.06209, "fx":[67.86819,68.32531,68.26426,67.80959], "fy":[72.8253,72.39672,72.45522,72.88079]},
+    {"t":0.03802, "x":1.48065, "y":0.69634, "heading":0.94939, "vx":0.16301, "vy":0.17396, "omega":0.00236, "ax":4.28759, "ay":4.57478, "alpha":0.06168, "fx":[67.87129,68.32531,68.26467,67.81307], "fy":[72.81247,72.38673,72.44491,72.86768]},
+    {"t":0.07604, "x":1.48995, "y":0.70626, "heading":0.94948, "vx":0.32603, "vy":0.3479, "omega":0.00471, "ax":4.2876, "ay":4.57405, "alpha":0.06136, "fx":[67.87252,68.32409,68.26374,67.81457], "fy":[72.79996,72.37646,72.43444,72.85499]},
+    {"t":0.11406, "x":1.50544, "y":0.7228, "heading":0.94966, "vx":0.48905, "vy":0.52181, "omega":0.00704, "ax":4.2875, "ay":4.57332, "alpha":0.06111, "fx":[67.87182,68.32149,68.26133,67.81403], "fy":[72.78749,72.36573,72.42361,72.84246]},
+    {"t":0.15208, "x":1.52713, "y":0.74594, "heading":0.94993, "vx":0.65206, "vy":0.69569, "omega":0.00936, "ax":4.28728, "ay":4.57256, "alpha":0.06093, "fx":[67.86912,68.31735,68.25728,67.81142], "fy":[72.7747,72.35426,72.41215,72.82969]},
+    {"t":0.1901, "x":1.55502, "y":0.7757, "heading":0.95028, "vx":0.81507, "vy":0.86954, "omega":0.01168, "ax":4.28695, "ay":4.57174, "alpha":0.06079, "fx":[67.86428,68.31141,68.25136,67.80658], "fy":[72.76112,72.34168,72.39966,72.81622]},
+    {"t":0.22812, "x":1.58911, "y":0.81206, "heading":0.95073, "vx":0.97806, "vy":1.04336, "omega":0.01399, "ax":4.28646, "ay":4.57082, "alpha":0.0607, "fx":[67.857,68.30331,68.24323,67.79925], "fy":[72.74618,72.32749,72.38564,72.80146]},
+    {"t":0.26614, "x":1.6294, "y":0.85503, "heading":0.95126, "vx":1.14103, "vy":1.21715, "omega":0.0163, "ax":4.2858, "ay":4.56978, "alpha":0.06063, "fx":[67.8468,68.29252,68.23234,67.78895], "fy":[72.72913,72.31098,72.36938,72.78467]},
+    {"t":0.30417, "x":1.67588, "y":0.90461, "heading":0.95188, "vx":1.30398, "vy":1.39089, "omega":0.0186, "ax":4.2849, "ay":4.56852, "alpha":0.0606, "fx":[67.83285,68.27818,68.21786,67.77485], "fy":[72.70891,72.29113,72.34987,72.76479]},
+    {"t":0.34219, "x":1.72855, "y":0.9608, "heading":0.95259, "vx":1.4669, "vy":1.56459, "omega":0.02091, "ax":4.28369, "ay":4.56697, "alpha":0.0606, "fx":[67.81378,68.25891,68.19843,67.75562], "fy":[72.68394,72.26636,72.32552,72.74024]},
+    {"t":0.38021, "x":1.78742, "y":1.02359, "heading":0.95338, "vx":1.62976, "vy":1.73823, "omega":0.02321, "ax":4.28201, "ay":4.56495, "alpha":0.06063, "fx":[67.78726,68.23242,68.17174,67.72889], "fy":[72.65162,72.23403,72.29374,72.70846]},
+    {"t":0.41823, "x":1.85248, "y":1.09297, "heading":0.95426, "vx":1.79257, "vy":1.91179, "omega":0.02552, "ax":4.27961, "ay":4.56218, "alpha":0.06073, "fx":[67.74898,68.19456,68.13366,67.6904], "fy":[72.60736,72.18944,72.24984,72.66489]},
+    {"t":0.45625, "x":1.92373, "y":1.16896, "heading":0.95523, "vx":1.95528, "vy":2.08525, "omega":0.02782, "ax":4.27594, "ay":4.55806, "alpha":0.06094, "fx":[67.69025,68.13699,68.07586,67.63144], "fy":[72.54204,72.1231,72.18448,72.60053]},
+    {"t":0.49427, "x":2.00116, "y":1.25154, "heading":0.95629, "vx":2.11786, "vy":2.25855, "omega":0.03014, "ax":4.26975, "ay":4.55125, "alpha":0.06146, "fx":[67.59057,68.04045,67.97902,67.53149], "fy":[72.43452,72.0128,72.07573,72.49453]},
+    {"t":0.53229, "x":2.08477, "y":1.3407, "heading":0.95744, "vx":2.2802, "vy":2.43159, "omega":0.03248, "ax":4.25727, "ay":4.53767, "alpha":0.06302, "fx":[67.38772,67.84783,67.78572,67.32807], "fy":[72.22202,71.79103,71.85728,72.2852]},
+    {"t":0.57031, "x":2.17454, "y":1.43643, "heading":0.95867, "vx":2.44206, "vy":2.60412, "omega":0.03487, "ax":4.21974, "ay":4.49708, "alpha":0.07151, "fx":[66.76483,67.28296,67.21659,66.70157], "fy":[71.5976,71.11339,71.19333,71.67368]},
+    {"t":0.60833, "x":2.27044, "y":1.53869, "heading":0.96, "vx":2.6025, "vy":2.7751, "omega":0.03759, "ax":0.83948, "ay":0.86929, "alpha":2.98807, "fx":[2.60726,15.78907,23.64637,11.26642], "fy":[12.44188,3.07137,15.37559,24.31366]},
+    {"t":0.64635, "x":2.37, "y":1.64483, "heading":0.96143, "vx":2.63441, "vy":2.80815, "omega":0.1512, "ax":-4.21292, "ay":-4.49196, "alpha":0.61956, "fx":[-68.77797,-64.42571,-64.87281,-69.45657], "fy":[-69.64652,-73.67115,-73.124,-68.81075]},
+    {"t":0.68437, "x":2.46711, "y":1.74835, "heading":0.96718, "vx":2.47424, "vy":2.63736, "omega":0.17476, "ax":-4.25547, "ay":-4.5366, "alpha":0.29051, "fx":[-68.45187,-66.3657,-66.63996,-68.77741], "fy":[-71.20924,-73.15233,-72.86696,-70.85873]},
+    {"t":0.72239, "x":2.55811, "y":1.84534, "heading":0.97382, "vx":2.31244, "vy":2.46488, "omega":0.1858, "ax":-4.26902, "ay":-4.55078, "alpha":0.17593, "fx":[-68.3124,-67.04064,-67.22597,-68.51641], "fy":[-71.75185,-72.93934,-72.75428,-71.54267]},
+    {"t":0.76041, "x":2.64294, "y":1.93577, "heading":0.98089, "vx":2.15013, "vy":2.29185, "omega":0.19249, "ax":-4.27566, "ay":-4.55772, "alpha":0.11778, "fx":[-68.23719,-67.3835,-67.51723,-68.37921], "fy":[-72.02638,-72.82452,-72.69342,-71.8844]},
+    {"t":0.79843, "x":2.7216, "y":2.01962, "heading":0.9882, "vx":1.98757, "vy":2.11857, "omega":0.19697, "ax":-4.2796, "ay":-4.56183, "alpha":0.08263, "fx":[-68.19071,-67.59118,-67.69091,-68.29447], "fy":[-72.19166,-72.75261,-72.65583,-72.0895]},
+    {"t":0.83645, "x":2.79408, "y":2.09687, "heading":0.99569, "vx":1.82485, "vy":1.94512, "omega":0.20011, "ax":-4.28221, "ay":-4.56455, "alpha":0.05909, "fx":[-68.15947,-67.73066,-67.80592,-68.23675], "fy":[-72.30175,-72.70318,-72.63063,-72.22644]},
+    {"t":0.87448, "x":2.86036, "y":2.16752, "heading":1.0033, "vx":1.66204, "vy":1.77158, "omega":0.20236, "ax":-4.28406, "ay":-4.56647, "alpha":0.04222, "fx":[-68.13725,-67.83094,-67.88742,-68.19475], "fy":[-72.38013,-72.66699,-72.61278,-72.32451]},
+    {"t":0.9125, "x":2.92046, "y":2.23158, "heading":1.011, "vx":1.49916, "vy":1.59796, "omega":0.20396, "ax":-4.28544, "ay":-4.56791, "alpha":0.02955, "fx":[-68.12083,-67.90663,-67.94799,-68.16269], "fy":[-72.43858,-72.63925,-72.59968,-72.39832]},
+    {"t":0.95052, "x":2.97436, "y":2.28903, "heading":1.01875, "vx":1.33622, "vy":1.42428, "omega":0.20509, "ax":-4.28651, "ay":-4.56903, "alpha":0.01967, "fx":[-68.10835,-67.96587,-67.99461,-68.1373], "fy":[-72.48372,-72.61723,-72.5898,-72.45598]},
+    {"t":0.98854, "x":3.02207, "y":2.33988, "heading":1.02655, "vx":1.17325, "vy":1.25056, "omega":0.20583, "ax":-4.28737, "ay":-4.56992, "alpha":0.01176, "fx":[-68.09866,-68.01356,-68.03145,-68.11663], "fy":[-72.51951,-72.59927,-72.58222,-72.50235]},
+    {"t":1.02656, "x":3.06358, "y":2.38413, "heading":1.03437, "vx":1.01024, "vy":1.07681, "omega":0.20628, "ax":-4.28806, "ay":-4.57065, "alpha":0.00529, "fx":[-68.09102,-68.05282,-68.06118,-68.0994], "fy":[-72.5485,-72.5843,-72.57635,-72.54052]},
+    {"t":1.06458, "x":3.09889, "y":2.42176, "heading":1.04222, "vx":0.8472, "vy":0.90303, "omega":0.20648, "ax":-4.28864, "ay":-4.57126, "alpha":-0.00011, "fx":[-68.08495,-68.08577,-68.08558,-68.08476], "fy":[-72.57236,-72.5716,-72.57178,-72.57254]},
+    {"t":1.1026, "x":3.128, "y":2.45279, "heading":1.05007, "vx":0.68415, "vy":0.72923, "omega":0.20648, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/ThreeL4/ThreeL4.chor b/src/main/deploy/choreo/ThreeL4/ThreeL4.chor
@@ -0,0 +1,78 @@
+{
+ "name":"ThreeL4",
+ "version":1,
+ "type":"Swerve",
+ "variables":{
+  "expressions":{},
+  "poses":{}
+ },
+ "config":{
+  "frontLeft":{
+   "x":{
+    "exp":"11.625 in",
+    "val":0.295275
+   },
+   "y":{
+    "exp":"11.625 in",
+    "val":0.295275
+   }
+  },
+  "backLeft":{
+   "x":{
+    "exp":"-11.625 in",
+    "val":-0.295275
+   },
+   "y":{
+    "exp":"11.625 in",
+    "val":0.295275
+   }
+  },
+  "mass":{
+   "exp":"140 lbs",
+   "val":63.502931800000006
+  },
+  "inertia":{
+   "exp":"6 kg m ^ 2",
+   "val":6.0
+  },
+  "gearing":{
+   "exp":"6.75",
+   "val":6.75
+  },
+  "radius":{
+   "exp":"2 in",
+   "val":0.0508
+  },
+  "vmax":{
+   "exp":"5000 RPM",
+   "val":523.5987755982989
+  },
+  "tmax":{
+   "exp":"0.75 N * m",
+   "val":0.75
+  },
+  "cof":{
+   "exp":"0.8",
+   "val":0.8
+  },
+  "bumper":{
+   "front":{
+    "exp":"17.5 in",
+    "val":0.4445
+   },
+   "side":{
+    "exp":"17.5 in",
+    "val":0.4445
+   },
+   "back":{
+    "exp":"17.5 in",
+    "val":0.4445
+   }
+  },
+  "differentialTrackWidth":{
+   "exp":"22 in",
+   "val":0.5588
+  }
+ },
+ "generationFeatures":[]
+}
diff --git a/src/main/deploy/modded_layout.json b/src/main/deploy/modded_layout.json
diff --git a/src/main/kotlin/frc/team449/auto/AutoUtil.kt b/src/main/kotlin/frc/team449/auto/AutoUtil.kt
@@ -21,7 +21,7 @@ object AutoUtil {
   )
 
   /** You will need to change this based on how the field is set up each year */
-  fun transformForRed(pathGroup: MutableList<ChoreoTrajectory>): MutableList<ChoreoTrajectory> {
+  fun transformForRedAlliance(pathGroup: MutableList<ChoreoTrajectory>): MutableList<ChoreoTrajectory> {
     for (index in 0 until pathGroup.size) {
       for (time in pathGroup[index].objectiveTimestamps) {
         val currentMatrix = pathGroup[index].stateMap.get(time)
@@ -44,6 +44,29 @@ object AutoUtil {
     return pathGroup
   }
 
+  fun transformForBlueStage(pathGroup: MutableList<ChoreoTrajectory>): MutableList<ChoreoTrajectory> {
+    for (index in 0 until pathGroup.size) {
+      for (time in pathGroup[index].objectiveTimestamps) {
+        val currentMatrix = pathGroup[index].stateMap.get(time)
+
+        val newMatrix = MatBuilder.fill(
+          Nat.N2(),
+          Nat.N3(),
+          currentMatrix[0, 0],
+          FieldConstants.fieldWidth - currentMatrix[0, 1],
+          -currentMatrix[0, 2],
+          currentMatrix[1, 0],
+          -currentMatrix[1, 1],
+          -currentMatrix[1, 2]
+        )
+
+        pathGroup[index].stateMap.put(time, newMatrix)
+      }
+    }
+
+    return pathGroup
+  }
+
   /** Add other methods that return commands that do groups of actions that are done
    * across different auto routines. For Charged UP, these methods were things such as
    * dropping a cone/cube, or getting in ground intake position, etc.

diff --git a/src/main/kotlin/frc/team449/auto/choreo/ChoreoFollower.kt b/src/main/kotlin/frc/team449/auto/choreo/ChoreoFollower.kt
@@ -34,7 +34,8 @@ class ChoreoFollower(
   poseTol: Pose2d = Pose2d(0.035, 0.035, Rotation2d(0.035)),
   private val timeout: Double = 0.65,
   private val resetPose: Boolean = false,
-  private val debug: Boolean = false
+  private val debug: Boolean = false,
+  private val stopDriveAfterTraj: Boolean = true
 ) : Command() {
 
   private val timer = Timer()
@@ -101,6 +102,9 @@ class ChoreoFollower(
   override fun end(interrupted: Boolean) {
     timer.stop()
     timer.reset()
-    drivetrain.stop()
+
+    if (stopDriveAfterTraj) {
+      drivetrain.stop()
+    }
   }
 }
diff --git a/src/main/kotlin/frc/team449/auto/choreo/ChoreoRoutine.kt b/src/main/kotlin/frc/team449/auto/choreo/ChoreoRoutine.kt
@@ -35,7 +35,8 @@ class ChoreoRoutine(
   private val resetPosition: Boolean = false,
   private val resetPositionTolerance: Pose2d = Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)),
   private val timeout: Double = 0.65,
-  private val debug: Boolean = false
+  private val debug: Boolean = false,
+  private val stopDriveAfterTraj: Boolean = true
 ) {
 
   private fun resetPose(trajectory: ChoreoTrajectory): Command {
@@ -74,8 +75,9 @@ class ChoreoRoutine(
             poseTol,
             timeout,
             resetPosition,
-            debug
-          ).andThen(drive::stop, drive),
+            debug,
+            stopDriveAfterTraj
+          ),
           parallelEventMap.getOrDefault(i, InstantCommand())
         )
       )

diff --git a/src/main/kotlin/frc/team449/auto/routines/ExampleRoutine.kt b/src/main/kotlin/frc/team449/auto/routines/ExampleRoutine.kt
@@ -32,7 +32,7 @@ class ExampleRoutine(
 
   override val trajectory: MutableList<ChoreoTrajectory> =
     if (isRed) {
-      AutoUtil.transformForRed(
+      AutoUtil.transformForRedAlliance(
         ChoreoTrajectory.createTrajectory(arrayListOf("part1", "part2"), "example")
       )
     } else {

diff --git a/src/main/kotlin/frc/team449/auto/routines/ThreeL4.kt b/src/main/kotlin/frc/team449/auto/routines/ThreeL4.kt
@@ -0,0 +1,91 @@
+package frc.team449.auto.routines
+
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.InstantCommand
+import edu.wpi.first.wpilibj2.command.PrintCommand
+import edu.wpi.first.wpilibj2.command.WaitCommand
+import frc.team449.Robot
+import frc.team449.auto.AutoUtil
+import frc.team449.auto.choreo.ChoreoRoutine
+import frc.team449.auto.choreo.ChoreoRoutineStructure
+import frc.team449.auto.choreo.ChoreoTrajectory
+import frc.team449.commands.driveAlign.SimpleReefAlign
+import frc.team449.subsystems.superstructure.SuperstructureGoal
+
+class ThreeL4(
+  robot: Robot,
+  isRedAlliance: Boolean,
+  isRedStage: Boolean
+) : ChoreoRoutineStructure {
+
+  /** TODO: Add logic to scoring to outtake and wait until coral isn't detected
+   * TODO: Add logic to substation to intake and wait until coral is detected
+   */
+  override val routine =
+    ChoreoRoutine(
+      drive = robot.drive,
+      poseSubsystem = robot.poseSubsystem,
+      parallelEventMap = hashMapOf(
+        0 to premoveL4(robot),
+        1 to premoveSubstation(robot),
+        2 to premoveL4(robot),
+        3 to premoveSubstation(robot),
+        4 to premoveL4(robot),
+      ),
+      stopEventMap = hashMapOf(
+        1 to scoreL4(robot),
+        2 to intakeSubstation(robot),
+        3 to scoreL4(robot),
+        4 to intakeSubstation(robot),
+        5 to scoreL4(robot)
+          .andThen(robot.superstructureManager.requestGoal(SuperstructureGoal.STOW)),
+      ),
+      debug = false,
+      timeout = 0.0,
+      stopDriveAfterTraj = false
+    )
+
+  private fun premoveL4(robot: Robot): Command {
+    return robot.superstructureManager.requestGoal(SuperstructureGoal.L4_PREMOVE)
+  }
+
+  private fun premoveSubstation(robot: Robot): Command {
+    return robot.superstructureManager.requestGoal(SuperstructureGoal.SUBSTATION_INTAKE)
+      .alongWith(PrintCommand("start intaking"))
+  }
+
+  private fun scoreL4(robot: Robot): Command {
+    return robot.superstructureManager.requestGoal(SuperstructureGoal.L4)
+      .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
+  }
+
+  private fun intakeSubstation(robot: Robot): Command {
+    return InstantCommand(robot.drive::stop)
+      .andThen(robot.superstructureManager.requestGoal(SuperstructureGoal.SUBSTATION_INTAKE))
+      .andThen(PrintCommand("wait until coral detected placeholder "))
+      .andThen(WaitCommand(1.0))
+  }
+
+  override val trajectory: MutableList<ChoreoTrajectory> =
+    if (isRedAlliance) {
+      if (isRedStage) {
+        AutoUtil.transformForRedAlliance(
+          ChoreoTrajectory.createTrajectory(arrayListOf("1", "2", "3", "4", "5"), "ThreeL4")
+        )
+      } else {
+        AutoUtil.transformForBlueStage(
+          AutoUtil.transformForRedAlliance(
+            ChoreoTrajectory.createTrajectory(arrayListOf("1", "2", "3", "4", "5"), "ThreeL4")
+          )
+        )
+      }
+    } else {
+      if (isRedStage) {
+        ChoreoTrajectory.createTrajectory(arrayListOf("1", "2", "3", "4", "5"), "ThreeL4")
+      } else {
+        AutoUtil.transformForBlueStage(
+          ChoreoTrajectory.createTrajectory(arrayListOf("1", "2", "3", "4", "5"), "ThreeL4")
+        )
+      }
+    }
+}