add controller bindings and update speeds

src/main/kotlin/frc/team449/ControllerBindings.kt

@@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.ConditionalCommand
 import edu.wpi.first.wpilibj2.command.InstantCommand
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
+import frc.team449.commands.driveAlign.SimpleReefAlign
 import frc.team449.subsystems.RobotConstants
 import frc.team449.subsystems.drive.swerve.SwerveSim
 import frc.team449.subsystems.superstructure.SuperstructureGoal
@@ -22,21 +23,86 @@ class ControllerBindings(
 
   private fun robotBindings() {
     /** Call robot functions you create below */
+    /** Driver: https://docs.google.com/drawings/d/13W3qlIxzIh5MTraZGWON7IqwJvovVr8eNBvjq8_vYZI/edit
+     * Operator: https://docs.google.com/drawings/d/1lF4Roftk6932jMCQthgKfoJVPuTVSgnGZSHs5j68uo4/edit
+     */
+    SCORE_L1()
+    SCORE_L2()
+    SCORE_L3()
+    SCORE_L4()
+
+    PREMOVE_L1()
+    PREMOVE_L2()
+    PREMOVE_L3()
+    PREMOVE_L4()
+
+    STOW()
+  }
+
+  private fun nonRobotBindings() {
+    // slowDrive()
+
+    if (RobotBase.isSimulation()) resetOdometrySim()
+
+    resetGyro()
+  }
+
+  private fun STOW() {
+    mechanismController.rightBumper().onTrue(
+      robot.superstructureManager.requestGoal(SuperstructureGoal.STOW)
+    )
+  }
+
+  private fun SCORE_L1() {
     driveController.a().onTrue(
       robot.superstructureManager.requestGoal(SuperstructureGoal.L1)
+        .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
     )
+  }
 
+  private fun SCORE_L2() {
+    driveController.x().onTrue(
+      robot.superstructureManager.requestGoal(SuperstructureGoal.L2)
+        .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
+    )
+  }
+
+  private fun SCORE_L3() {
     driveController.b().onTrue(
-      robot.superstructureManager.requestGoal(SuperstructureGoal.STOW)
+      robot.superstructureManager.requestGoal(SuperstructureGoal.L3)
+        .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
     )
   }
 
-  private fun nonRobotBindings() {
-    // slowDrive()
+  private fun SCORE_L4() {
+    mechanismController.y().onTrue(
+      robot.superstructureManager.requestGoal(SuperstructureGoal.L4)
+        .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
+    )
+  }
 
-    if (RobotBase.isSimulation()) resetOdometrySim()
+  private fun PREMOVE_L1() {
+    mechanismController.a().onTrue(
+      robot.superstructureManager.requestGoal(SuperstructureGoal.L1_PREMOVE)
+    )
+  }
 
-    resetGyro()
+  private fun PREMOVE_L2() {
+    mechanismController.x().onTrue(
+      robot.superstructureManager.requestGoal(SuperstructureGoal.L2_PREMOVE)
+    )
+  }
+
+  private fun PREMOVE_L3() {
+    mechanismController.b().onTrue(
+      robot.superstructureManager.requestGoal(SuperstructureGoal.L3_PREMOVE)
+    )
+  }
+
+  private fun PREMOVE_L4() {
+    mechanismController.y().onTrue(
+      robot.superstructureManager.requestGoal(SuperstructureGoal.L4_PREMOVE)
+    )
   }
 
   private fun slowDrive() {

src/main/kotlin/frc/team449/RobotLoop.kt

@@ -14,6 +14,7 @@ import frc.team449.auto.RoutineChooser
 import frc.team449.commands.light.BlairChasing
 import frc.team449.commands.light.BreatheHue
 import frc.team449.commands.light.Rainbow
+import frc.team449.subsystems.FieldConstants
 import frc.team449.subsystems.drive.swerve.SwerveSim
 import frc.team449.subsystems.elevator.ElevatorConstants
 import frc.team449.subsystems.elevator.ElevatorFeedForward.Companion.createElevatorFeedForward
@@ -23,6 +24,7 @@ import monologue.Annotations.Log
 import monologue.Logged
 import monologue.Monologue
 import org.littletonrobotics.urcl.URCL
+import kotlin.jvm.optionals.getOrDefault
 import kotlin.jvm.optionals.getOrNull
 
 /** The main class of the robot, constructs all the subsystems
@@ -83,6 +85,8 @@ class RobotLoop : TimedRobot(), Logged {
 
   override fun driverStationConnected() {
     Monologue.setFileOnly(DriverStation.isFMSAttached())
+
+    FieldConstants.configureReef(DriverStation.getAlliance().getOrDefault(DriverStation.Alliance.Blue))
   }
 
   override fun robotPeriodic() {

src/main/kotlin/frc/team449/auto/RoutineChooser.kt

@@ -4,18 +4,26 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser
 import edu.wpi.first.wpilibj2.command.Command
 import frc.team449.Robot
 import frc.team449.auto.routines.DoNothing
+import frc.team449.auto.routines.ThreeL4
 
 class RoutineChooser(private val robot: Robot) : SendableChooser<String>() {
 
   fun routineMap(): HashMap<String, Command> {
     return hashMapOf(
       "RedDoNothing" to DoNothing(robot).createCommand(),
-      "BlueDoNothing" to DoNothing(robot).createCommand()
+      "BlueDoNothing" to DoNothing(robot).createCommand(),
+      "RedRedStageThreeL4" to ThreeL4(robot, isRedAlliance = true, isRedStage = true).createCommand(),
+      "RedBlueStageThreeL4" to ThreeL4(robot, isRedAlliance = true, isRedStage = false).createCommand(),
+      "BlueRedStageThreeL4" to ThreeL4(robot, isRedAlliance = false, isRedStage = true).createCommand(),
+      "BlueBlueStageThreeL4" to ThreeL4(robot, isRedAlliance = false, isRedStage = false).createCommand(),
     )
   }
 
   fun createOptions() {
     /** Add auto options here */
     this.setDefaultOption("Do Nothing", "DoNothing")
+
+    this.addOption("Red Stage 3 L4", "RedStageThreeL4")
+    this.addOption("Blue Stage 3 L4", "BlueStageThreeL4")
   }
 }

src/main/kotlin/frc/team449/subsystems/pivot/PivotConstants.kt

@@ -39,10 +39,9 @@ object PivotConstants {
   // TODO(Adjust gains.)
   const val KS = 0.0
   const val KV = 0.0
-  const val KA = 0.0
   const val KG_MIN = 0.0 // (KG when elevator is fully retracted.)
   const val KG_MAX = 0.0 // TODO(KG when elevator is fully extended.)
 
-  val CRUISE_VEL = RadiansPerSecond.of(PI)
-  val MAX_ACCEL = RadiansPerSecondPerSecond.of(6 * PI)
+  val CRUISE_VEL = RotationsPerSecond.of(0.3)
+  val MAX_ACCEL = RotationsPerSecondPerSecond.of(5.0)
 }

src/main/kotlin/frc/team449/subsystems/pivot/PivotFeedForward.kt

@@ -12,7 +12,6 @@ class PivotFeedForward(
   private val elevator: Elevator,
   private val ks: Double,
   private val kv: Double,
-  private val ka: Double,
   kgRetracted: Double,
   kgExtended: Double
 ) {
@@ -37,7 +36,6 @@ class PivotFeedForward(
         elevator,
         PivotConstants.KS,
         PivotConstants.KV,
-        PivotConstants.KA,
         PivotConstants.KG_MIN,
         PivotConstants.KG_MAX,
       )

src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureGoal.kt

@@ -8,7 +8,7 @@ import frc.team449.subsystems.RobotConstants
 import frc.team449.subsystems.drive.swerve.SwerveDrive
 
 object SuperstructureGoal {
-
+  /** TODO: All placeholder guesses, need actual values */
   val L1 = SuperstructureState(
     Degrees.of(30.0),
     Meters.of(0.25),
@@ -32,7 +32,7 @@ object SuperstructureGoal {
 
   val L4 = SuperstructureState(
     Degrees.of(60.0),
-    Meters.of(1.75),
+    Meters.of(1.65),
     Degrees.of(-50.0),
     DriveDynamics(RobotConstants.MAX_LINEAR_SPEED, RobotConstants.MAX_ACCEL, RobotConstants.MAX_ROT_SPEED)
   )
@@ -75,7 +75,7 @@ object SuperstructureGoal {
   val L4_PREMOVE = SuperstructureState(
     L4.pivot,
     STOW.elevator,
-    STOW.wrist,
+    SUBSTATION_INTAKE.wrist,
     DriveDynamics(RobotConstants.MAX_LINEAR_SPEED, RobotConstants.MAX_ACCEL, RobotConstants.MAX_ROT_SPEED)
   )
 

src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureManager.kt

@@ -12,10 +12,10 @@ import frc.team449.subsystems.pivot.Pivot
 import frc.team449.subsystems.wrist.Wrist
 
 class SuperstructureManager(
-  val elevator: Elevator,
-  val pivot: Pivot,
-  val wrist: Wrist,
-  val drive: SwerveDrive
+  private val elevator: Elevator,
+  private val pivot: Pivot,
+  private val wrist: Wrist,
+  private val drive: SwerveDrive
 ) {
 
   fun requestGoal(goal: SuperstructureGoal.SuperstructureState): Command {