From 6f5e08e0cdd21d49fb48bdc69fa601d052a72fdf Mon Sep 17 00:00:00 2001 From: robotics2 Date: Fri, 3 Nov 2023 16:36:34 -0700 Subject: [PATCH] auto blue, and red front have been made not tested but values switched --- .../ftc/teamcode/Autonomoustest.java | 29 ++- .../firstinspires/ftc/teamcode/bluefront.java | 5 +- .../ftc/teamcode/colorsense.java | 183 ++++++++++++++++++ 3 files changed, 199 insertions(+), 18 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/colorsense.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 ab52f9d..1ab5777 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -245,34 +245,34 @@ public class Autonomoustest extends LinearOpMode { } public void testGripper() { - gripper.setPosition(0); - sleep(3000); - gripper.setPosition(1); - sleep(3000); + gripper.setPosition(0.5); + } public void executeAuto() { arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + gripper.setPosition(1); + sleep(1000); driveForward(28); int blueleft = readColorLeft(); int blueright = readColorRight(); double backboard = 29; - if (blueleft > 100) + if (blueleft > 75) { //telemetry.addData("color sensor","left"); if(blueleft > blueright) telemetry.addData("color sensor","left"); turnLeft(90); straightLeft(2); - driveForward(8); - raisearm(30); + driveForward(6.5); + raisearm(80); arm.setPower(0); driveForward(-23); straightLeft(32); turnLeft(10); driveForward(18); driveForward(-31); - straightRight(34); + straightRight(31.5); raisearm(80); wrist.setPosition(0); raisearm(100); @@ -284,12 +284,12 @@ public class Autonomoustest extends LinearOpMode { } - if (blueright > 100) + if (blueright > 175) { if(blueleft < blueright) telemetry.addData("color sensor","right"); - straightRight(8); - raisearm(30); + straightRight(11); + raisearm(80); arm.setPower(0); driveForward(-13.5); turnLeft(90); @@ -309,11 +309,11 @@ public class Autonomoustest extends LinearOpMode { else telemetry.addData("position","center"); driveForward(4); - raisearm(30); + raisearm(80); arm.setPower(0); - driveForward(-6); + driveForward(-8); straightRight(11.5); - driveForward(-17); + driveForward(-15); turnLeft(90); straightLeft(15); driveForward(8); @@ -322,7 +322,6 @@ public class Autonomoustest extends LinearOpMode { raisearm(80); wrist.setPosition(0); raisearm(100); - driveForward(-5); 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 index 86843ec..49f5650 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java @@ -289,10 +289,9 @@ public class bluefront extends LinearOpMode { telemetry.addData("color sensor","right"); turnRight(90); straightLeft(2); - driveForward(8); raisearm(30); arm.setPower(0); - driveForward(-23); + driveForward(-15); straightRight(32); turnRight(10); driveForward(18); @@ -309,7 +308,7 @@ public class bluefront extends LinearOpMode { } else telemetry.addData("position","center"); - driveForward(4); + driveForward(11); raisearm(30); arm.setPower(0); driveForward(-6); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/colorsense.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/colorsense.java new file mode 100644 index 0000000..bf5c4d2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/colorsense.java @@ -0,0 +1,183 @@ +/* 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="color", group="Robot") +//@Disabled +public class colorsense extends LinearOpMode { + + /* Declare OpMode members. */ + private ColorSensor colorRight = null; + private ColorSensor colorLeft = 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 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.red(); + 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.red(); + return bluenumber; + + + } + public void hardwareinit() + { + colorRight = hardwareMap.get(ColorSensor.class, "color right"); + colorLeft = hardwareMap.get(ColorSensor.class, "left color"); + } + public void executeAuto() + { + while(opModeIsActive()) + { + + 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(); + } + + } + + + + + + /* + * 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. + + */ + + + +}