added multiple Routines for auto, updated some of the trajectories

also merged jj changes manually cuz something went wrong
blair-robot-project · Jan 18, 2025 · 812340d · 812340d
1 parent 8505d08
commit 812340d
Show file tree

Hide file tree

Showing 10 changed files with 248 additions and 71 deletions.
diff --git a/src/main/deploy/choreo/leftStation_K.traj b/src/main/deploy/choreo/leftStation_K.traj
@@ -0,0 +1,70 @@
+{
+ "name":"leftStation_K",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":1.2438292503356934, "y":7.022215366363525, "heading":2.1765898115132356, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.812013864517212, "y":5.024738311767578, "heading":2.0344433342221064, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "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":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"1.2438292503356934 m", "val":1.2438292503356934}, "y":{"exp":"7.022215366363525 m", "val":7.022215366363525}, "heading":{"exp":"2.1765898115132356 rad", "val":2.1765898115132356}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"3.812013864517212 m", "val":3.812013864517212}, "y":{"exp":"5.024738311767578 m", "val":5.024738311767578}, "heading":{"exp":"2.0344433342221064 rad", "val":2.0344433342221064}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "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":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.05313],
+  "samples":[
+    {"t":0.0, "x":1.24383, "y":7.02222, "heading":2.17659, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.28119, "ay":-7.21871, "alpha":-0.51435, "fx":[121.38776,120.84823,120.68104,121.21904], "fy":[-93.68145,-94.3758,-94.59143,-93.90161]},
+    {"t":0.03009, "x":1.24803, "y":7.01895, "heading":2.17659, "vx":0.27927, "vy":-0.21721, "omega":-0.01548, "ax":9.2804, "ay":-7.21809, "alpha":-0.51414, "fx":[121.37724,120.83796,120.67092,121.20866], "fy":[-93.67364,-94.36762,-94.5832,-93.89374]},
+    {"t":0.06018, "x":1.26063, "y":7.00914, "heading":2.17612, "vx":0.55851, "vy":-0.43439, "omega":-0.03095, "ax":9.27951, "ay":-7.21739, "alpha":-0.51392, "fx":[121.36543,120.82649,120.65935,121.19675], "fy":[-93.66465,-94.35816,-94.574,-93.88501]},
+    {"t":0.09027, "x":1.28164, "y":6.99281, "heading":2.17519, "vx":0.83772, "vy":-0.65156, "omega":-0.04641, "ax":9.27848, "ay":-7.2166, "alpha":-0.5137, "fx":[121.35202,120.81351,120.64603,121.18302], "fy":[-93.65427,-94.34717,-94.5636,-93.8752]},
+    {"t":0.12036, "x":1.31105, "y":6.96993, "heading":2.1738, "vx":1.11691, "vy":-0.8687, "omega":-0.06187, "ax":9.2773, "ay":-7.21568, "alpha":-0.51347, "fx":[121.33662,120.79863,120.63058,121.16706], "fy":[-93.64219,-94.33437,-94.5517,-93.86403]},
+    {"t":0.15045, "x":1.34885, "y":6.94053, "heading":2.17193, "vx":1.39605, "vy":-1.08582, "omega":-0.07732, "ax":9.27592, "ay":-7.21461, "alpha":-0.51324, "fx":[121.31873,120.78136,120.6125,121.14838], "fy":[-93.62802,-94.31933,-94.53791,-93.85108]},
+    {"t":0.18054, "x":1.39506, "y":6.90459, "heading":2.16961, "vx":1.67516, "vy":-1.3029, "omega":-0.09276, "ax":9.27429, "ay":-7.21334, "alpha":-0.51299, "fx":[121.29763,120.76097,120.59109,121.12628], "fy":[-93.61121,-94.30153,-94.52166,-93.83582]},
+    {"t":0.21063, "x":1.44966, "y":6.86212, "heading":2.16682, "vx":1.95422, "vy":-1.51995, "omega":-0.1082, "ax":9.27234, "ay":-7.21182, "alpha":-0.51274, "fx":[121.27234,120.7365,120.56538,121.09977], "fy":[-93.59102,-94.28019,-94.50221,-93.81749]},
+    {"t":0.24072, "x":1.51266, "y":6.81312, "heading":2.16356, "vx":2.23322, "vy":-1.73695, "omega":-0.12362, "ax":9.26995, "ay":-7.20996, "alpha":-0.51249, "fx":[121.24145,120.70653,120.53396,121.06745], "fy":[-93.56634,-94.25421,-94.47843,-93.79501]},
+    {"t":0.2708, "x":1.58405, "y":6.7576, "heading":2.15984, "vx":2.51214, "vy":-1.95389, "omega":-0.13904, "ax":9.26696, "ay":-7.20764, "alpha":-0.51222, "fx":[121.2028,120.66892,120.49473,121.02721], "fy":[-93.53554,-94.22192,-94.44868,-93.76673]},
+    {"t":0.30089, "x":1.66384, "y":6.69554, "heading":2.15566, "vx":2.79098, "vy":-2.17076, "omega":-0.15446, "ax":9.26312, "ay":-7.20465, "alpha":-0.51194, "fx":[121.15307,120.62036,120.44437,120.97572], "fy":[-93.49605,-94.18072,-94.41035,-93.73009]},
+    {"t":0.33098, "x":1.75201, "y":6.62696, "heading":2.15101, "vx":3.0697, "vy":-2.38755, "omega":-0.16986, "ax":9.25801, "ay":-7.20068, "alpha":-0.51165, "fx":[121.08668,120.55528,120.37738,120.90745], "fy":[-93.44357,-94.12628,-94.35912,-93.68079]},
+    {"t":0.36107, "x":1.84857, "y":6.55186, "heading":2.1459, "vx":3.34827, "vy":-2.60421, "omega":-0.18526, "ax":9.25086, "ay":-7.19511, "alpha":-0.51134, "fx":[120.99362,120.4637,120.28384,120.81247], "fy":[-93.37037,-94.0508,-94.2872,-93.61114]},
+    {"t":0.39116, "x":1.9535, "y":6.47025, "heading":2.14033, "vx":3.62662, "vy":-2.82071, "omega":-0.20064, "ax":9.24014, "ay":-7.18677, "alpha":-0.51102, "fx":[120.85389,120.32566,120.14395,120.67092], "fy":[-93.26105,-93.93869,-94.17907,-93.50576]},
+    {"t":0.42125, "x":2.06681, "y":6.38212, "heading":2.13429, "vx":3.90465, "vy":-3.03695, "omega":-0.21602, "ax":9.2223, "ay":-7.1729, "alpha":-0.51068, "fx":[120.62093,120.09473,119.91159,120.43659], "fy":[-93.07968,-93.75372,-93.99857,-93.32882]},
+    {"t":0.45134, "x":2.18847, "y":6.28749, "heading":2.12779, "vx":4.18215, "vy":-3.25278, "omega":-0.23138, "ax":9.18672, "ay":-7.14523, "alpha":-0.51031, "fx":[120.15566,119.63212,119.44898,119.97136], "fy":[-92.71895,-93.38755,-93.63766,-92.97329]},
+    {"t":0.48143, "x":2.31847, "y":6.18639, "heading":2.12083, "vx":4.45857, "vy":-3.46778, "omega":-0.24674, "ax":9.08083, "ay":-7.06287, "alpha":-0.50983, "fx":[118.76917,118.25046,118.07359,118.59119], "fy":[-91.64729,-92.30367,-92.56129,-91.90898]},
+    {"t":0.51152, "x":2.45673, "y":6.07885, "heading":2.1134, "vx":4.7318, "vy":-3.68029, "omega":-0.26208, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[0.0,0.00002,0.0,-0.00002], "fy":[-0.00002,0.0,0.00002,0.0]},
+    {"t":0.54161, "x":2.59911, "y":5.96811, "heading":2.10552, "vx":4.7318, "vy":-3.68029, "omega":-0.26208, "ax":-9.08083, "ay":7.06287, "alpha":0.50983, "fx":[-118.77171,-118.25584,-118.07099,-118.58587], "fy":[91.64434,92.29664,92.56427,91.91598]},
+    {"t":0.5717, "x":2.73738, "y":5.86057, "heading":2.09763, "vx":4.45857, "vy":-3.46778, "omega":-0.24674, "ax":-9.18672, "ay":7.14523, "alpha":0.51031, "fx":[-120.16056,-119.64291,-119.44397,-119.96069], "fy":[92.71294,93.37358,93.64373,92.98721]},
+    {"t":0.60179, "x":2.86737, "y":5.75946, "heading":2.09021, "vx":4.18215, "vy":-3.25278, "omega":-0.23138, "ax":-9.2223, "ay":7.1729, "alpha":0.51068, "fx":[-120.62804,-120.11061,-119.90432,-120.42087], "fy":[93.0708,93.73323,94.00752,93.34924]},
+    {"t":0.63188, "x":2.98904, "y":5.66483, "heading":2.08324, "vx":3.90465, "vy":-3.03695, "omega":-0.21602, "ax":-9.24014, "ay":7.18677, "alpha":0.51102, "fx":[-120.86305,-120.34631,-120.13457,-120.65049], "fy":[93.2495,93.91211,94.19072,93.53225]},
+    {"t":0.66197, "x":3.10234, "y":5.57671, "heading":2.07674, "vx":3.62662, "vy":-2.82071, "omega":-0.20064, "ax":-9.25086, "ay":7.19511, "alpha":0.51134, "fx":[-121.00469,-120.48877,-120.27251,-120.78766], "fy":[93.35634,94.01854,94.30135,93.64329]},
+    {"t":0.69206, "x":3.20728, "y":5.49509, "heading":2.07071, "vx":3.34827, "vy":-2.60421, "omega":-0.18525, "ax":-9.25801, "ay":7.20068, "alpha":0.51164, "fx":[-121.09951,-120.58445,-120.36425,-120.87859], "fy":[93.42724,94.08878,94.37557,93.71816]},
+    {"t":0.72215, "x":3.30383, "y":5.41999, "heading":2.06513, "vx":3.0697, "vy":-2.38755, "omega":-0.16986, "ax":-9.26312, "ay":7.20465, "alpha":0.51194, "fx":[-121.16751,-120.6533,-120.42959,-120.94313], "fy":[93.47763,94.1384,94.42892,93.77226]},
+    {"t":0.75223, "x":3.39201, "y":5.35141, "heading":2.06002, "vx":2.79098, "vy":-2.17076, "omega":-0.15446, "ax":-9.26696, "ay":7.20764, "alpha":0.51222, "fx":[-121.2187,-120.70529,-120.47845,-120.99122], "fy":[93.51522,94.17521,94.46917,93.81328]},
+    {"t":0.78232, "x":3.47179, "y":5.28936, "heading":2.05537, "vx":2.51214, "vy":-1.95389, "omega":-0.13904, "ax":-9.26995, "ay":7.20996, "alpha":0.51248, "fx":[-121.25865,-120.74598,-120.51634,-121.02841], "fy":[93.54431,94.20353,94.50064,93.8455]},
+    {"t":0.81241, "x":3.54318, "y":5.23383, "heading":2.05119, "vx":2.23322, "vy":-1.73695, "omega":-0.12362, "ax":-9.27234, "ay":7.21182, "alpha":0.51274, "fx":[-121.29072,-120.77872,-120.54656,-121.058], "fy":[93.56747,94.22599,94.52595,93.8715]},
+    {"t":0.8425, "x":3.60618, "y":5.18483, "heading":2.04747, "vx":1.95422, "vy":-1.51995, "omega":-0.1082, "ax":-9.27429, "ay":7.21334, "alpha":0.51299, "fx":[-121.31702,-120.80561,-120.57124,-121.08211], "fy":[93.58634,94.24423,94.54674,93.89291]},
+    {"t":0.87259, "x":3.66078, "y":5.14236, "heading":2.04422, "vx":1.67516, "vy":-1.3029, "omega":-0.09276, "ax":-9.27592, "ay":7.21461, "alpha":0.51323, "fx":[-121.33898,-120.82808,-120.59176,-121.10215], "fy":[93.60201,94.25937,94.56413,93.91082]},
+    {"t":0.90268, "x":3.70699, "y":5.10642, "heading":2.04142, "vx":1.39605, "vy":-1.08582, "omega":-0.07732, "ax":-9.2773, "ay":7.21568, "alpha":0.51347, "fx":[-121.35759,-120.8471,-120.6091,-121.11911], "fy":[93.61524,94.27217,94.57887,93.92599]},
+    {"t":0.93277, "x":3.7448, "y":5.07702, "heading":2.0391, "vx":1.11691, "vy":-0.8687, "omega":-0.06187, "ax":-9.27848, "ay":7.2166, "alpha":0.5137, "fx":[-121.37356,-120.86337,-120.62396,-121.13368], "fy":[93.62657,94.28319,94.59153,93.93896]},
+    {"t":0.96286, "x":3.7742, "y":5.05415, "heading":2.03724, "vx":0.83772, "vy":-0.65156, "omega":-0.04641, "ax":-9.27951, "ay":7.21739, "alpha":0.51392, "fx":[-121.38739,-120.87742,-120.63685,-121.14636], "fy":[93.6364,94.29282,94.60249,93.95011]},
+    {"t":0.99295, "x":3.79521, "y":5.03781, "heading":2.03584, "vx":0.55851, "vy":-0.43439, "omega":-0.03095, "ax":-9.2804, "ay":7.21809, "alpha":0.51414, "fx":[-121.39948,-120.88961,-120.64814,-121.15756], "fy":[93.64502,94.30136,94.61206,93.95976]},
+    {"t":1.02304, "x":3.80781, "y":5.02801, "heading":2.03491, "vx":0.27927, "vy":-0.21721, "omega":-0.01548, "ax":-9.28119, "ay":7.21871, "alpha":0.51435, "fx":[-121.41012,-120.90026,-120.65813,-121.16755], "fy":[93.65265,94.30905,94.62047,93.96811]},
+    {"t":1.05313, "x":3.81201, "y":5.02474, "heading":2.03444, "vx":0.0, "vy":0.0, "omega":0.0, "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/kotlin/frc/team449/RobotLoop.kt b/src/main/kotlin/frc/team449/RobotLoop.kt
@@ -21,7 +21,6 @@ import monologue.Annotations.Log
 import monologue.Logged
 import monologue.Monologue
 import org.littletonrobotics.urcl.URCL
-import kotlin.jvm.optionals.getOrNull
 import kotlin.math.PI
 
 /** The main class of the robot, constructs all the subsystems

diff --git a/src/main/kotlin/frc/team449/auto/Routines.kt b/src/main/kotlin/frc/team449/auto/Routines.kt
@@ -18,16 +18,16 @@ class Routines(
     robot.drive
   )
 
-  //do nothing
-  fun doNothing():AutoRoutine{
-    val nothing: AutoRoutine =autoFactory.newRoutine("Nothing")
+  // do nothing
+  fun doNothing(): AutoRoutine {
+    val nothing: AutoRoutine = autoFactory.newRoutine("Nothing")
     return nothing
   }
 
-  //right taxi
-  fun rightTaxi():AutoRoutine{
+  // right taxi
+  fun rightTaxi(): AutoRoutine {
     val rTaxi: AutoRoutine = autoFactory.newRoutine("Right Taxi")
-    val rTaxiTrajectory:AutoTrajectory = rTaxi.trajectory("rightTaxi")
+    val rTaxiTrajectory: AutoTrajectory = rTaxi.trajectory("rightTaxi")
     rTaxi.active().onTrue(
       Commands.sequence(
         rTaxiTrajectory.resetOdometry(),
@@ -37,10 +37,10 @@ class Routines(
     return rTaxi
   }
 
-  //left taxi
-  fun leftTaxi():AutoRoutine{
+  // left taxi
+  fun leftTaxi(): AutoRoutine {
     val lTaxi: AutoRoutine = autoFactory.newRoutine("Left Taxi")
-    val lTaxiTrajectory:AutoTrajectory = lTaxi.trajectory("leftTaxi")
+    val lTaxiTrajectory: AutoTrajectory = lTaxi.trajectory("leftTaxi")
     lTaxi.active().onTrue(
       Commands.sequence(
         lTaxiTrajectory.resetOdometry(),
@@ -50,67 +50,65 @@ class Routines(
     return lTaxi
   }
 
-  //coral at l1 one on reef E
-  fun reefEL1(): AutoRoutine {
-    val l1E: AutoRoutine = autoFactory.newRoutine("L1 reef E")
-    val reefETrajectory: AutoTrajectory = l1E.trajectory("right_E")
-    l1E.active().onTrue(
+  // coral at l1 one on reef E
+  fun reefE(): AutoRoutine {
+    val E: AutoRoutine = autoFactory.newRoutine("L1 reef E")
+    val reefETrajectory: AutoTrajectory = E.trajectory("right_E")
+    E.active().onTrue(
       Commands.sequence(
         reefETrajectory.resetOdometry(),
         reefETrajectory.cmd()
       )
     )
-    return l1E
+    return E
   }
 
-  //coral at l1 one on reef F
-  fun reefFL1(): AutoRoutine {
-    val l1F: AutoRoutine = autoFactory.newRoutine("L1 reef F")
-    val reefFTrajectory: AutoTrajectory = l1F.trajectory("left_F")
-    l1F.active().onTrue(
+  // coral at l1 one on reef F
+  fun reefF(): AutoRoutine {
+    val F: AutoRoutine = autoFactory.newRoutine("L1 reef F")
+    val reefFTrajectory: AutoTrajectory = F.trajectory("left_F")
+    F.active().onTrue(
       Commands.sequence(
         reefFTrajectory.resetOdometry(),
         reefFTrajectory.cmd()
       )
     )
-    return l1F
+    return F
   }
 
-
-  //coral at l1 one on reef G
-  fun reefGL1(): AutoRoutine {
-    val l1G: AutoRoutine = autoFactory.newRoutine("L1 reef G")
-    val reefGTrajectory: AutoTrajectory = l1G.trajectory("middle_G")
-    l1G.active().onTrue(
+  // coral at l1 one on reef G
+  fun reefG(): AutoRoutine {
+    val g: AutoRoutine = autoFactory.newRoutine("L1 reef G")
+    val reefGTrajectory: AutoTrajectory = g.trajectory("middle_G")
+    g.active().onTrue(
       Commands.sequence(
         reefGTrajectory.resetOdometry(),
         reefGTrajectory.cmd()
       )
     )
-    return l1G
+    return g
   }
 
-  //coral at l1 one on reef J
-  fun reefJL1(): AutoRoutine {
-    val l1J: AutoRoutine = autoFactory.newRoutine("L1 reef J")
-    val reefJTrajectory: AutoTrajectory = l1J.trajectory("left_J")
-    l1J.active().onTrue(
+  // coral at l1 one on reef J
+  fun reefJ(): AutoRoutine {
+    val J: AutoRoutine = autoFactory.newRoutine("L1 reef J")
+    val reefJTrajectory: AutoTrajectory = J.trajectory("left_J")
+    J.active().onTrue(
       Commands.sequence(
         reefJTrajectory.resetOdometry(),
         reefJTrajectory.cmd()
       )
     )
-    return l1J
+    return J
   }
 
-
-  //placing L4 at reef E then l1 at reef D
-  fun reefEl4Dl1(): AutoRoutine {
-    val l4E_l1D: AutoRoutine = autoFactory.newRoutine("L4 reef E and L1 reef D")
-    val reefETrajectory: AutoTrajectory = l4E_l1D.trajectory("right_E")
-    val reefEtoStationTrajectory: AutoTrajectory = l4E_l1D.trajectory("E_rightStation")
-    val stationToDTrajectory: AutoTrajectory = l4E_l1D.trajectory("rightStation_D")
-    l4E_l1D.active().onTrue(
+  // placing L4 at reef E then l1 at reef D
+  fun reefED(): AutoRoutine {
+    val E_D: AutoRoutine = autoFactory.newRoutine("L4 reef E and L1 reef D")
+    val reefETrajectory: AutoTrajectory = E_D.trajectory("right_E")
+    val reefEtoStationTrajectory: AutoTrajectory = E_D.trajectory("E_rightStation")
+    val stationToDTrajectory: AutoTrajectory = E_D.trajectory("rightStation_D")
+    E_D.active().onTrue(
       Commands.sequence(
         Commands.sequence(
           reefETrajectory.resetOdometry(),
@@ -126,18 +124,40 @@ class Routines(
         )
       )
     )
-    return l4E_l1D
+    return E_D
   }
 
+  // placing L4 at reef E then l1 at reef L
+  fun reefJL(): AutoRoutine {
+    val J_L: AutoRoutine = autoFactory.newRoutine("L4 reef J and L1 reef L")
+    val reefJTrajectory: AutoTrajectory = J_L.trajectory("left_J")
+    val reefJtoStationTrajectory: AutoTrajectory = J_L.trajectory("J_leftStation")
+    val stationToLTrajectory: AutoTrajectory = J_L.trajectory("leftStation_L")
+    J_L.active().onTrue(
+      Commands.sequence(
+        Commands.sequence(
+          reefJTrajectory.resetOdometry(),
+          reefJTrajectory.cmd(),
+          Commands.waitSeconds(0.7)
+        ),
+        Commands.sequence(
+          reefJtoStationTrajectory.cmd(),
+          Commands.waitSeconds(0.9)
+        ),
+        Commands.sequence(
+          stationToLTrajectory.cmd()
+        )
+      )
+    )
+    return J_L
+  }
 
-
-  //placing L4 at reef E then l1 at reef D
-  fun reefl4Jl1L(): AutoRoutine {
-    val l4J_l1L: AutoRoutine = autoFactory.newRoutine("L4 reef J and L1 reef D")
-    val reefJTrajectory: AutoTrajectory = l4J_l1L.trajectory("left_J")
-    val reefJtoStationTrajectory: AutoTrajectory = l4J_l1L.trajectory("J_leftStation")
-    val stationToDTrajectory: AutoTrajectory = l4J_l1L.trajectory("leftStation_L")
-    l4J_l1L.active().onTrue(
+  fun reefJK(): AutoRoutine {
+    val J_K: AutoRoutine = autoFactory.newRoutine("L4 reef J and L1 reef K")
+    val reefJTrajectory: AutoTrajectory = J_K.trajectory("left_J")
+    val reefJtoStationTrajectory: AutoTrajectory = J_K.trajectory("J_leftStation")
+    val stationToKTrajectory: AutoTrajectory = J_K.trajectory("leftStation_K")
+    J_K.active().onTrue(
       Commands.sequence(
         Commands.sequence(
           reefJTrajectory.resetOdometry(),
@@ -149,23 +169,24 @@ class Routines(
           Commands.waitSeconds(0.9)
         ),
         Commands.sequence(
-          stationToDTrajectory.cmd()
+          stationToKTrajectory.cmd()
         )
       )
     )
-    return l4J_l1L
+    return J_K
   }
 
-//autoChooser that will be displayed on dashboard
+// autoChooser that will be displayed on dashboard
   fun addOptions(autoChooser: AutoChooser) {
-    autoChooser.addRoutine("Nothing",this::doNothing)
-    autoChooser.addRoutine("RightTaxi",this::rightTaxi)
-    autoChooser.addRoutine("LeftTaxi",this::leftTaxi)
-    autoChooser.addRoutine("L1E", this::reefEL1)
-    autoChooser.addRoutine("L1F", this::reefFL1)
-    autoChooser.addRoutine("L1G",this::reefGL1)
-    autoChooser.addRoutine("L1J",this::reefJL1)
-    autoChooser.addRoutine("l4E & L1D ", this::reefEl4Dl1)
-    autoChooser.addRoutine("l1J & l4L",this::reefl4Jl1L)
+    autoChooser.addRoutine("Nothing", this::doNothing)
+    autoChooser.addRoutine("RightTaxi", this::rightTaxi)
+    autoChooser.addRoutine("LeftTaxi", this::leftTaxi)
+    autoChooser.addRoutine("E", this::reefE)
+    autoChooser.addRoutine("F", this::reefF)
+    autoChooser.addRoutine("G", this::reefG)
+    autoChooser.addRoutine("J", this::reefJ)
+    autoChooser.addRoutine("E & D ", this::reefED)
+    autoChooser.addRoutine("J & L", this::reefJL)
+    autoChooser.addRoutine("J & K", this::reefJK)
   }
 }
diff --git a/src/main/kotlin/frc/team449/commands/PIDPoseAlign.kt b/src/main/kotlin/frc/team449/commands/PIDPoseAlign.kt
@@ -82,4 +82,4 @@ class PIDPoseAlign(
     timer.reset()
     drivetrain.stop()
   }
-}
+}
diff --git a/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveDrive.kt b/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveDrive.kt
@@ -185,6 +185,10 @@ open class SwerveDrive(
     )
   }
 
+  fun set(fromFieldRelativeSpeeds: ChassisSpeeds?) {
+    TODO("Not yet implemented")
+  }
+
   companion object {
     /** Create a [SwerveDrive] using [SwerveConstants]. */
     fun createSwerveKraken(field: Field2d): SwerveDrive {

diff --git a/src/main/kotlin/frc/team449/subsystems/elevator/Elevator.kt b/src/main/kotlin/frc/team449/subsystems/elevator/Elevator.kt
@@ -73,6 +73,7 @@ open class Elevator(
       motor.setControl(
         request
           .withPosition(position)
+          .withUpdateFreqHz(ElevatorConstants.REQUEST_UPDATE_RATE)
           .withFeedForward(
             elevatorFeedForward.calculate(motor.closedLoopReferenceSlope.valueAsDouble)
           )
@@ -141,7 +142,7 @@ open class Elevator(
       if (!status1.isOK) println("Error applying configs to Elevator Lead Motor -> Error Code: $status1")
 
       val status2 = followerMotor.configurator.apply(config)
-      if (!status2.isOK) println("Error applying configs to Elevator Follower Motor -> Error Code: $status1")
+      if (!status2.isOK) println("Error applying configs to Elevator Follower Motor -> Error Code: $status2")
 
       BaseStatusSignal.setUpdateFrequencyForAll(
         ElevatorConstants.VALUE_UPDATE_RATE,
-Original file line number
+Diff line change
@@ Expand Up / @@ -82,4 +82,4 @@ class PIDPoseAlign( @@
         timer.reset()
         drivetrain.stop()
       }
-    }
+    }