From 2a1f6b68adf7a016fed61c7b4009b372eefc408a Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Sat, 21 Dec 2024 11:28:15 -0500 Subject: [PATCH] Setpoints for TeleOp --- .../teamcode/commands/intake/IntakeCommand.kt | 2 +- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 79 ++++++++++++++----- .../subsystems/intake/IntakeBeltSubsystem.kt | 4 +- .../subsystems/intake/IntakeSubsystem.kt | 4 +- 4 files changed, 65 insertions(+), 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 index bd9429c..9c3d548 100644 --- 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 index f17c8dd..1dc63c4 100644 --- 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/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt index 7964e8c..7561f3c 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt +++ b/TeamCode/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 index 5d387cd..fc3c583 100644 --- 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 }