From 284263a43b36071a87c81aa6f4c0e80abd326afe Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Wed, 30 Oct 2024 12:12:56 -0700 Subject: [PATCH] Working tests and reverting basic omni back to default --- .../firstinspires/ftc/teamcode/ArmTest.java | 148 ++++++++++++++++ .../ftc/teamcode/BasicOmniOpMode_Linear.java | 59 ------- .../firstinspires/ftc/teamcode/ClawTest.java | 125 ++++++++++++++ .../ftc/teamcode/CometDrive.java | 6 +- .../firstinspires/ftc/teamcode/LiftTest.java | 159 ++++++++++++++++++ .../firstinspires/ftc/teamcode/WristTest.java | 150 +++++++++++++++++ .../ftc/teamcode/runmodes/Teleop.java | 31 +--- 7 files changed, 594 insertions(+), 84 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClawTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WristTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java new file mode 100644 index 0000000..f5dd5b9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java @@ -0,0 +1,148 @@ +/* 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; + +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.Gamepad; +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.ClawSubsystem; + +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Arm Test", 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 + */ + ServoImplEx arm = hardwareMap.get(ServoImplEx.class, "arm-servo"); + arm.resetDeviceConfigurationForOpMode(); + arm.setPosition(.15); + + /* + * Instantiate gamepad state holders + */ + Gamepad currentGamepad1 = new Gamepad(); + Gamepad previousGamepad1 = new Gamepad(); + + 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.setPosition(0.3); + } + + if (currentGamepad1.square && !previousGamepad1.square) { + arm.setPosition(.45); + } + + 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); + } + } + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Arm Servo Position", arm.getPosition()); + telemetry.update(); + } + }} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java index abbd6e8..18aa94e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java @@ -49,15 +49,9 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.Gamepad; -import com.qualcomm.robotcore.hardware.PwmControl; -import com.qualcomm.robotcore.hardware.Servo; -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.ClawSubsystem; -import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; /* * This file contains an example of a Linear "OpMode". @@ -93,10 +87,6 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { // Declare OpMode members for each of the 4 motors. private final ElapsedTime runtime = new ElapsedTime(); - private ClawSubsystem.ClawState clawState; - -// private WristSubsystem.WristState wristState; - @Override public void runOpMode() { @@ -107,25 +97,7 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR); DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR); - /* - * Instantiate Claw - */ - ClawSubsystem claw = new ClawSubsystem(hardwareMap, clawState); - /* - * Instantiate Arm - */ - ServoImplEx arm = hardwareMap.get(ServoImplEx.class, "arm-servo"); - //arm.setPwmRange(PwmControl.PwmRange.defaultRange); - - /* - * Instantiate gamepad state holders - */ - Gamepad currentGamepad1 = new Gamepad(); - Gamepad previousGamepad1 = new Gamepad(); - - - arm.resetDeviceConfigurationForOpMode(); // TODO: replace these with your encoder ports Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER)); @@ -162,37 +134,8 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { waitForStart(); runtime.reset(); - claw.setState(ClawSubsystem.ClawState.CLOSED); - - // run until the end of the match (driver presses STOP) while (opModeIsActive()) { - - previousGamepad1.copy(currentGamepad1); - currentGamepad1.copy(gamepad1); -// -// if (gamepad1.cross) { -// claw.switchState(); -// } - - if (currentGamepad1.cross && !previousGamepad1.cross) { - // This will set intakeToggle to true if it was previously false - // and intakeToggle to false if it was previously true, - // providing a toggling behavior. - claw.switchState(); - } - - if (currentGamepad1.circle && !previousGamepad1.circle) { -// wrist.switchState(); - arm.setPosition(0); - } - - if (currentGamepad1.square && !previousGamepad1.square) { -// wrist.switchState(); - arm.setPosition(.5); - } - - double max; // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. @@ -250,8 +193,6 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition()); telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition()); telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition()); - telemetry.addData("Claw State", claw.getState()); - telemetry.addData("Wrist Servo Position", arm.getPosition()); telemetry.update(); } }} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClawTest.java new file mode 100644 index 0000000..8c1ae29 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClawTest.java @@ -0,0 +1,125 @@ +/* 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; + +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.Gamepad; +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.ClawSubsystem; + +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Claw Test", group="Debug") +public class ClawTest extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private final ElapsedTime runtime = new ElapsedTime(); + + @Override + public void runOpMode() { + + /* + * Instantiate Claw + */ + ClawSubsystem claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.OPEN); + + /* + * Instantiate gamepad state holders + */ + Gamepad currentGamepad1 = new Gamepad(); + Gamepad previousGamepad1 = new Gamepad(); + + waitForStart(); + runtime.reset(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + previousGamepad1.copy(currentGamepad1); + currentGamepad1.copy(gamepad1); + + if (currentGamepad1.cross && !previousGamepad1.cross) { + claw.switchState(); + } + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Claw State", claw.getState()); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometDrive.java index aeea4d1..fdd263d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometDrive.java @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode; import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.blueBucketStartPose; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -9,6 +10,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.runmodes.Teleop; @TeleOp(name="Drive", group="A") +@Disabled public class CometDrive extends OpMode { private Teleop teleop; @@ -19,9 +21,7 @@ public class CometDrive extends OpMode { telemetry, new Follower(hardwareMap), blueBucketStartPose, - false, - gamepad1, - gamepad2); + gamepad1); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java new file mode 100644 index 0000000..bd188db --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java @@ -0,0 +1,159 @@ +/* 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; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.util.ElapsedTime; + +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name = "Lift Test", group = "Debug") +public class LiftTest extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private final ElapsedTime runtime = new ElapsedTime(); + + private final int MIN_POINT = 0; + private final int MAX_POINT = 6500; + + @Override + public void runOpMode() { + + /* + * Instantiate Lift + */ + DcMotor liftDrive = hardwareMap.get(DcMotor.class, "lift-motor"); + liftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + liftDrive.setDirection(DcMotorSimple.Direction.REVERSE); + + /* + * Instantiate gamepad state holders + */ + Gamepad currentGamepad1 = new Gamepad(); + Gamepad previousGamepad1 = new Gamepad(); + + waitForStart(); + runtime.reset(); + + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + previousGamepad1.copy(currentGamepad1); + currentGamepad1.copy(gamepad1); + + liftDrive.setPower(.5); + + // Max position is 6800, safely setting to 6500 + + if (currentGamepad1.square && !previousGamepad1.square) { + liftDrive.setTargetPosition(MIN_POINT); + liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (currentGamepad1.triangle && !previousGamepad1.triangle) { + liftDrive.setTargetPosition(2000); + liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (currentGamepad1.circle && !previousGamepad1.circle) { + liftDrive.setTargetPosition(4000); + liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (currentGamepad1.cross && !previousGamepad1.cross) { + liftDrive.setTargetPosition(MAX_POINT); + liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) { + int newPosition = liftDrive.getCurrentPosition() - 250; + if (newPosition < MIN_POINT) { + liftDrive.setTargetPosition(MIN_POINT); + } else { + liftDrive.setTargetPosition(newPosition); + } + liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { + int newPosition = liftDrive.getCurrentPosition() + 250; + if (newPosition > MAX_POINT) { + liftDrive.setTargetPosition(MAX_POINT); + } else { + liftDrive.setTargetPosition(newPosition); + } + liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. +// double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + + // Send calculated power to wheels + + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Lift Drive Position", liftDrive.getCurrentPosition()); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WristTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WristTest.java new file mode 100644 index 0000000..b14fe9c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WristTest.java @@ -0,0 +1,150 @@ +/* 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; + +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; +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.ClawSubsystem; + +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Wrist Test", group="Debug") +public class WristTest extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private final ElapsedTime runtime = new ElapsedTime(); + + @Override + public void runOpMode() { + + /* + * Instantiate Wrist + */ + ServoImplEx wrist = hardwareMap.get(ServoImplEx.class, "wrist-servo"); +// wrist.resetDeviceConfigurationForOpMode(); + wrist.setDirection(Servo.Direction.FORWARD); + wrist.setPosition(0); + + /* + * Instantiate gamepad state holders + */ + Gamepad currentGamepad1 = new Gamepad(); + Gamepad previousGamepad1 = new Gamepad(); + + waitForStart(); + runtime.reset(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + previousGamepad1.copy(currentGamepad1); + currentGamepad1.copy(gamepad1); + + if (currentGamepad1.square && !previousGamepad1.square) { + wrist.setPosition(0.25); + } + + if (currentGamepad1.circle && !previousGamepad1.circle) { + wrist.setPosition(0.75); + } + + if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) { + double newPosition = wrist.getPosition() - .05; + if (newPosition < 0.1) { + wrist.setPosition(0.1); + } else { + wrist.setPosition(newPosition); + } + } + + if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { + double newPosition = wrist.getPosition() + .05; + if (newPosition > .9) { + wrist.setPosition(.9); + } else { + wrist.setPosition(newPosition); + } + } + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Wrist Servo Position", wrist.getPosition()); + telemetry.update(); + } + }} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java index 65111df..1124102 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java @@ -20,28 +20,24 @@ public class Teleop { private Telemetry telemetry; private Gamepad gamepad1; - private Gamepad gamepad2; - private Gamepad currentGamepad1 = new Gamepad(); - private Gamepad currentGamepad2 = new Gamepad(); - private Gamepad previousGamepad1 = new Gamepad(); - private Gamepad previousGamepad2 = new Gamepad(); + private Gamepad currentGamepad1; + private Gamepad previousGamepad1; public RunAction stopDrive, startDrive; - private boolean fieldCentric = true; + public double speed = 0.5; - public double speed = 0.75; - - public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Pose startPose, boolean fieldCentric, Gamepad gamepad1, Gamepad gamepad2) { + public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Pose startPose, Gamepad gamepad1) { claw = new ClawSubsystem(hardwareMap, clawState); this.follower = follower; this.startPose = startPose; - this.fieldCentric = fieldCentric; this.telemetry = telemetry; this.gamepad1 = gamepad1; - this.gamepad2 = gamepad2; + + this.currentGamepad1 = new Gamepad(); + this.previousGamepad1 = new Gamepad(); stopDrive = new RunAction(this::stopDrive); startDrive = new RunAction(this::startDrive); @@ -57,27 +53,18 @@ public class Teleop { follower.startTeleopDrive(); } - private void stopDrive(){ + private void stopDrive() { follower.breakFollowing(); } public void update() { previousGamepad1.copy(currentGamepad1); - previousGamepad2.copy(currentGamepad2); currentGamepad1.copy(gamepad1); - currentGamepad2.copy(gamepad2); - - if (gamepad1.right_bumper) - speed = 1; - else if (gamepad1.left_bumper) - speed = 0.25; - else - speed = 0.75; if (currentGamepad1.a && !previousGamepad1.a) claw.switchState(); - follower.setTeleOpMovementVectors(-gamepad1.left_stick_y * speed, -gamepad1.left_stick_x * speed, -gamepad1.right_stick_x * speed, !fieldCentric); + follower.setTeleOpMovementVectors(-gamepad1.left_stick_y * speed, -gamepad1.left_stick_x * speed, -gamepad1.right_stick_x * speed); follower.update(); telemetry.addData("X", follower.getPose().getX());