Setpoints for TeleOp

ftc13100 · Dec 21, 2024 · 2a1f6b6 · 2a1f6b6
1 parent 91fad6a
commit 2a1f6b6
Show file tree

Hide file tree

Showing 4 changed files with 65 additions and 24 deletions.
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeCommand.kt
@@ -7,7 +7,7 @@ class IntakeCommand(
     private val intake: Boolean,
     private val subsystem: IntakeSubsystem
 ) : CommandBase() {
-    override fun execute() {
+    override fun initialize() {
         if (intake) {
             subsystem.intake()
         }

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt
@@ -1,29 +1,39 @@
 package org.firstinspires.ftc.teamcode.opModes.teleOp
 
 import com.arcrobotics.ftclib.command.CommandOpMode
+import com.arcrobotics.ftclib.command.ConditionalCommand
 import com.arcrobotics.ftclib.command.RunCommand
 import com.arcrobotics.ftclib.gamepad.GamepadEx
 import com.arcrobotics.ftclib.gamepad.GamepadKeys
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp
-import org.firstinspires.ftc.teamcode.commands.arm.OpenArmCommand
+import com.qualcomm.robotcore.hardware.Servo
+import org.firstinspires.ftc.teamcode.commands.arm.ArmCommand
 import org.firstinspires.ftc.teamcode.commands.drive.DriveCommand
-import org.firstinspires.ftc.teamcode.commands.elevator.SpinDownCommand
-import org.firstinspires.ftc.teamcode.commands.elevator.SpinUpCommand
+import org.firstinspires.ftc.teamcode.commands.elevator.ElevatorCommand
+import org.firstinspires.ftc.teamcode.commands.intake.IntakeBeltCommand
 import org.firstinspires.ftc.teamcode.commands.intake.IntakeCommand
+import org.firstinspires.ftc.teamcode.constants.ControlBoard
 import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 
 @TeleOp
 class MainTeleOp : CommandOpMode() {
-    private lateinit var spinUpCommand: SpinUpCommand
-    private lateinit var spinDownCommand: SpinDownCommand
-    private lateinit var armUpCommand: OpenArmCommand
-    private lateinit var armDownCommand: OpenArmCommand
+    private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem
+
+    private lateinit var spinUpCommand: ElevatorCommand
+    private lateinit var spinDownCommand: ElevatorCommand
+
+    private lateinit var armUpCommand: ArmCommand
+    private lateinit var armDownCommand: ArmCommand
+
     private lateinit var driveCommand: DriveCommand
     private lateinit var intakeCommand: IntakeCommand
     private lateinit var outtakeCommand: IntakeCommand
+    private lateinit var intakeBeltCommand: IntakeBeltCommand
+    private lateinit var outtakeBeltCommand: IntakeBeltCommand
 
     private lateinit var driver: GamepadEx
     private lateinit var operator: GamepadEx
@@ -37,20 +47,51 @@ class MainTeleOp : CommandOpMode() {
         DriveSubsystem.initialize(hardwareMap)
         IntakeSubsystem.initialize(hardwareMap)
 
-        spinUpCommand = SpinUpCommand(ElevatorSubsystem)
-        spinDownCommand = SpinDownCommand(ElevatorSubsystem)
-        armUpCommand = OpenArmCommand(ArmSubsystem, true)
-        armDownCommand = OpenArmCommand(ArmSubsystem, false)
-//        intakeCommand = IntakeCommand(true, intakeSubsystem)
-//        outtakeCommand = IntakeCommand(false, intakeSubsystem)
+        intakeBeltSubsystem = IntakeBeltSubsystem(
+            hardwareMap[Servo::class.java, ControlBoard.INTAKE_BELT.deviceName]
+        )
+
+        spinUpCommand = ElevatorCommand(30.0, ElevatorSubsystem)
+        spinDownCommand = ElevatorCommand(0.0, ElevatorSubsystem)
+
+        armUpCommand = ArmCommand(Math.toRadians(87.5), ArmSubsystem)
+        armDownCommand = ArmCommand(Math.toRadians(0.0), ArmSubsystem)
+
+        intakeCommand = IntakeCommand(true, IntakeSubsystem)
+        outtakeCommand = IntakeCommand(false, IntakeSubsystem)
+
+        intakeBeltCommand = IntakeBeltCommand(true, intakeBeltSubsystem)
+        outtakeBeltCommand = IntakeBeltCommand(false, intakeBeltSubsystem)
+
         driveCommand = DriveCommand(DriveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0)
 
-        operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whileHeld(spinUpCommand)
-        operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whileHeld(spinDownCommand)
-        operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armUpCommand)
-        operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armDownCommand)
-        //operator.getGamepadButton(GamepadKeys.Button.Y).whenHeld(intakeCommand)
-       // operator.getGamepadButton(GamepadKeys.Button.A).whileHeld(outtakeCommand)
+        operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenPressed(
+            spinUpCommand.withTimeout(1500)
+        )
+
+        operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenPressed(
+            spinDownCommand.withTimeout(1500)
+        )
+
+        operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whenPressed(armUpCommand)
+
+        operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whenPressed(armDownCommand)
+
+        operator.getGamepadButton(GamepadKeys.Button.Y).whenPressed(
+            ConditionalCommand(
+                intakeCommand,
+                outtakeCommand,
+                IntakeSubsystem::intakePos
+            )
+        )
+
+        operator.getGamepadButton(GamepadKeys.Button.A).whenPressed(
+            ConditionalCommand(
+                intakeBeltCommand,
+                outtakeBeltCommand,
+                intakeBeltSubsystem::beltPos
+            )
+        )
 
         DriveSubsystem.defaultCommand = driveCommand
 

diff --git a/...e/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt b/...e/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt
@@ -9,12 +9,12 @@ class IntakeBeltSubsystem(
     var beltPos = false
 
     fun outtakePos() {
-        intakeBelt.position = 0.75
+        intakeBelt.position = 0.4
         beltPos = !beltPos
     }
 
     fun intakePos() {
-        intakeBelt.position = 0.47
+        intakeBelt.position = 0.0
         beltPos = !beltPos
     }
 

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt
@@ -15,12 +15,12 @@ object IntakeSubsystem : SubsystemBase() {
     var intakePos = false
 
     fun intake() {
-        intake.position = 1.0
+        intake.position = 0.8
         intakePos = !intakePos
     }
 
     fun outtake() {
-        intake.position = 0.6
+        intake.position = 0.2
         intakePos = !intakePos
     }