arav preset

ftc13100 · Dec 26, 2024 · 322ab39 · 322ab39
1 parent 27e15d5
commit 322ab39
Show file tree

Hide file tree

Showing 3 changed files with 48 additions and 62 deletions.
diff --git a/...mcode/commands/intake/ThrowbackCommand.kt → ...ode/commands/intake/ThrowItBackCommand.kt b/...mcode/commands/intake/ThrowbackCommand.kt → ...ode/commands/intake/ThrowItBackCommand.kt
@@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.commands.intake
 import com.arcrobotics.ftclib.command.CommandBase
 import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
 
-class ThrowbackCommand(
+class ThrowItBackCommand(
     private val subsystem: IntakeSubsystem
 ) : CommandBase() {
     override fun execute() = subsystem.wristUp()

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
@@ -4,8 +4,10 @@ import com.arcrobotics.ftclib.command.Command
 import com.arcrobotics.ftclib.command.CommandOpMode
 import com.arcrobotics.ftclib.command.ConditionalCommand
 import com.arcrobotics.ftclib.command.InstantCommand
+import com.arcrobotics.ftclib.command.ParallelCommandGroup
 import com.arcrobotics.ftclib.command.RunCommand
 import com.arcrobotics.ftclib.command.SelectCommand
+import com.arcrobotics.ftclib.command.SequentialCommandGroup
 import com.arcrobotics.ftclib.command.button.Trigger
 import com.arcrobotics.ftclib.gamepad.GamepadEx
 import com.arcrobotics.ftclib.gamepad.GamepadKeys
@@ -20,7 +22,7 @@ import org.firstinspires.ftc.teamcode.commands.elevator.SpinUpCommand
 import org.firstinspires.ftc.teamcode.commands.intake.IntakeBeltCommand
 import org.firstinspires.ftc.teamcode.commands.intake.IntakeCommand
 import org.firstinspires.ftc.teamcode.commands.intake.SlowIntakeBeltCommand
-import org.firstinspires.ftc.teamcode.commands.intake.ThrowbackCommand
+import org.firstinspires.ftc.teamcode.commands.intake.ThrowItBackCommand
 import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
@@ -42,6 +44,10 @@ class MainTeleOp : CommandOpMode() {
     private lateinit var slowIntakeBeltCommand: Command
     private lateinit var slowOuttakeBeltCommand: Command
 
+    private lateinit var operatorLeft: Trigger
+    private lateinit var operatorRight: Trigger
+
+
 
     private lateinit var driver: GamepadEx
     private var driverMode = DRIVER_MODE.SPEED
@@ -107,7 +113,7 @@ class MainTeleOp : CommandOpMode() {
 
         intakeBeltCommand = IntakeBeltCommand(Math.toRadians(-60.0), IntakeSubsystem)
 //        outtakeBeltCommand = IntakeBeltCommand(Math.toRadians(90.0), IntakeSubsystem)
-        outtakeBeltCommand = ThrowbackCommand(IntakeSubsystem)
+        outtakeBeltCommand = ThrowItBackCommand(IntakeSubsystem)
         slowIntakeBeltCommand = SlowIntakeBeltCommand(IntakeSubsystem, true)
         slowOuttakeBeltCommand = SlowIntakeBeltCommand(IntakeSubsystem, false)
 
@@ -130,33 +136,6 @@ class MainTeleOp : CommandOpMode() {
             operatorMode != OPERATOR_MODE.MANUAL
         }) .whenActive(spinDownCommand)
 
-/*//        operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenHeld(
-//            InstantCommand({
-//                if(operatorMode == OPERATOR_MODE.MANUAL)
-//                    schedule(spinUpCommand)
-//            })
-//        )
-//
-//        operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whenPressed(
-//            InstantCommand({
-//                if(operatorMode != OPERATOR_MODE.MANUAL)
-//                    schedule(spinUpCommand)
-//            })
-//        )*/
-//
-/*//        operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenHeld(
-//            InstantCommand({
-//                if(operatorMode == OPERATOR_MODE.MANUAL)
-//                    schedule(spinDownCommand)
-//            })
-//        )
-//
-//        operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenPressed(
-//            InstantCommand({
-//                if(operatorMode != OPERATOR_MODE.MANUAL)
-//                    schedule(spinDownCommand)
-//            })
-//        )*/
 
         (operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER) and Trigger {
             operatorMode == OPERATOR_MODE.MANUAL
@@ -174,33 +153,6 @@ class MainTeleOp : CommandOpMode() {
             operatorMode != OPERATOR_MODE.MANUAL
         }) .whenActive(armDownCommand)
 
-/*//        operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whenHeld(
-//            InstantCommand({
-//                if(operatorMode == OPERATOR_MODE.MANUAL)
-//                    schedule(armUpCommand)
-//            })
-//        )
-//
-//        operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whenPressed(
-//            InstantCommand({
-//                if(operatorMode != OPERATOR_MODE.MANUAL)
-//                    schedule(armUpCommand)
-//            })
-//        )*/
-/*//
-//        operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whenHeld(
-//            InstantCommand({
-//                if(operatorMode == OPERATOR_MODE.MANUAL)
-//                    schedule(armDownCommand)
-//            })
-//        )
-//
-//        operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whenPressed(
-//            InstantCommand({
-//                if(operatorMode != OPERATOR_MODE.MANUAL)
-//                    schedule(armDownCommand)
-//            })
-//        )*/
 
         operator.getGamepadButton(GamepadKeys.Button.Y).toggleWhenPressed(
             intakeCommand,
@@ -212,16 +164,50 @@ class MainTeleOp : CommandOpMode() {
             outtakeBeltCommand,
         )
 
-        driver.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(
+        operatorLeft = Trigger {
+            operator.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) == 1.0
+        }
+
+
+        operatorRight = Trigger {
+            operator.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) == 1.0
+        }
+
+        operatorRight.whileActiveContinuous(
             slowOuttakeBeltCommand
-//            intakeBeltCommand
         )
 
-        driver.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(
+        operatorLeft.whileActiveContinuous(
             slowIntakeBeltCommand
-//            outtakeBeltCommand
         )
 
+        (operator.getGamepadButton(GamepadKeys.Button.DPAD_LEFT) and Trigger {
+            operatorMode != OPERATOR_MODE.MANUAL
+        }) .whenActive(
+            SequentialCommandGroup(
+                ParallelCommandGroup(
+                    ThrowItBackCommand(IntakeSubsystem),
+                    ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000)
+                ),
+                ArmCommand(Math.toRadians(89.0), ArmSubsystem).withTimeout(2000)
+            )
+        )
+
+        (operator.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT) and Trigger {
+            operatorMode != OPERATOR_MODE.MANUAL
+        }) .whenActive(
+            SequentialCommandGroup(
+                ElevatorCommand(0.0, ElevatorSubsystem).withTimeout(1000),
+                ArmCommand(0.0, ArmSubsystem).withTimeout(2000),
+                ParallelCommandGroup(
+                    IntakeBeltCommand(-60.0, IntakeSubsystem).withTimeout(500),
+                    ElevatorCommand(17.0, ElevatorSubsystem)
+                ),
+
+            )
+        )
+
+
         driver.getGamepadButton(GamepadKeys.Button.B).whenPressed(
             InstantCommand({ operatorMode = operatorMode.toggle(operatorMode)})
         )

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
@@ -96,7 +96,7 @@ object IntakeSubsystem : SubsystemBase() {
 
     fun wristDown() = wrist.set(-1.0)
 
-    fun wristDownSlow() = if (!isPressed) wrist.set(-0.2) else wrist.stop()
+    fun wristDownSlow() = wrist.set(-0.2)
Original file line number	Diff line number	Diff line change
Expand Up		@@ -96,7 +96,7 @@ object IntakeSubsystem : SubsystemBase() {

		fun wristDown() = wrist.set(-1.0)

		fun wristDownSlow() = if (!isPressed) wrist.set(-0.2) else wrist.stop()
		fun wristDownSlow() = wrist.set(-0.2)



Expand Down