tune amp score

simgui-ds.json

@@ -1,9 +1,4 @@
 {
-  "System Joysticks": {
-    "window": {
-      "visible": false
-    }
-  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
@@ -97,6 +92,9 @@
   "robotJoysticks": [
     {
       "guid": "Keyboard0"
+    },
+    {
+      "guid": "Keyboard1"
     }
   ]
 }

src/main/kotlin/com/team4099/robot2023/RobotContainer.kt

@@ -152,16 +152,16 @@ 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.intake.whileTrue(superstructure.groundIntakeCommand())
+    ControlBoard.prepAmp.whileTrue(superstructure.prepAmpCommand())
+    ControlBoard.prepMid.whileTrue(superstructure.prepSpeakerMidCommand())
+    ControlBoard.prepHigh.whileTrue(superstructure.prepSpeakerHighCommand())
+    ControlBoard.score.whileTrue(superstructure.scoreCommand())
+    ControlBoard.extendClimb.whileTrue(superstructure.climbExtendCommand())
+    ControlBoard.retractClimb.whileTrue(superstructure.climbRetractCommand())
+
+    ControlBoard.testWrist.whileTrue(superstructure.testWristCommand())
+    ControlBoard.forceIdle.whileTrue(superstructure.requestIdleCommand())
 
     /*
     TUNING COMMANDS

src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt

@@ -27,41 +27,27 @@ object ControlBoard {
     get() = -driver.leftYAxis
 
   val turn: Double
-    get() = driver.rightXAxis
+    get() = operator.leftXAxis
 
   val slowMode: Boolean
     get() = driver.leftShoulderButton
 
   val resetGyro = Trigger { driver.startButton && driver.selectButton }
 
-  val shooterUp = Trigger { driver.bButton }
-  val shooterDown = Trigger { driver.xButton }
-  val wristTestUp = Trigger { driver.yButton }
-  val wristTestDown = Trigger { driver.aButton }
-  val feederTest = Trigger { driver.rightShoulderButton }
+  // sim triggers
+  val intake = Trigger {driver.aButton}
+  val score = Trigger { driver.bButton}
+  val forceIdle = Trigger { driver.yButton }
 
-  val elevatorUp = Trigger { driver.rightTriggerAxis > 0.5 }
-  val elevatorDown = Trigger { driver.leftTriggerAxis > 0.5 }
+  val prepAmp = Trigger  { operator.aButton }
+  val prepMid = Trigger { operator.bButton }
+  val prepHigh = Trigger { operator.xButton}
 
-  val runGroundIntake = Trigger { driver.leftShoulderButton }
-  val ejectGamePiece = Trigger { driver.rightShoulderButton }
-  val prepAmpScore = Trigger { driver.xButton }
-  val ampScore = Trigger { driver.yButton }
+  val extendClimb = Trigger { technician.aButton }
+  val retractClimb = Trigger { technician.bButton }
 
-  val scoreSpeakerLow = Trigger { operator.aButton }
-  val scoreSpeakerMid = Trigger { operator.bButton }
-  val scoreSpeakerHigh = Trigger { operator.xButton }
-  val requestIdle = Trigger { operator.yButton }
+  val testWrist = Trigger {driver.xButton}
 
-  val climbExtend = Trigger { driver.dPadUp }
-  val climbRetract = Trigger { driver.dPadDown }
 
-  // testing Trigger
-  val testIntake = Trigger { driver.aButton }
-  val testFeederIntake = Trigger { driver.bButton }
-  val testFeederShoot = Trigger { driver.xButton }
-  val testFlywheel = Trigger { driver.yButton }
-  val testWrist = Trigger { operator.aButton }
-  val testElevator = Trigger { operator.bButton }
-  val setTuningMode = Trigger { driver.rightShoulderButton }
+  // week0 controls
 }

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

@@ -64,7 +64,7 @@ object ElevatorConstants {
   val SHOOT_SPEAKER_LOW_POSITION = 0.0.inches
   val SHOOT_SPEAKER_MID_POSITION = 9.0.inches
   val SHOOT_SPEAKER_HIGH_POSITION = 14.0.inches
-  val SHOOT_AMP_POSITION = 0.0.inches
+  val SHOOT_AMP_POSITION = 10.0.inches
   val SOURCE_NOTE_OFFSET = 0.0.inches
   val ELEVATOR_THETA_POS = 0.0.degrees
   val HOMING_STATOR_CURRENT = 3.0.amps

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt

@@ -107,14 +107,11 @@ class SwerveModuleIOTalon(
 
     /*
     steeringConfiguration.ClosedLoopGeneral.ContinuousWrap = true
-<<<<<<< HEAD
     steeringConfiguration.Feedback.SensorToMechanismRatio =
       1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO
-=======
     steeringConfiguration.Feedback.SensorToMechanismRatio = 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO
      */
 
->>>>>>> e907d12 (MVP fixes)
     steeringConfiguration.MotorOutput.NeutralMode =
       NeutralModeValue.Brake // change back to coast maybe?
     steeringFalcon.inverted = true

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

@@ -21,9 +21,12 @@ sealed interface Request {
 
     class ScoreAmp() : SuperstructureRequest
 
-    class ScoreSpeakerLow() : SuperstructureRequest
-    class ScoreSpeakerMid() : SuperstructureRequest
-    class ScoreSpeakerHigh() : SuperstructureRequest
+    class PrepScoreSpeakerMid() : SuperstructureRequest
+
+    class PrepScoreSpeakerHigh() : SuperstructureRequest
+
+    class ScoreSpeaker() : SuperstructureRequest
+
     class ClimbExtend() : SuperstructureRequest
 
     class ClimbRetract() : SuperstructureRequest

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

@@ -197,13 +197,14 @@ class Superstructure(
           is Request.SuperstructureRequest.ScoreAmp -> {
             nextState = SuperstructureStates.SCORE_AMP
           }
-          is Request.SuperstructureRequest.ScoreSpeakerLow -> {
+          is Request.SuperstructureRequest.ScoreSpeaker -> {
             nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP
           }
-          is Request.SuperstructureRequest.ScoreSpeakerMid -> {
+
+          is Request.SuperstructureRequest.PrepScoreSpeakerMid -> {
             nextState = SuperstructureStates.SCORE_SPEAKER_MID_PREP
           }
-          is Request.SuperstructureRequest.ScoreSpeakerHigh -> {
+          is Request.SuperstructureRequest.PrepScoreSpeakerHigh -> {
             nextState = SuperstructureStates.SCORE_SPEAKER_HIGH_PREP
           }
           is Request.SuperstructureRequest.ClimbExtend -> {
@@ -280,7 +281,7 @@ class Superstructure(
         if (!feeder.hasNote &&
           Clock.fpgaTime - shootStartTime > Flywheel.TunableFlywheelStates.ampScoreTime.get()
         ) {
-          nextState = SuperstructureStates.IDLE
+          currentRequest = Request.SuperstructureRequest.Idle()
         }
 
         when (currentRequest) {
@@ -306,7 +307,7 @@ class Superstructure(
           flywheel.isAtTargetedVelocity &&
           elevator.isAtTargetedPosition
         ) {
-          nextState = SuperstructureStates.SCORE_SPEAKER_LOW
+          nextState = SuperstructureStates.SCORE_SPEAKER
         }
 
         when (currentRequest) {
@@ -315,14 +316,14 @@ class Superstructure(
           }
         }
       }
-      SuperstructureStates.SCORE_SPEAKER_LOW -> {
+      SuperstructureStates.SCORE_SPEAKER -> {
         feeder.currentRequest =
           Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.shootVoltage.get())
         if (!feeder.hasNote &&
           Clock.fpgaTime - shootStartTime >
           Flywheel.TunableFlywheelStates.speakerScoreTime.get()
         ) {
-          nextState = SuperstructureStates.IDLE
+          currentRequest = Request.SuperstructureRequest.Idle()
         }
 
         when (currentRequest) {
@@ -346,9 +347,9 @@ class Superstructure(
           )
         if (wrist.isAtTargetedPosition &&
           flywheel.isAtTargetedVelocity &&
-          elevator.isAtTargetedPosition
+          elevator.isAtTargetedPosition && currentRequest is Request.SuperstructureRequest.ScoreSpeaker
         ) {
-          nextState = SuperstructureStates.SCORE_SPEAKER_MID
+          nextState = SuperstructureStates.SCORE_SPEAKER
         }
 
         when (currentRequest) {
@@ -357,21 +358,7 @@ class Superstructure(
           }
         }
       }
-      SuperstructureStates.SCORE_SPEAKER_MID -> {
-        feeder.currentRequest =
-          Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.shootVoltage.get())
-        if (!feeder.hasNote &&
-          Clock.fpgaTime - shootStartTime > Flywheel.TunableFlywheelStates.ampScoreTime.get()
-        ) {
-          nextState = SuperstructureStates.IDLE
-        }
 
-        when (currentRequest) {
-          is Request.SuperstructureRequest.Idle -> {
-            nextState = SuperstructureStates.IDLE
-          }
-        }
-      }
       SuperstructureStates.SCORE_SPEAKER_HIGH_PREP -> {
         elevator.currentRequest =
           Request.ElevatorRequest.TargetingPosition(
@@ -387,24 +374,9 @@ class Superstructure(
           )
         if (wrist.isAtTargetedPosition &&
           flywheel.isAtTargetedVelocity &&
-          elevator.isAtTargetedPosition
+          elevator.isAtTargetedPosition && currentRequest is Request.SuperstructureRequest.ScoreSpeaker
         ) {
-          nextState = SuperstructureStates.SCORE_SPEAKER_HIGH
-        }
-
-        when (currentRequest) {
-          is Request.SuperstructureRequest.Idle -> {
-            nextState = SuperstructureStates.IDLE
-          }
-        }
-      }
-      SuperstructureStates.SCORE_SPEAKER_HIGH -> {
-        feeder.currentRequest =
-          Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.shootVoltage.get())
-        if (!feeder.hasNote &&
-          Clock.fpgaTime - shootStartTime > Flywheel.TunableFlywheelStates.ampScoreTime.get()
-        ) {
-          nextState = SuperstructureStates.IDLE
+          nextState = SuperstructureStates.SCORE_SPEAKER
         }
 
         when (currentRequest) {
@@ -543,39 +515,37 @@ class Superstructure(
     return returnCommand
   }
 
-  fun scoreAmpCommand(): Command {
-    val returnCommand =
-      runOnce { currentRequest = Request.SuperstructureRequest.ScoreAmp() }.until {
-        isAtRequestedState && currentState == SuperstructureStates.SCORE_AMP
-      }
-    returnCommand.name = "ScoreAmpCommand"
-    return returnCommand
-  }
 
-  fun scoreSpeakerLowCommand(): Command {
+
+  fun scoreCommand(): Command {
     val returnCommand =
-      runOnce { currentRequest = Request.SuperstructureRequest.ScoreSpeakerLow() }.until {
-        isAtRequestedState && currentState == SuperstructureStates.SCORE_SPEAKER_LOW
+      runOnce { if (currentState == SuperstructureStates.SCORE_AMP_PREP) {
+        currentRequest = Request.SuperstructureRequest.ScoreAmp()
+      } else {
+        currentRequest = Request.SuperstructureRequest.ScoreSpeaker()
+      }
+        }.until {
+        isAtRequestedState && currentState == SuperstructureStates.SCORE_SPEAKER
       }
-    returnCommand.name = "ScoreSpeakerLowCommand"
+    returnCommand.name = "ScoreSpeakerCommand"
     return returnCommand
   }
 
-  fun scoreSpeakerMidCommand(): Command {
+  fun prepSpeakerMidCommand(): Command {
     val returnCommand =
-      runOnce { currentRequest = Request.SuperstructureRequest.ScoreSpeakerMid() }.until {
-        isAtRequestedState && currentState == SuperstructureStates.SCORE_SPEAKER_MID
+      runOnce { currentRequest = Request.SuperstructureRequest.PrepScoreSpeakerMid() }.until {
+        isAtRequestedState && currentState == SuperstructureStates.SCORE_SPEAKER_MID_PREP
       }
-    returnCommand.name = "ScoreSpeakerMidCommand"
+    returnCommand.name = "ScoreSpeakerCommand"
     return returnCommand
   }
 
-  fun scoreSpeakerHighCommand(): Command {
+  fun prepSpeakerHighCommand(): Command {
     val returnCommand =
-      runOnce { currentRequest = Request.SuperstructureRequest.ScoreSpeakerHigh() }.until {
-        isAtRequestedState && currentState == SuperstructureStates.SCORE_SPEAKER_HIGH
+      runOnce { currentRequest = Request.SuperstructureRequest.PrepScoreSpeakerHigh() }.until {
+        isAtRequestedState && currentState == SuperstructureStates.SCORE_SPEAKER_HIGH_PREP
       }
-    returnCommand.name = "ScoreSpeakerHighCommand"
+    returnCommand.name = "ScoreSpeakerCommand"
     return returnCommand
   }
 
@@ -685,11 +655,9 @@ class Superstructure(
       SCORE_AMP_PREP,
       SCORE_AMP,
       SCORE_SPEAKER_LOW_PREP,
-      SCORE_SPEAKER_LOW,
       SCORE_SPEAKER_MID_PREP,
-      SCORE_SPEAKER_MID,
       SCORE_SPEAKER_HIGH_PREP,
-      SCORE_SPEAKER_HIGH,
+      SCORE_SPEAKER,
       CLIMB_EXTEND,
       CLIMB_RETRACT,
       EJECT_GAME_PIECE,

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

@@ -69,14 +69,14 @@ class Wrist(val io: WristIO) : SubsystemBase() {
       )
     val subwooferSpeakerShotAngleMid =
       LoggedTunableValue(
-        "Wrist/subwooferSpeakerShotAngleLow",
+        "Wrist/subwooferSpeakerShotAngleMid",
         WristConstants.SUBWOOFER_SPEAKER_SHOT_ANGLE_MID,
         Pair({ it.inDegrees }, { it.degrees })
       )
     val subwooferSpeakerShotAngleHigh =
       LoggedTunableValue(
-        "Wrist/subwooferSpeakerShotAngleLow",
-        WristConstants.SUBWOOFER_SPEAKER_SHOT_ANGLE_HIGH,
+        "Wrist/subwooferSpeakerShotAngleHigh",
+        WristConstants.SUBWOOFER_SPEAKER_SHOT_ANGLgiE_HIGH,
         Pair({ it.inDegrees }, { it.degrees })
       )
     val climbAngle =