completed superstructure uninitialized, homeprep, home, idle 🥳

src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt

@@ -60,7 +60,9 @@ object ElevatorConstants {
   val MAX_VELOCITY = 0.82.meters.perSecond
   val MAX_ACCELERATION = 2.meters.perSecond.perSecond
 
-  val SHOOT_SPEAKER_POSITION = 0.0.inches
+  val SHOOT_SPEAKER_LOW_POSITION = 0.0.inches
+  val SHOOT_SPEAKER_MID_POSITION = 9.0.inches
+  val SHOOT_SPEAKER_HIGH_POSITION = 17.0.inches
   val SHOOT_AMP_POSITION = 0.0.inches
   val SOURCE_NOTE_OFFSET = 0.0.inches
   val ELEVATOR_THETA_POS = 0.0.degrees

src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt

@@ -25,6 +25,8 @@ object FeederConstants {
 
   var WAIT_BEFORE_DETECT_VELOCITY_DROP = 1.seconds
 
+  val IDLE_VOLTAGE = 0.volts
   val INTAKE_NOTE_VOLTAGE = 6.volts
+  val OUTTAKE_NOTE_VOLTAGE = (-6).volts
   val SHOOT_NOTE_VOLTAGE = 11.9.volts
 }

src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt

@@ -82,4 +82,7 @@ object FlywheelConstants {
     val LEFT_SIM_FLYWHEEL_KV = 0.0197.volts / 1.radians.perSecond
     val LEFT_SIM_FLYWHEEL_KA = 0.03.volts / 1.radians.perSecond.perSecond
   }
+
+  val IDLE_VELOCITY = 0.0.rotations.perMinute
+  val SHOOT_VELOCITY = 10_000.rotations.perMinute
 }

src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt

@@ -14,4 +14,8 @@ object IntakeConstants {
 
   // TODO: Update the idle roller voltage later
   val IDLE_ROLLER_VOLTAGE = 1.0.volts
+
+  val IDLE_VOLTAGE = 0.0.volts
+  val INTAKE_VOLTAGE = 10.volts
+  val OUTTAKE_VOLTAGE = (-10).volts
 }

src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt

@@ -54,4 +54,9 @@ object WristConstants {
   }
 
   val WRIST_TOLERANCE = 0.01.degrees
+
+  val IDLE_ANGLE = (-45.0).degrees
+  val AMP_SCORE_ANGLE = 0.0.degrees
+  val SUBWOOFER_SPEAKER_SHOT_ANGLE = 0.0.degrees
+  val CLIMB_ANGLE = 20.0.degrees
 }

src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt

@@ -100,10 +100,15 @@ class Elevator(val io: ElevatorIO) : SubsystemBase() {
         Pair({ it.inVolts }, { it.volts })
       )
 
-    val shootSpeakerPosition =
-      LoggedTunableValue(
-        "Elevator/shootSpeakerPosition", ElevatorConstants.SHOOT_SPEAKER_POSITION
-      )
+    val shootSpeakerLow = LoggedTunableValue(
+      "Elevator/shootSpeakerLow", ElevatorConstants.SHOOT_SPEAKER_LOW_POSITION
+    )
+    val shootSpeakerMid = LoggedTunableValue(
+      "Elevator/shootSpeakerMid", ElevatorConstants.SHOOT_SPEAKER_MID_POSITION
+    )
+    val shootSpeakerHigh = LoggedTunableValue(
+      "Elevator/shootSpeakerHigh", ElevatorConstants.SHOOT_SPEAKER_HIGH_POSITION
+    )
     val shootAmpPosition =
       LoggedTunableValue("Elevator/shootAmpPosition", ElevatorConstants.SHOOT_AMP_POSITION)
     val sourceNoteOffset =

src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt

@@ -1,6 +1,7 @@
 package com.team4099.robot2023.subsystems.feeder
 
 import com.team4099.lib.hal.Clock
+import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.FeederConstants
 import com.team4099.robot2023.subsystems.superstructure.Request
 import edu.wpi.first.wpilibj2.command.Command
@@ -9,11 +10,27 @@ import org.littletonrobotics.junction.Logger
 import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.inVolts
 import org.team4099.lib.units.derived.volts
 
 class Feeder(val io: FeederIO) : SubsystemBase() {
   val inputs = FeederIO.FeederIOInputs()
 
+  object TunableFeederStates {
+    val idleVoltage = LoggedTunableValue(
+      "Feeder/idleVoltage", FeederConstants.IDLE_VOLTAGE, Pair({ it.inVolts}, { it.volts })
+    )
+    val intakeVoltage = LoggedTunableValue(
+      "Feeder/intakeVoltage", FeederConstants.INTAKE_NOTE_VOLTAGE, Pair({ it.inVolts}, { it.volts })
+    )
+    val outtakeVoltage = LoggedTunableValue(
+      "Feeder/outtakeVoltage", FeederConstants.OUTTAKE_NOTE_VOLTAGE, Pair({ it.inVolts}, { it.volts })
+    )
+    val shootVoltage = LoggedTunableValue(
+      "Feeder/shootVoltage", FeederConstants.SHOOT_NOTE_VOLTAGE, Pair({ it.inVolts}, { it.volts })
+    )
+  }
+
   var feederTargetVoltage: ElectricalPotential = 0.0.volts
   var lastFeederRunTime = 0.0.seconds
 

src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt

@@ -3,6 +3,7 @@ package com.team4099.robot2023.subsystems.flywheel
 import com.team4099.lib.hal.Clock
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.FeederConstants
 import com.team4099.robot2023.config.constants.FlywheelConstants
 import com.team4099.robot2023.subsystems.superstructure.Request
 import edu.wpi.first.wpilibj.RobotBase
@@ -30,6 +31,15 @@ import org.team4099.lib.units.perMinute
 import org.team4099.lib.units.perSecond
 
 class Flywheel(val io: FlywheelIO) : SubsystemBase() {
+  object TunableFlywheelStates {
+    val idleVelocity = LoggedTunableValue(
+      "Flywheel/idleVelocity", FlywheelConstants.IDLE_VELOCITY, Pair({ it.inRotationsPerMinute}, { it.rotations.perMinute })
+    )
+    val shootVelocity = LoggedTunableValue(
+      "Flywheel/shootVelocity", FlywheelConstants.SHOOT_VELOCITY, Pair({ it.inRotationsPerMinute}, { it.rotations.perMinute })
+    )
+  }
+
   private val rightkP =
     LoggedTunableValue(
       "Flywheel/Right kP",

src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt

@@ -1,19 +1,36 @@
 package com.team4099.robot2023.subsystems.intake
 
 import com.team4099.lib.hal.Clock
+import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.IntakeConstants
+import com.team4099.robot2023.config.constants.WristConstants
 import com.team4099.robot2023.subsystems.superstructure.Request
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inDegrees
 import org.team4099.lib.units.derived.inVolts
 import org.team4099.lib.units.derived.volts
 
 class Intake(val io: IntakeIO) : SubsystemBase() {
   val inputs = IntakeIO.IntakeIOInputs()
+
+  object TunableIntakeStates {
+    val idleVoltage = LoggedTunableValue(
+      "Intake/idleVoltage", IntakeConstants.IDLE_VOLTAGE, Pair({ it.inVolts}, { it.volts })
+    )
+    val intakeVoltage = LoggedTunableValue(
+      "Intake/intakeVoltage", IntakeConstants.INTAKE_VOLTAGE, Pair({ it.inVolts}, { it.volts })
+    )
+    val outtakeVoltage = LoggedTunableValue(
+      "Intake/outtakeVoltage", IntakeConstants.OUTTAKE_VOLTAGE, Pair({ it.inVolts}, { it.volts })
+    )
+  }
+
   var rollerVoltageTarget: ElectricalPotential = 0.0.volts
   var isZeroed = false
   var currentState: IntakeState = IntakeState.UNINITIALIZED

src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt

@@ -1,7 +1,6 @@
 package com.team4099.robot2023.subsystems.superstructure
 
 import com.team4099.lib.hal.Clock
-import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.subsystems.elevator.Elevator
 import com.team4099.robot2023.subsystems.feeder.Feeder
 import com.team4099.robot2023.subsystems.flywheel.Flywheel
@@ -13,7 +12,7 @@ import org.team4099.lib.units.base.inMilliseconds
 
 class Superstructure(
     private val intake: Intake,
-    private val feeder: Feeder
+    private val feeder: Feeder,
     private val elevator: Elevator,
     private val wrist: Wrist,
     private val flywheel: Flywheel
@@ -57,12 +56,35 @@ class Superstructure(
 
         var nextState = currentState
         when (currentState) {
-            SuperstructureStates.UNINITIALIZED -> {}
+            SuperstructureStates.UNINITIALIZED -> {
+                nextState = SuperstructureStates.HOME_PREP
+            }
             SuperstructureStates.TUNING -> {}
-            SuperstructureStates.IDLE -> {}
             SuperstructureStates.HOME_PREP -> {
+                wrist.currentRequest = Request.WristRequest.Zero()
+
+                if (wrist.isZeroed) {
+                    wrist.currentRequest = Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.idleAngle.get())
+
+                    if (wrist.isAtTargetedPosition) {
+                        nextState = SuperstructureStates.HOME
+                    }
+                }
+            }
+            SuperstructureStates.HOME -> {
+                elevator.currentRequest = Request.ElevatorRequest.Home()
+
+                if (elevator.isHomed) {
+                    nextState = SuperstructureStates.IDLE
+                }
+            }
+            SuperstructureStates.IDLE -> {
+                intake.currentRequest = Request.IntakeRequest.OpenLoop(Intake.TunableIntakeStates.idleVoltage.get())
+                feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(Feeder.TunableFeederStates.idleVoltage.get())
+                flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(Flywheel.TunableFlywheelStates.idleVelocity.get())
+                wrist.currentRequest = Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.idleAngle.get())
+                elevator.currentRequest = Request.ElevatorRequest.TargetingPosition(Elevator.TunableElevatorHeights.minPosition.get())
             }
-            SuperstructureStates.HOME -> {}
             SuperstructureStates.GROUND_INTAKE_PREP -> {}
             SuperstructureStates.GROUND_INTAKE -> {}
             SuperstructureStates.SCORE_AMP_PREP -> {}

src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt

@@ -38,6 +38,21 @@ import org.team4099.lib.units.perSecond
 class Wrist(val io: WristIO) : SubsystemBase() {
   val inputs = WristIO.WristIOInputs()
 
+  object TunableWristStates {
+    val idleAngle = LoggedTunableValue(
+      "Wrist/idleAngle", WristConstants.IDLE_ANGLE, Pair({ it.inDegrees}, { it.degrees })
+    )
+    val ampScoreAngle = LoggedTunableValue(
+      "Wrist/ampScoreAngle", WristConstants.AMP_SCORE_ANGLE, Pair({ it.inDegrees}, { it.degrees })
+    )
+    val subwooferSpeakerShotAngle = LoggedTunableValue(
+      "Wrist/subwooferSpeakerShotAngle", WristConstants.SUBWOOFER_SPEAKER_SHOT_ANGLE, Pair({ it.inDegrees}, { it.degrees })
+    )
+    val climbAngle = LoggedTunableValue(
+      "Wrist/climbAngle", WristConstants.CLIMB_ANGLE, Pair({ it.inDegrees}, { it.degrees })
+    )
+  }
+
   private val wristkS =
     LoggedTunableValue(
       "Wrist/kS", WristConstants.PID.WRIST_KS, Pair({ it.inVolts }, { it.volts })
@@ -85,6 +100,8 @@ class Wrist(val io: WristIO) : SubsystemBase() {
 
   var currentState: WristStates = WristStates.UNINITIALIZED
 
+  var isZeroed = false
+
   var wristTargetVoltage: ElectricalPotential = 0.0.volts
 
   private var lastWristPositionTarget = 0.0.degrees
@@ -172,6 +189,7 @@ class Wrist(val io: WristIO) : SubsystemBase() {
       }
       WristStates.ZERO -> {
         io.zeroEncoder()
+        isZeroed = true
         nextState = fromRequestToState(currentRequest)
       }
       WristStates.OPEN_LOOP -> {