From b5c6e03ef3002487167cdc0284e3851804f7cd8b Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Wed, 30 Oct 2024 20:36:38 -0700 Subject: [PATCH] Implemented Arm Subsytem wholeheartedly --- .../firstinspires/ftc/teamcode/ArmTest.java | 30 +++-------- .../ftc/teamcode/configs/RobotConstants.java | 5 +- .../ftc/teamcode/subsystem/ArmSubsystem.java | 53 +++++++++++-------- 3 files changed, 42 insertions(+), 46 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java index f5dd5b9..066e75a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java @@ -53,6 +53,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; +import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; /* @@ -95,9 +96,7 @@ public class ArmTest extends LinearOpMode { /* * Instantiate Arm */ - ServoImplEx arm = hardwareMap.get(ServoImplEx.class, "arm-servo"); - arm.resetDeviceConfigurationForOpMode(); - arm.setPosition(.15); + ArmSubsystem arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK); /* * Instantiate gamepad state holders @@ -105,6 +104,7 @@ public class ArmTest extends LinearOpMode { Gamepad currentGamepad1 = new Gamepad(); Gamepad previousGamepad1 = new Gamepad(); + arm.init(); waitForStart(); runtime.reset(); @@ -115,34 +115,20 @@ public class ArmTest extends LinearOpMode { currentGamepad1.copy(gamepad1); if (currentGamepad1.circle && !previousGamepad1.circle) { - arm.setPosition(0.3); + arm.parkArm(); } if (currentGamepad1.square && !previousGamepad1.square) { - arm.setPosition(.45); + arm.engageArm(); } - if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) { - double newPosition = arm.getPosition() - .05; - if (newPosition < 0.15) { - arm.setPosition(0.15); - } else { - arm.setPosition(newPosition); - } - } - - if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { - double newPosition = arm.getPosition() + .05; - if (newPosition > .6) { - arm.setPosition(.6); - } else { - arm.setPosition(newPosition); - } + if (currentGamepad1.cross && !previousGamepad1.cross) { + arm.switchState(); } // Show the elapsed game time and wheel power. telemetry.addData("Status", "Run Time: " + runtime.toString()); - telemetry.addData("Arm Servo Position", arm.getPosition()); + telemetry.addData("Arm State", arm.getState()); telemetry.update(); } }} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java index b5931a9..8fd546d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java @@ -7,9 +7,8 @@ public class RobotConstants { public static double clawClose = 0.95; public static double clawOpen = 0.5; - public static double wristEngage = 0.95; - public static double wristHold = 0.50; - public static double wristDisengage = 0.05; + public static double armEngage = 0.5; + public static double armPark = 0.125; public static double intakeSpinInPwr = 1; public static double intakeSpinOutPwr = -0.25; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java index 90be386..46bd677 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java @@ -1,42 +1,41 @@ package org.firstinspires.ftc.teamcode.subsystem; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristDisengage; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristEngage; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHold; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armEngage; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark; import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.ServoImplEx; +import org.firstinspires.ftc.teamcode.util.action.Actions; import org.firstinspires.ftc.teamcode.util.action.RunAction; public class ArmSubsystem { public enum ArmState { - DISENGAGE, ENGAGE + PARK, ENGAGE } - public ServoImplEx wrist; + public ServoImplEx arm; public ArmState state; - public RunAction engageWrist, disengageWrist; + public RunAction engageArm, parkArm; public ArmSubsystem(HardwareMap hardwareMap, ArmState armState) { - wrist = hardwareMap.get(ServoImplEx.class, "arm-servo"); - wrist.resetDeviceConfigurationForOpMode(); + arm = hardwareMap.get(ServoImplEx.class, "arm-servo"); + arm.resetDeviceConfigurationForOpMode(); this.state = armState; - engageWrist = new RunAction(this::engageArm); - disengageWrist = new RunAction(this::disengageArm); + parkArm = new RunAction(this::parkArm); + engageArm = new RunAction(this::engageArm); } - public void setState(ArmState wristState) { - if (wristState == ArmState.ENGAGE) { - wrist.setPosition(wristEngage); + public void setState(ArmState armState) { + if (armState == ArmState.ENGAGE) { + arm.setPosition(armEngage); this.state = ArmState.ENGAGE; - } else if (wristState == ArmState.DISENGAGE) { - wrist.setPosition(wristDisengage); - this.state = ArmState.DISENGAGE; + } else if (armState == ArmState.PARK) { + arm.setPosition(armPark); + this.state = ArmState.PARK; } } @@ -44,16 +43,28 @@ public class ArmSubsystem { setState(ArmState.ENGAGE); } - public void disengageArm() { - setState(ArmState.DISENGAGE); + public void parkArm() { + setState(ArmState.PARK); } public void switchState() { if (state == ArmState.ENGAGE) { - setState(ArmState.DISENGAGE); - } else if (state == ArmState.DISENGAGE) { + setState(ArmState.PARK); + } else if (state == ArmState.PARK) { setState(ArmState.ENGAGE); } } + public ArmState getState() { + return this.state; + } + + public void init() { + Actions.runBlocking(parkArm); + } + + public void start() { + Actions.runBlocking(parkArm); + } + }