0.5 mode

ftc13100 · Dec 22, 2024 · 99de4f0 · 99de4f0
1 parent 2a1f6b6
commit 99de4f0
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 11 deletions.
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt
@@ -10,16 +10,17 @@ class DriveCommand(
     private val leftY: () -> Double,
     private val rightX: () -> Double,
     private val zoneVal: Double,
+    private val multipier: Double = 0.9,
 ) : CommandBase() {
     init {
         addRequirements(subsystem)
     }
 
     override fun execute() {
         subsystem.drive(
-            leftY = zonedDrive(leftY.invoke() * 0.9, zoneVal).pow(3),
-            leftX = zonedDrive(leftX.invoke() * 0.9, zoneVal).pow(3),
-            rightX = zonedDrive(rightX.invoke() * 0.9, zoneVal).pow(3),
+            leftY = zonedDrive(leftY.invoke() * multipier, zoneVal).pow(3),
+            leftX = zonedDrive(leftX.invoke() * multipier, zoneVal).pow(3),
+            rightX = zonedDrive(rightX.invoke() * multipier, zoneVal).pow(3),
         )
     }
 

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
@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.opModes.teleOp
 
 import com.arcrobotics.ftclib.command.CommandOpMode
 import com.arcrobotics.ftclib.command.ConditionalCommand
+import com.arcrobotics.ftclib.command.InstantCommand
 import com.arcrobotics.ftclib.command.RunCommand
 import com.arcrobotics.ftclib.gamepad.GamepadEx
 import com.arcrobotics.ftclib.gamepad.GamepadKeys
@@ -77,20 +78,45 @@ class MainTeleOp : CommandOpMode() {
 
         operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whenPressed(armDownCommand)
 
-        operator.getGamepadButton(GamepadKeys.Button.Y).whenPressed(
-            ConditionalCommand(
+        operator.getGamepadButton(GamepadKeys.Button.Y).toggleWhenPressed(
+
                 intakeCommand,
                 outtakeCommand,
-                IntakeSubsystem::intakePos
-            )
+
         )
 
-        operator.getGamepadButton(GamepadKeys.Button.A).whenPressed(
-            ConditionalCommand(
+        operator.getGamepadButton(GamepadKeys.Button.A).toggleWhenPressed(
+
                 intakeBeltCommand,
                 outtakeBeltCommand,
-                intakeBeltSubsystem::beltPos
-            )
+
+        )
+
+        driver.getGamepadButton(GamepadKeys.Button.A).toggleWhenPressed(
+            InstantCommand(
+                {
+                    driveCommand = DriveCommand(
+                        DriveSubsystem,
+                        driver::getLeftX,
+                        driver::getLeftY,
+                        driver::getRightX,
+                        0.0,
+                        0.5
+                    )
+                }
+            ),
+            InstantCommand(
+                {
+                    driveCommand = DriveCommand(
+                        DriveSubsystem,
+                        driver::getLeftX,
+                        driver::getLeftY,
+                        driver::getRightX,
+                        0.0,
+                        0.9
+                    )
+                }
+            ),
         )
 
         DriveSubsystem.defaultCommand = driveCommand