diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java new file mode 100644 index 0000000..8f9c131 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java @@ -0,0 +1,95 @@ +/* Copyright (c) 2021 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode.cometbots.tests; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; + +@TeleOp(name = "Arm Test v2", group = "Debug") +public class ArmTest extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private final ElapsedTime runtime = new ElapsedTime(); + + @Override + public void runOpMode() { + + /* + * Instantiate Arm + */ + ArmSubsystem arm = new ArmSubsystem(hardwareMap); + + /* + * Instantiate gamepad state holders + */ + Gamepad currentGamepad1 = new Gamepad(); + Gamepad previousGamepad1 = new Gamepad(); + + arm.init(); + waitForStart(); + runtime.reset(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + previousGamepad1.copy(currentGamepad1); + currentGamepad1.copy(gamepad1); + + if (currentGamepad1.circle && !previousGamepad1.circle) { + arm.toParkPosition(); + } + + if (currentGamepad1.square && !previousGamepad1.square) { + arm.toBucketPosition(); + } + + if (currentGamepad1.cross && !previousGamepad1.cross) { + arm.toFloorPosition(); + } + + if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) { + arm.setPosition(arm.getPosition() - .05); + } + + if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { + arm.setPosition(arm.getPosition() + .05); + } + + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Arm State", arm.getState()); + telemetry.addData("Arm Position", arm.getPosition()); + 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 9b795a0..d374d20 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,10 +7,10 @@ public class RobotConstants { public final static double clawClose = 0.85; public final static double clawOpen = 0.05; - public final static double armFloor = 0.7; + public final static double armFloor = 0.6375; public final static double armSubmarine = 0.55; - public final static double armPark = 0.0; - public final static double armBucket = 0.2; + public final static double armPark = 0.1250; + public final static double armBucket = 0.25; public final static double wristPickup = 0.6; public final static double wristBucket = 0.3; 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 new file mode 100644 index 0000000..5f5cdb9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java @@ -0,0 +1,60 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import static org.firstinspires.ftc.teamcode.PedroConstants.ARM_SERVO; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armFloor; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.ServoImplEx; + +public class ArmSubsystem { + + public enum ArmState { + PARK, FLOOR, BUCKET, SUBMARINE + } + + private ServoImplEx arm; + private ArmState state; + + public ArmSubsystem(HardwareMap hardwareMap) { + this.arm = hardwareMap.get(ServoImplEx.class, ARM_SERVO); + } + + public void toParkPosition() { + arm.setPosition(armPark); + setState(ArmState.PARK); + } + + public void toFloorPosition() { + arm.setPosition(armFloor); + setState(ArmState.FLOOR); + } + + public void toBucketPosition() { + arm.setPosition(armBucket); + setState(ArmState.BUCKET); + } + + public void setState(ArmState armState) { + this.state = armState; + } + + public ArmState getState() { + return this.state; + } + + public void init() { + arm.resetDeviceConfigurationForOpMode(); + toParkPosition(); + } + + public double getPosition() { + return this.arm.getPosition(); + } + + public void setPosition(double position) { + this.arm.setPosition(position); + } + +}