From 162922a1648a298cf187bd62d0d376f4785938ec Mon Sep 17 00:00:00 2001 From: robotics2 Date: Thu, 2 Nov 2023 20:46:41 -0700 Subject: [PATCH] auto blue, and red front have been made not tested but values switched --- .../ftc/teamcode/Autonomoustest.java | 46 +- .../firstinspires/ftc/teamcode/bluefront.java | 467 ++++++++++++++++++ 2 files changed, 492 insertions(+), 21 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java index 9d9c5c4..ab52f9d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -63,7 +63,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list */ -@Autonomous(name="Robot: Auto Drive By Encoder", group="Robot") +@Autonomous(name="red front", group="Robot") //@Disabled public class Autonomoustest extends LinearOpMode { @@ -182,7 +182,8 @@ public class Autonomoustest extends LinearOpMode { telemetry.addData("Red ", colorRight.red()); telemetry.addData("Green", colorRight.green()); telemetry.addData("Blue ", colorRight.blue()); - int bluenumber = colorRight.blue(); + //telemetry.update(); + int bluenumber = colorRight.red(); return bluenumber; } @@ -191,8 +192,8 @@ public class Autonomoustest extends LinearOpMode { telemetry.addData("Red left ", colorLeft.red()); telemetry.addData("Green left", colorLeft.green()); telemetry.addData("Blue left", colorLeft.blue()); - - int bluenumber = colorLeft.blue(); + //telemetry.update(); + int bluenumber = colorLeft.red(); return bluenumber; @@ -255,35 +256,36 @@ public class Autonomoustest extends LinearOpMode { driveForward(28); int blueleft = readColorLeft(); int blueright = readColorRight(); - if (blueleft > 75) + double backboard = 29; + if (blueleft > 100) { //telemetry.addData("color sensor","left"); if(blueleft > blueright) telemetry.addData("color sensor","left"); turnLeft(90); straightLeft(2); - driveForward(11); - raisearm(26); + driveForward(8); + raisearm(30); arm.setPower(0); - driveForward(-26); + driveForward(-23); straightLeft(32); turnLeft(10); driveForward(18); driveForward(-31); - double backboard = 34; - straightRight(backboard); + straightRight(34); raisearm(80); + wrist.setPosition(0); + raisearm(100); driveForward(-1); - gripper.setPosition(0); + gripper.setPosition(0.25); terminateOpModeNow(); } - if (blueright > 75) + if (blueright > 100) { - //telemetry.addData("color sensor", "right"); if(blueleft < blueright) telemetry.addData("color sensor","right"); straightRight(8); @@ -294,32 +296,34 @@ public class Autonomoustest extends LinearOpMode { straightLeft(15); driveForward(8); driveForward(-26); - double backboard = 22.5; - straightRight(backboard); + straightRight(22.5); raisearm(80); + wrist.setPosition(0); + raisearm(100); driveForward(-5); - gripper.setPosition(1); + gripper.setPosition(.25); terminateOpModeNow(); } else telemetry.addData("position","center"); - driveForward(3.5); + driveForward(4); raisearm(30); arm.setPower(0); - driveForward(-3.5); + driveForward(-6); straightRight(11.5); driveForward(-17); turnLeft(90); straightLeft(15); driveForward(8); driveForward(-26); - double backboard = 29; - straightRight(backboard); + straightRight(29); raisearm(80); + wrist.setPosition(0); + raisearm(100); driveForward(-5); - gripper.setPosition(1); + gripper.setPosition(.25); telemetry.update(); sleep(250); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java new file mode 100644 index 0000000..86843ec --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java @@ -0,0 +1,467 @@ +/* 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 com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +/** + * 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="blue front", group="Robot") +//@Disabled +public class bluefront extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + private DcMotor backrightDrive = null; + private DcMotor backleftDrive = null; + private ColorSensor colorRight = null; + private ColorSensor colorLeft = null; + private Servo wrist = null; + private Servo gripper = null; + private DcMotor arm = 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.2; + 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 int readColorRight() { + telemetry.addData("Clear", colorRight.alpha()); + telemetry.addData("Red ", colorRight.red()); + telemetry.addData("Green", colorRight.green()); + telemetry.addData("Blue ", colorRight.blue()); + //telemetry.update(); + int bluenumber = colorRight.blue(); + return bluenumber; + } + + public int readColorLeft() { + telemetry.addData("Clear Left", colorLeft.alpha()); + telemetry.addData("Red left ", colorLeft.red()); + telemetry.addData("Green left", colorLeft.green()); + telemetry.addData("Blue left", colorLeft.blue()); + //telemetry.update(); + int bluenumber = colorLeft.blue(); + return bluenumber; + + + } + + 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"); + colorRight = hardwareMap.get(ColorSensor.class, "color right"); + colorLeft = hardwareMap.get(ColorSensor.class, "color left"); + gripper = hardwareMap.get(Servo.class, "gripper"); + arm = hardwareMap.get(DcMotor.class, "arm raise"); + wrist = hardwareMap.get(Servo.class, "wrist"); + // 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); + sleep(3000); + gripper.setPosition(1); + sleep(3000); + } + public void executeAuto() + { + arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + driveForward(28); + int blueleft = readColorLeft(); + int blueright = readColorRight(); + double backboard = 29; + if (blueleft > 100) + { + //telemetry.addData("color sensor","left"); + if(blueleft > blueright) + telemetry.addData("color sensor","left"); + + straightLeft(8); + raisearm(30); + arm.setPower(0); + driveForward(-13.5); + turnRight(90); + straightRight(15); + driveForward(8); + driveForward(-26); + straightLeft(22.5); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + driveForward(-5); + gripper.setPosition(.25); + terminateOpModeNow(); + + + + + } + if (blueright > 100) + { + if(blueleft < blueright) + telemetry.addData("color sensor","right"); + turnRight(90); + straightLeft(2); + driveForward(8); + raisearm(30); + arm.setPower(0); + driveForward(-23); + straightRight(32); + turnRight(10); + driveForward(18); + driveForward(-31); + straightLeft(34); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + driveForward(-1); + gripper.setPosition(0.25); + terminateOpModeNow(); + + + } + else + telemetry.addData("position","center"); + driveForward(4); + raisearm(30); + arm.setPower(0); + driveForward(-6); + straightLeft(11.5); + driveForward(-17); + turnRight(90); + straightRight(15); + driveForward(8); + driveForward(-26); + straightLeft(29); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + driveForward(-5); + gripper.setPosition(.25); + telemetry.update(); + sleep(250); + + + + + //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) (rightInches * 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