team4099 · sswadkar · Feb 18, 2024 · Feb 18, 2024
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -1,4 +1,9 @@
 {
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
   "System Joysticks": {
     "window": {
       "visible": false

diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
@@ -0,0 +1,18 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2,
+      "y": 2
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": []
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/5NotePureMove.path b/src/main/deploy/pathplanner/paths/5NotePureMove.path
@@ -0,0 +1,182 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2289055048108568,
+        "y": 5.4387004560018815
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.7231721740590975,
+        "y": 3.908136928915673
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.22637826150855,
+        "y": 4.12372243302023
+      },
+      "prevControl": {
+        "x": 2.172429912120765,
+        "y": 4.09804759236747
+      },
+      "nextControl": {
+        "x": 2.280326610896335,
+        "y": 4.1493972736729905
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.371676329252173,
+        "y": 5.4387004560018815
+      },
+      "prevControl": {
+        "x": 1.1350935870429284,
+        "y": 5.125820818897388
+      },
+      "nextControl": {
+        "x": 1.6082590714614176,
+        "y": 5.751580093106375
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.6622268722657387,
+        "y": 5.462296362194021
+      },
+      "prevControl": {
+        "x": 2.07483477608705,
+        "y": 5.411457747949859
+      },
+      "nextControl": {
+        "x": 2.7066640964687334,
+        "y": 5.466142390960894
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.1878985224745173,
+        "y": 5.696696223214165
+      },
+      "prevControl": {
+        "x": 2.129863497405182,
+        "y": 5.953154353689664
+      },
+      "nextControl": {
+        "x": 0.966076214972528,
+        "y": 5.636303176982367
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.4021521705928346,
+        "y": 6.677901317715116
+      },
+      "prevControl": {
+        "x": 1.8381221065820927,
+        "y": 5.508856306149623
+      },
+      "nextControl": {
+        "x": 2.417172472108316,
+        "y": 6.709033361340628
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.0688795243068614,
+        "y": 5.985668113961836
+      },
+      "prevControl": {
+        "x": 2.1442753447778573,
+        "y": 6.4137426928772
+      },
+      "nextControl": {
+        "x": 0.9454947637610032,
+        "y": 5.936553287207062
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.766426772723887,
+        "y": 7.256520752911847
+      },
+      "prevControl": {
+        "x": 3.670312735497631,
+        "y": 6.7083552256964465
+      },
+      "nextControl": {
+        "x": 8.420823369195706,
+        "y": 7.344095867289241
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.654099576432541,
+        "y": 6.019814612562775
+      },
+      "prevControl": {
+        "x": 5.070880344350176,
+        "y": 6.220795602050137
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0,
+      "rotationDegrees": 0,
+      "rotateFast": false
+    },
+    {
+      "waypointRelativePos": 5,
+      "rotationDegrees": 28.75499731851916,
+      "rotateFast": false
+    },
+    {
+      "waypointRelativePos": 7,
+      "rotationDegrees": 14.598436530771252,
+      "rotateFast": false
+    },
+    {
+      "waypointRelativePos": 3,
+      "rotationDegrees": 0,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 270.0,
+    "maxAngularAcceleration": 500.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 22.463820093818477,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
@@ -3,57 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.0,
-        "y": 7.0
+        "x": 1.2289055048108568,
+        "y": 5.4387004560018815
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.0,
-        "y": 6.5
+        "x": 1.2526592560220629,
+        "y": 3.8988635337354443
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.061558522422601,
-        "y": 6.1060047436211775
+        "x": 4.682015652524012,
+        "y": 6.1630012238954786
       },
       "prevControl": {
-        "x": 5.061558522422601,
-        "y": 7.1060047436211775
-      },
-      "nextControl": {
-        "x": 7.061558522422601,
-        "y": 5.1060047436211775
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 7.9885342675054565,
-        "y": 3.354442578025781
-      },
-      "prevControl": {
-        "x": 7.7385342675054565,
-        "y": 4.8544425780257825
-      },
-      "nextControl": {
-        "x": 8.238534267505454,
-        "y": 1.8544425780257794
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 3.3963635116216997,
-        "y": 1.641386992337064
-      },
-      "prevControl": {
-        "x": 5.380848911075716,
-        "y": 2.0910493199386275
+        "x": 2.7753614271785247,
+        "y": 6.054149658970127
       },
       "nextControl": null,
       "isLocked": false,
@@ -71,7 +39,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 173.99592794454193,
+    "rotation": -1.6558108046829694,
     "rotateFast": false
   },
   "reversed": false,

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -1,6 +1,7 @@
 package com.team4099.robot2023
 
 import com.team4099.robot2023.auto.AutonomousSelector
+import com.team4099.robot2023.auto.mode.ExamplePathAuto
 import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
 import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
 import com.team4099.robot2023.config.ControlBoard
@@ -11,7 +12,7 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2
 import com.team4099.robot2023.subsystems.elevator.Elevator
-import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO
+import com.team4099.robot2023.subsystems.elevator.ElevatorIO
 import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim
 import com.team4099.robot2023.subsystems.feeder.Feeder
 import com.team4099.robot2023.subsystems.feeder.FeederIONeo
@@ -68,7 +69,7 @@ object RobotContainer {
       limelight = LimelightVision(object : LimelightVisionIO {})
       intake = Intake(IntakeIONEO)
       feeder = Feeder(FeederIONeo)
-      elevator = Elevator(ElevatorIONEO)
+      elevator = Elevator(object : ElevatorIO {})
       flywheel = Flywheel(FlywheelIOTalon)
       wrist = Wrist(object : WristIO {})
     } else {
@@ -98,9 +99,9 @@ object RobotContainer {
     drivetrain.defaultCommand =
       TeleopDriveCommand(
         driver = Ryan(),
-        { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
-        { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
-        { -1 * ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
+        { -ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
+        { -ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
+        { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
         { ControlBoard.slowMode },
         drivetrain
       )
@@ -150,17 +151,18 @@ object RobotContainer {
 
   fun mapTeleopControls() {
 
-    ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees))
-    ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand())
-    ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
-    ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand())
-    ControlBoard.ampScore.whileTrue(superstructure.scoreAmpCommand())
-    ControlBoard.scoreSpeakerLow.whileTrue(superstructure.scoreSpeakerLowCommand())
-    ControlBoard.scoreSpeakerMid.whileTrue(superstructure.scoreSpeakerMidCommand())
-    ControlBoard.scoreSpeakerHigh.whileTrue(superstructure.scoreSpeakerHighCommand())
-    ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand())
-    ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand())
-    ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand())
+    ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 0.degrees))
+    // ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand())
+    //    ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
+    //    ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand())
+    //    ControlBoard.ampScore.whileTrue(superstructure.scoreAmpCommand())
+    //    ControlBoard.scoreSpeakerLow.whileTrue(superstructure.scoreSpeakerLowCommand())
+    //    ControlBoard.scoreSpeakerMid.whileTrue(superstructure.scoreSpeakerMidCommand())
+    //    ControlBoard.scoreSpeakerHigh.whileTrue(superstructure.scoreSpeakerHighCommand())
+    //    ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand())
+    //    ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand())
+    //    ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand())
+    ControlBoard.runGroundIntake.whileTrue(ExamplePathAuto(drivetrain))
 
     /*
     TUNING COMMANDS

diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
@@ -1,6 +1,6 @@
 package com.team4099.robot2023.auto
 
-import com.team4099.robot2023.auto.mode.TestAutoPath
+import com.team4099.robot2023.auto.mode.ExamplePathAuto
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import edu.wpi.first.networktables.GenericEntry
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets
@@ -56,11 +56,11 @@ object AutonomousSelector {
     get() = secondaryWaitInAuto.getDouble(0.0).seconds
 
   fun getCommand(drivetrain: Drivetrain): Command {
-    val mode = autonomousModeChooser.get()
+    val mode = AutonomousMode.TEST_AUTO_PATH
     //    println("${waitTime().inSeconds} wait command")
     when (mode) {
       AutonomousMode.TEST_AUTO_PATH ->
-        return WaitCommand(waitTime.inSeconds).andThen(TestAutoPath(drivetrain))
+        return WaitCommand(waitTime.inSeconds).andThen(ExamplePathAuto(drivetrain))
       else -> println("ERROR: unexpected auto mode: $mode")
     }
     return InstantCommand()

diff --git a/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt b/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt
@@ -3,5 +3,5 @@ package com.team4099.robot2023.auto
 import com.pathplanner.lib.path.PathPlannerPath
 
 object PathStore {
-  val examplePath: PathPlannerPath = PathPlannerPath.fromPathFile("Example Path")
+  val examplePath: PathPlannerPath = PathPlannerPath.fromPathFile("5NotePureMove")
 }