New arm command

ftc13100 · Nov 30, 2024 · 50f537f · 50f537f
1 parent ce13986
commit 50f537f
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 20 deletions.
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
@@ -14,7 +14,7 @@ import org.firstinspires.ftc.teamcode.commands.arm.OpenArmCommand
 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.intake.IntakeCommand
+import org.firstinspires.ftc.teamcode.commands.intake.IntakeBeltCommand
 import org.firstinspires.ftc.teamcode.constants.ControlBoard
 import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
@@ -47,10 +47,10 @@ class MainTeleOp : CommandOpMode() {
 
     private lateinit var driveCommand: DriveCommand
 
-    private lateinit var intakeCommand: IntakeCommand
-    private lateinit var outtakeCommand: IntakeCommand
+    private lateinit var intakeCommand: IntakeBeltCommand
+    private lateinit var outtakeCommand: IntakeBeltCommand
 
-    private lateinit var intakeBeltCommand: ConditionalCommand
+    private lateinit var clawCommand: ConditionalCommand
 
     private lateinit var driver: GamepadEx
     private lateinit var operator: GamepadEx
@@ -83,13 +83,13 @@ class MainTeleOp : CommandOpMode() {
         armUpCommand = OpenArmCommand(armSubsystem, true)
         armDownCommand = OpenArmCommand(armSubsystem, false)
 
-        intakeCommand = IntakeCommand(true, intakeSubsystem)
-        outtakeCommand = IntakeCommand(false, intakeSubsystem)
+        intakeCommand = IntakeBeltCommand(true, intakeBeltSubsystem)
+        outtakeCommand = IntakeBeltCommand(false, intakeBeltSubsystem)
 
-        intakeBeltCommand = ConditionalCommand(
-            InstantCommand({ intakeBeltSubsystem.intakePos() }),
-            InstantCommand({ intakeBeltSubsystem.outtakePos() }),
-            intakeBeltSubsystem::intakePos
+        clawCommand = ConditionalCommand(
+            InstantCommand({ intakeSubsystem.intake() }),
+            InstantCommand({ intakeSubsystem.outtake() }),
+            intakeSubsystem::intakePos
         )
 
         driveCommand = DriveCommand(driveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0)
@@ -99,7 +99,7 @@ class MainTeleOp : CommandOpMode() {
         operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armUpCommand)
         operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armDownCommand)
 
-        operator.getGamepadButton(GamepadKeys.Button.B).whenPressed(intakeBeltCommand)
+        operator.getGamepadButton(GamepadKeys.Button.B).whenPressed(clawCommand)
 
         operatorLeft = Trigger {
             operator.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) != 0.0
@@ -116,7 +116,7 @@ class MainTeleOp : CommandOpMode() {
 
         RunCommand({
             telemetry.addData("Arm Position: ", Math.toDegrees(armSubsystem.armAngle))
-            telemetry.addData("Intake Position", intakeBeltSubsystem.intakePos)
+            telemetry.addData("Intake Position", intakeSubsystem.intakePos)
             telemetry.update()
         }).perpetually().schedule()
     }

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
@@ -6,19 +6,14 @@ import com.qualcomm.robotcore.hardware.Servo
 class IntakeBeltSubsystem(
     private val intakeBelt: Servo
 ) : SubsystemBase() {
-    var intakePos = false
-
     fun outtakePos() {
-        intakeBelt.position = 0.25
-        intakePos = true
+        intakeBelt.position += 0.01
     }
 
     fun intakePos() {
-        intakeBelt.position = 0.02
-        intakePos = false
+        intakeBelt.position -= 0.01
     }
 
-
     fun setPos(pos: Double) {
         intakeBelt.position = pos
     }

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
@@ -6,12 +6,16 @@ import com.qualcomm.robotcore.hardware.Servo
 class IntakeSubsystem(
     private val intake: Servo
 ) : SubsystemBase() {
+    var intakePos = false
+
     fun intake() {
-        intake.position = 1.0
+        intake.position = 0.6
+        intakePos = true
     }
 
     fun outtake() {
         intake.position = 0.0
+        intakePos = false
     }
 
     fun setSpeed(speed: Double) { intake.position = speed }