force idle when auto aim release

team4099 · Apr 12, 2024 · 38b5483 · 38b5483
1 parent b8a4c77
commit 38b5483
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 1 deletion.
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -122,6 +122,8 @@ object RobotContainer {
         drivetrain
       )
 
+    superstructure.defaultCommand = superstructure.forceIdleIfAutoAimCommand()
+
     /*
     module steeing tuning
 
@@ -332,6 +334,7 @@ object RobotContainer {
           { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
           { ControlBoard.slowMode },
           drivetrain,
+          superstructure,
           vision
         ),
         superstructure.autoAimCommand()

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt
@@ -6,6 +6,7 @@ import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.config.constants.FieldConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import com.team4099.robot2023.subsystems.superstructure.Request
+import com.team4099.robot2023.subsystems.superstructure.Superstructure
 import com.team4099.robot2023.subsystems.vision.Vision
 import com.team4099.robot2023.util.driver.DriverProfile
 import com.team4099.robot2023.util.inverse
@@ -40,6 +41,7 @@ class TargetSpeakerCommand(
   val turn: () -> Double,
   val slowMode: () -> Boolean,
   val drivetrain: Drivetrain,
+  val superstructure: Superstructure,
   val vision: Vision
 ) : Command() {
   private var thetaPID: PIDController<Radian, Velocity<Radian>>
@@ -123,6 +125,7 @@ class TargetSpeakerCommand(
     }
 
     drivetrain.defaultCommand.end(true)
+    superstructure.defaultCommand.end(true)
     Logger.recordOutput("ActiveCommands/TargetAngleCommand", true)
     Logger.recordOutput(
       "Testing/CurrentDrivetrainRotation", drivetrain.odomTRobot.rotation.inDegrees

diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt
@@ -112,5 +112,5 @@ object WristConstants {
   val TRAP_ANGLE = 35.degrees
   val INTAKE_ANGLE = (-33.75).degrees
   val IDLE_ANGLE_HAS_GAMEPEICE = -34.degrees
-  val PASSING_SHOT_ANGLE = -20.degrees
+  val PASSING_SHOT_ANGLE = -34.degrees
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
@@ -874,6 +874,17 @@ class Superstructure(
     return returnCommand
   }
 
+  fun forceIdleIfAutoAimCommand() : Command {
+    val returnCommand =
+      run {
+        if (currentState == SuperstructureStates.AUTO_AIM) {
+          currentRequest = Request.SuperstructureRequest.Idle()
+        }
+      }
+    returnCommand.name = "forceIdleIfAutoAim"
+    return returnCommand
+  }
+
   fun ejectGamePieceCommand(): Command {
     val returnCommand =
       runOnce { currentRequest = Request.SuperstructureRequest.EjectGamePiece() }.until {
@@ -1027,6 +1038,8 @@ class Superstructure(
     return returnCommand
   }
 
+
+
   fun climbExtendCommand(): Command {
     val returnCommand =
       runOnce { currentRequest = Request.SuperstructureRequest.ClimbExtend() }.until {