add definitions for superstructure states

team4099 · Jan 30, 2024 · 0cf4874 · 0cf4874
1 parent a3c0419
commit 0cf4874
Show file tree

Hide file tree

Showing 10 changed files with 479 additions and 153 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt
@@ -24,6 +24,7 @@ object ElevatorConstants {
 
   val ELEVATOR_MAX_RETRACTION = 0.0.inches
   val ELEVATOR_MAX_EXTENSION = 18.0.inches
+  val ELEVATOR_CLIMB_EXTENSION = 12.0.inches
 
   val LEADER_INVERTED = false
   val FOLLOWER_INVERTED = true

diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt
@@ -85,4 +85,8 @@ object FlywheelConstants {
 
   val IDLE_VELOCITY = 0.0.rotations.perMinute
   val SHOOT_VELOCITY = 10_000.rotations.perMinute
+  val AMP_VELOCITY = 5_000.rotations.perMinute
+  val AMP_SCORE_TIME = 1.seconds
+  val SPEAKER_SCORE_TIME = 1.seconds
+  val EJECT_VELOCITY = 5_000.rotations.perMinute
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt
@@ -55,8 +55,12 @@ object WristConstants {
 
   val WRIST_TOLERANCE = 0.01.degrees
 
-  val IDLE_ANGLE = (-45.0).degrees
+  val IDLE_ANGLE = (-40.0).degrees
   val AMP_SCORE_ANGLE = 0.0.degrees
-  val SUBWOOFER_SPEAKER_SHOT_ANGLE = 0.0.degrees
+  val SUBWOOFER_SPEAKER_SHOT_ANGLE_LOW = 0.0.degrees
+  val SUBWOOFER_SPEAKER_SHOT_ANGLE_MID = 30.0.degrees
+  val SUBWOOFER_SPEAKER_SHOT_ANGLE_HIGH = 50.0.degrees
   val CLIMB_ANGLE = 20.0.degrees
+  val INTAKE_ANGLE = (-40.0).degrees
+  val IDLE_ANGLE_HAS_GAMEPEICE = 40.0.degrees
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt
@@ -85,6 +85,12 @@ class Elevator(val io: ElevatorIO) : SubsystemBase() {
         ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION,
         Pair({ it.inInches }, { it.inches })
       )
+    val climbExtend =
+      LoggedTunableValue(
+        "Elevator/climbExtend",
+        ElevatorConstants.ELEVATOR_CLIMB_EXTENSION,
+        Pair({ it.inInches }, { it.inches })
+      )
 
     // TODO: change voltages
     val openLoopExtendVoltage =
@@ -100,15 +106,14 @@ class Elevator(val io: ElevatorIO) : SubsystemBase() {
         Pair({ it.inVolts }, { it.volts })
       )
 
-    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 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 =

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt
@@ -17,18 +17,28 @@ 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 })
-    )
+    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

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt
@@ -3,7 +3,6 @@ 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
@@ -12,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.SimpleMotorFeedforward
 import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.Radian
@@ -32,12 +32,42 @@ 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 })
-    )
+    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 })
+      )
+    val ampVelocity =
+      LoggedTunableValue(
+        "Flywheel/ampVelocity",
+        FlywheelConstants.AMP_VELOCITY,
+        Pair({ it.inRotationsPerMinute }, { it.rotations.perMinute })
+      )
+    val ampScoreTime =
+      LoggedTunableValue(
+        "Flywheel/ampScoreTime",
+        FlywheelConstants.AMP_SCORE_TIME,
+        Pair({ it.inSeconds }, { it.seconds })
+      )
+    val speakerScoreTime =
+      LoggedTunableValue(
+        "Flywheel/speakerScoreTime",
+        FlywheelConstants.SPEAKER_SCORE_TIME,
+        Pair({ it.inSeconds }, { it.seconds })
+      )
+    val ejectVelocity =
+      LoggedTunableValue(
+        "Flywheel/ejectVelocity",
+        FlywheelConstants.EJECT_VELOCITY,
+        Pair({ it.inRotationsPerMinute }, { it.rotations.perMinute })
+      )
   }
 
   private val rightkP =

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt
@@ -4,31 +4,35 @@ 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 })
-    )
+    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

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt
@@ -21,8 +21,9 @@ sealed interface Request {
 
     class ScoreAmp() : SuperstructureRequest
 
-    class ScoreSpeaker() : SuperstructureRequest
-
+    class ScoreSpeakerLow() : SuperstructureRequest
+    class ScoreSpeakerMid() : SuperstructureRequest
+    class ScoreSpeakerHigh() : SuperstructureRequest
     class ClimbExtend() : SuperstructureRequest
 
     class ClimbRetract() : SuperstructureRequest