From aecb6122b35c19449f6a57825e430c0021ba3c85 Mon Sep 17 00:00:00 2001 From: robotics2 Date: Sat, 2 Dec 2023 09:48:51 -0800 Subject: [PATCH] red backstage works consistantly for 45 --- .../ftc/teamcode/BlueBackStage.java | 6 +- .../ftc/teamcode/RedBackStage.java | 20 +- .../firstinspires/ftc/teamcode/RedDirect.java | 460 ++++++++++++++++++ 3 files changed, 479 insertions(+), 7 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedDirect.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBackStage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBackStage.java index 1ae4362..700182e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBackStage.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBackStage.java @@ -255,7 +255,7 @@ public class BlueBackStage extends LinearOpMode { raisearm(80); wrist.setPosition(0); raisearm(100); - gripper.setPosition(0); + gripper.setPosition(0.25); sleep(500); driveForward(5); terminateOpModeNow(); @@ -281,7 +281,7 @@ public class BlueBackStage extends LinearOpMode { raisearm(80); wrist.setPosition(0); raisearm(100); - gripper.setPosition(0); + gripper.setPosition(0.25); sleep(500); driveForward(5); terminateOpModeNow(); @@ -305,7 +305,7 @@ public class BlueBackStage extends LinearOpMode { raisearm(80); wrist.setPosition(0); raisearm(100); - gripper.setPosition(0); + gripper.setPosition(0.25); sleep(500); driveForward(5); terminateOpModeNow(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStage.java index 4132d09..040530d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStage.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStage.java @@ -261,13 +261,17 @@ public class RedBackStage extends LinearOpMode { straightLeft(32); driveForward(-10); straightRight(33); - driveForward(-1.5); + driveForward(-7.5); + telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH)); raisearm(80); wrist.setPosition(0); raisearm(100); - gripper.setPosition(0); + gripper.setPosition(0.25); sleep(500); driveForward(5); + telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH)); + telemetry.update(); + sleep(1000); terminateOpModeNow(); @@ -287,12 +291,16 @@ public class RedBackStage extends LinearOpMode { driveForward(-20.5); straightRight(19); driveForward(-1.5); + telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH)); raisearm(80); wrist.setPosition(0); raisearm(100); - gripper.setPosition(0); + gripper.setPosition(0.25); sleep(500); driveForward(5); + telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH)); + telemetry.update(); + sleep(1000); terminateOpModeNow(); @@ -311,12 +319,16 @@ else driveForward(-18); straightRight(29); driveForward(-1.5); + telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH)); raisearm(80); wrist.setPosition(0); raisearm(100); - gripper.setPosition(0); + gripper.setPosition(0.25); sleep(500); driveForward(5); + telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH)); + telemetry.update(); + sleep(1000); terminateOpModeNow(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedDirect.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedDirect.java new file mode 100644 index 0000000..e82ae3b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedDirect.java @@ -0,0 +1,460 @@ +/* Copyright (c) 2017 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 android.annotation.SuppressLint; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DistanceSensor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/** + * This file illustrates the concept of driving a path based on encoder counts. + * The code is structured as a LinearOpMode + * + * The code REQUIRES that you DO have encoders on the wheels, + * otherwise you would use: RobotAutoDriveByTime; + * + * This code ALSO requires that the drive Motors have been configured such that a positive + * power command moves them forward, and causes the encoders to count UP. + * + * The desired path in this example is: + * - Drive forward for 48 inches + * - Spin right for 12 Inches + * - Drive Backward for 24 inches + * - Stop and close the claw. + * + * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) + * that performs the actual movement. + * This method assumes that each movement is relative to the last stopping place. + * There are other ways to perform encoder based moves, but this method is probably the simplest. + * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile + * + * 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 + */ + +@Autonomous(name="red (direct)", group="Robot") +//@Disabled +public class RedDirect extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + private DcMotor backrightDrive = null; + private DcMotor backleftDrive = null; + private DistanceSensor distanceRight = null; + private DistanceSensor distanceLeft = null; + private Servo wrist = null; + private Servo gripper = null; + private DcMotor arm = null; + private DistanceSensor distance = null; + + + private ElapsedTime runtime = new ElapsedTime(); + + // Calculate the COUNTS_PER_INCH for your specific drive train. + // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV + // For external drive gearing, set DRIVE_GEAR_REDUCTION as needed. + // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear. + // This is gearing DOWN for less speed and more torque. + // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation. + static final double COUNTS_PER_MOTOR_REV = 537.6; // eg: TETRIX Motor Encoder + static final double DRIVE_GEAR_REDUCTION = 1.0; // No External Gearing. + static final double WHEEL_DIAMETER_INCHES = 3.77953; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * Math.PI); + static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI); + static final double DRIVE_SPEED = 0.3; + static final double TURN_SPEED = 0.4; + + static final double LONG_TIMEOUT = 1000; + static final double DEGREE_TOO_DISTANCE = 0.21944444444; + static final double ARM_SPEED = .1; + static final double TICKS_TO_DEGREES = 0.07462686567; + + @Override + public void runOpMode() + { + hardwareinit(); + + // Send telemetry message to indicate successful Encoder reset + /* telemetry.addData("Starting at", "%7d :%7d", + leftDrive.getCurrentPosition(), + rightDrive.getCurrentPosition(), + backleftDrive.getCurrentPosition(), + backrightDrive.getCurrentPosition());*/ + + telemetry.update(); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + { + executeAuto(); + + } + + // Step through each leg of the path, + // Note: Reverse movement is obtained by setting a negative distance (not speed) + + + } + + +// + + public void driveForward(double distance) + { + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backrightDrive.setDirection(DcMotor.Direction.REVERSE); + backleftDrive.setDirection(DcMotor.Direction.REVERSE); + encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); // S1: Forward 47 Inches with 5 Sec timeout + } + + public void straightLeft(double distance) + { + leftDrive.setDirection(DcMotor.Direction.FORWARD); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backrightDrive.setDirection(DcMotor.Direction.FORWARD); + backleftDrive.setDirection(DcMotor.Direction.REVERSE); + encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); + } + + public void straightRight(double distance) + { + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.REVERSE); + backrightDrive.setDirection(DcMotor.Direction.REVERSE); + backleftDrive.setDirection(DcMotor.Direction.FORWARD); + encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); + } + + public void turnLeft(double degrees) + { + leftDrive.setDirection(DcMotor.Direction.FORWARD); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backrightDrive.setDirection(DcMotor.Direction.REVERSE); + backleftDrive.setDirection(DcMotor.Direction.FORWARD); + double turning_distance = degrees * DEGREE_TOO_DISTANCE; + encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT); + } + + public void turnRight(double degrees) { + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.REVERSE); + backrightDrive.setDirection(DcMotor.Direction.FORWARD); + backleftDrive.setDirection(DcMotor.Direction.REVERSE); + double turning_distance = degrees * DEGREE_TOO_DISTANCE; + encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT); + } + + + public void raisearm(int degrees) { + armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT); + + } + public void hardwareinit() + { + leftDrive = hardwareMap.get(DcMotor.class, "Drive front lt"); + rightDrive = hardwareMap.get(DcMotor.class, "Drive front rt"); + backleftDrive = hardwareMap.get(DcMotor.class, "Drive back lt"); + backrightDrive = hardwareMap.get(DcMotor.class, "Drive back rt"); + distanceRight = hardwareMap.get(DistanceSensor.class, "color right"); + distanceLeft = hardwareMap.get(DistanceSensor.class, "color left"); + gripper = hardwareMap.get(Servo.class, "gripper"); + arm = hardwareMap.get(DcMotor.class, "arm raise"); + wrist = hardwareMap.get(Servo.class, "wrist"); + distance = hardwareMap.get(DistanceSensor.class, "distance"); + wrist.setPosition(1); + sleep(1000); + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backrightDrive.setDirection(DcMotor.Direction.REVERSE); + backleftDrive.setDirection(DcMotor.Direction.REVERSE); + arm.setDirection(DcMotor.Direction.REVERSE); + + leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + backleftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + backrightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + + leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + public void testWrist() + { + wrist.setPosition(0); + sleep(3000); + wrist.setPosition(1); + sleep(3000); + } + public void testGripper() + { + gripper.setPosition(0.5); + + } + @SuppressLint("SuspiciousIndentation") + public void executeAuto() + { + + arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + driveForward(26); + sleep(500); + + int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH); + int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH); + telemetry.addData("color left sensor",distanceleft); + telemetry.addData("color right sensor",distanceright); + telemetry.update(); + + if (distanceleft < 7) + { + telemetry.addData("postion","left"); + telemetry.update(); + turnLeft(90); + straightLeft(2); + driveForward(6.5); + raisearm(80); + arm.setPower(0); + driveForward(-21); + straightLeft(32); + driveForward(-10); + straightRight(33); + driveForward(-1.5); + telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH)); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + gripper.setPosition(0.25); + sleep(500); + driveForward(5); + telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH)); + telemetry.update(); + terminateOpModeNow(); + + + + + } + if (distanceright < 7) + { + telemetry.addData("postion", "right"); + telemetry.update(); + straightRight(12); + raisearm(80); + arm.setPower(0); + driveForward(-10); + turnLeft(90); + driveForward(12); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + gripper.setPosition(0.25); + sleep(500); + driveForward(5); + terminateOpModeNow(); + + + } +else + telemetry.addData("postion","center"); + telemetry.update(); + driveForward(3.5); + raisearm(80); + arm.setPower(0); + driveForward(-8); + straightRight(11.5); + driveForward(-15); + turnLeft(90); + straightLeft(15); + driveForward(-18); + straightRight(29); + driveForward(-1.5); + telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH)); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + gripper.setPosition(0.25); + telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH)); + telemetry.update(); + sleep(500); + driveForward(5); + terminateOpModeNow(); + + + + //Values were created from robot with wheel issues 9/28/23 + + telemetry.addData("Path", "Complete"); + telemetry.update(); +// sleep(1000); // pause to display final telemetry message. + } + + + + + + /* + * Method to perform a relative move, based on encoder counts. + * Encoders are not reset as the move is based on the current position. + * Move will stop if any of three conditions occur: + * 1) Move gets to the desired position + * 2) Move runs out of time + * 3) Driver stops the opmode running. + + */ + + public void encoderDrive(double speed, + double leftInches, double rightInches, + double timeoutS) { + int newLeftTarget; + int newRightTarget; + int newBackLeftTarget; + int newbackRightTarget; + + + if (opModeIsActive()) { + + // Determine new target position, and pass to motor controller + newLeftTarget = leftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH); + newRightTarget = rightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH); + newBackLeftTarget = backleftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH); + newbackRightTarget = backrightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH); + leftDrive.setTargetPosition(newLeftTarget); + rightDrive.setTargetPosition(newRightTarget); + backrightDrive.setTargetPosition(newbackRightTarget); + backleftDrive.setTargetPosition(newBackLeftTarget); + + // Turn On RUN_TO_POSITION + leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + backrightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + backleftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // reset the timeout time and start motion. + runtime.reset(); + leftDrive.setPower(Math.abs(speed)); + rightDrive.setPower(Math.abs(speed)); + backrightDrive.setPower(Math.abs(speed)); + backleftDrive.setPower(Math.abs(speed)); + + // keep looping while we are still active, and there is time left, and both motors are running. + // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits + // its target position, the motion will stop. This is "safer" in the event that the robot will + // always end the motion as soon as possible. + // However, if you require that BOTH motors have finished their moves before the robot continues + // onto the next step, use (isBusy() || isBusy()) in the loop test. + while (opModeIsActive() && + (runtime.seconds() < timeoutS) && + (leftDrive.isBusy() && rightDrive.isBusy() && backleftDrive.isBusy() && backrightDrive.isBusy() && backrightDrive.isBusy())) { + + // Display it for the driver. + telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget); + telemetry.addData("Currently at", " at %7d :%7d", + leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backrightDrive.getCurrentPosition(), backleftDrive.getCurrentPosition()); + telemetry.update(); + } + + + leftDrive.setPower(0); + rightDrive.setPower(0); + backrightDrive.setPower(0); + backleftDrive.setPower(0); + + + // Turn off RUN_TO_POSITION + leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + sleep(250); // optional pause after each move. + } + } + + public void armEncoder(double speed, + double Inches, double timeoutS) { + int newarmTarget; + + + if (opModeIsActive()) { + + // Determine new target position, and pass to motor controller + newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH); + arm.setTargetPosition(newarmTarget); + + // Turn On RUN_TO_POSITION + arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // reset the timeout time and start motion. + runtime.reset(); + arm.setPower(Math.abs(speed)); + + // keep looping while we are still active, and there is time left, and both motors are running. + // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits + // its target position, the motion will stop. This is "safer" in the event that the robot will + // always end the motion as soon as possible. + // However, if you require that BOTH motors have finished their moves before the robot continues + // onto the next step, use (isBusy() || isBusy()) in the loop test. + while (opModeIsActive() && + (runtime.seconds() < timeoutS) && + (arm.isBusy())) { + + // Display it for the driver. + telemetry.addData("Running to", " %7d", newarmTarget); + telemetry.addData("Currently at", " at %7d", + arm.getCurrentPosition()); + telemetry.update(); + } + + + arm.setPower(0); + + + // Turn off RUN_TO_POSITION + arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + } + } +} \ No newline at end of file