From 0d131c1b1e057d4bc641f2bfed71b4e35c1f6977 Mon Sep 17 00:00:00 2001 From: robotics2 Date: Mon, 27 Nov 2023 11:34:59 -0800 Subject: [PATCH] All autos made and backstage area conistantly getting 45 points, other than Red (Backstage) all untested. --- .../ftc/teamcode/Autonomoustest.java | 118 +++-- .../org/firstinspires/ftc/teamcode/Blue.java | 443 ++++++++++++++++++ .../ftc/teamcode/Motor_Test.java | 51 -- .../org/firstinspires/ftc/teamcode/Red.java | 417 +++++++++++++++++ .../org/firstinspires/ftc/teamcode/arm.java | 2 +- .../firstinspires/ftc/teamcode/bluefront.java | 111 ++--- .../firstinspires/ftc/teamcode/colorRead.java | 93 ---- .../ftc/teamcode/colorsense.java | 183 -------- 8 files changed, 980 insertions(+), 438 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Blue.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Motor_Test.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Red.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/colorRead.java delete 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 8d3540f..bfcec30 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -29,6 +29,8 @@ 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.ColorSensor; @@ -66,7 +68,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list */ -@Autonomous(name="red front", group="Robot") +@Autonomous(name="red (backstage)", group="Robot") //@Disabled public class Autonomoustest extends LinearOpMode { @@ -109,8 +111,6 @@ public class Autonomoustest extends LinearOpMode { public void runOpMode() { hardwareinit(); - gripper.setPosition(1); - sleep(1000); // Send telemetry message to indicate successful Encoder reset /* telemetry.addData("Starting at", "%7d :%7d", @@ -236,89 +236,105 @@ public class Autonomoustest extends LinearOpMode { gripper.setPosition(0.5); } + @SuppressLint("SuspiciousIndentation") public void executeAuto() { arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); driveForward(26); + sleep(500); - int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.CM); - int distanceright = (int)distanceRight.getDistance(DistanceUnit.CM); + 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(); - sleep(500); - if (distanceleft < 30) + if (distanceleft < 7) { - //telemetry.addData("color sensor","left"); - + telemetry.addData("postion","left"); + telemetry.update(); turnLeft(90); straightLeft(2); driveForward(6.5); raisearm(80); arm.setPower(0); - driveForward(-23); + driveForward(-21); straightLeft(32); - turnLeft(10); driveForward(18); - driveForward(-40); -// straightRight(31.5); -// raisearm(80); -// wrist.setPosition(0); -// raisearm(100); -// driveForward(-1); -// gripper.setPosition(0.25); + driveForward(-28); + straightRight(33); + driveForward(-1.5); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + gripper.setPosition(0); + sleep(500); + driveForward(5); + raisearm(-270); + raisearm(50); + wrist.setPosition(1); + driveForward(-5); terminateOpModeNow(); } - if (distanceright < 30) + if (distanceright < 7) { - straightRight(13.5); + telemetry.addData("postion", "right"); + telemetry.update(); + straightRight(12); raisearm(80); arm.setPower(0); driveForward(-15.5); turnLeft(90); straightLeft(15); driveForward(8); - driveForward(-38); -// straightRight(14.5); -// raisearm(80); -// wrist.setPosition(0); -// raisearm(100); -// gripper.setPosition(.25); -// driveForward(5); -// raisearm(-200); + driveForward(-28.5); + straightRight(19); + driveForward(-1.5); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + gripper.setPosition(0); + sleep(500); + driveForward(5); + raisearm(-270); + raisearm(50); + wrist.setPosition(1); + driveForward(-5); terminateOpModeNow(); } - - driveForward(6.5); - raisearm(80); - arm.setPower(0); - driveForward(-8); - straightRight(11.5); - driveForward(-15); - turnLeft(90); - straightLeft(15); - driveForward(8); - driveForward(-26); - straightRight(29); - driveForward(-1.5); - raisearm(80); - wrist.setPosition(0); - raisearm(100); - gripper.setPosition(.25); - sleep(500); - driveForward(5); - raisearm(-270); +else + telemetry.addData("postion","center"); telemetry.update(); - sleep(250); - + driveForward(3.5); + raisearm(80); + arm.setPower(0); + driveForward(-8); + straightRight(11.5); + driveForward(-15); + turnLeft(90); + straightLeft(15); + driveForward(8); + driveForward(-26); + straightRight(29); + driveForward(-1.5); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + gripper.setPosition(0); + sleep(500); + driveForward(5); + raisearm(-270); + raisearm(50); + wrist.setPosition(1); + driveForward(-5); + terminateOpModeNow(); @@ -326,7 +342,7 @@ public class Autonomoustest extends LinearOpMode { telemetry.addData("Path", "Complete"); telemetry.update(); - sleep(1000); // pause to display final telemetry message. +// sleep(1000); // pause to display final telemetry message. } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Blue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Blue.java new file mode 100644 index 0000000..a67b224 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Blue.java @@ -0,0 +1,443 @@ +/* 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.ColorSensor; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +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="Blue", group="Robot") +//@Disabled +public class Blue 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(-8); + terminateOpModeNow(); + + + + + } + if (distanceright < 7) + { + telemetry.addData("postion", "right"); + telemetry.update(); + straightRight(12); + raisearm(80); + arm.setPower(0); + driveForward(-10); + 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(8); + driveForward(-26); + straightRight(29); + driveForward(-1.5); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + gripper.setPosition(0); + sleep(500); + driveForward(5); + raisearm(-270); + raisearm(50); + wrist.setPosition(1); + 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); + + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Motor_Test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Motor_Test.java deleted file mode 100644 index 0861605..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Motor_Test.java +++ /dev/null @@ -1,51 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.hardware.DcMotor; - -import java.util.concurrent.TimeUnit; - -@Autonomous(name="Motor_test") -public class Motor_Test extends OpMode { - DcMotor hwMotorDriveFrontLeft; - DcMotor hwMotorDriveFrontRight; - - DcMotor hwMotorDriveBackLeft; - DcMotor hwMotorDriveBackRight; - - public void init() { - hwMotorDriveFrontLeft = hardwareMap.dcMotor.get("Drive front lt"); - hwMotorDriveFrontRight = hardwareMap.dcMotor.get("Drive front rt"); - hwMotorDriveBackLeft = hardwareMap.dcMotor.get("Drive back lt"); - hwMotorDriveBackRight = hardwareMap.dcMotor.get("Drive back rt"); - } - - public void sleepSec(int iSecs){ - try { - Thread.sleep(iSecs*1000); - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); - } - } - public void loop() { - - hwMotorDriveFrontLeft.setPower(1); - sleepSec(1); - hwMotorDriveFrontRight.setPower(1); - sleepSec(1); - hwMotorDriveBackLeft.setPower(1); - sleepSec(1); - hwMotorDriveBackRight.setPower(1); - sleepSec(1); - hwMotorDriveFrontLeft.setPower(0); - hwMotorDriveFrontRight.setPower(0); - hwMotorDriveBackLeft.setPower(0); - hwMotorDriveBackRight.setPower(0); - sleepSec(10); - - } - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Red.java new file mode 100644 index 0000000..cdb4049 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Red.java @@ -0,0 +1,417 @@ +/* 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.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", group="Robot") +//@Disabled +public class Red 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 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"); + 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); + sleep(3000); + gripper.setPosition(1); + sleep(3000); + } + 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); + if (distanceleft < 7) + { + telemetry.addData("position", "left"); + telemetry.update(); + straightLeft(12); + raisearm(80); + arm.setPower(0); + driveForward(-10); + terminateOpModeNow(); + + + + + } + if (distanceright < 7) + { + telemetry.addData("position","right"); + telemetry.update(); + turnRight(90); + straightLeft(2); + driveForward(6.5); + raisearm(80); + arm.setPower(0); + driveForward(-10); + terminateOpModeNow(); + + + } + else + telemetry.addData("position","center"); + telemetry.update(); + driveForward(3.5); + raisearm(80); + arm.setPower(0); + driveForward(-8); + 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) (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); + + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/arm.java index e952d5e..666ebff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/arm.java @@ -129,7 +129,7 @@ public class arm extends OpMode { } if(gamepad2.left_trigger > 0.35) { - gripper.setPosition(0.5); + gripper.setPosition(0); } if(gamepad2.right_trigger > 0.35){ gripper.setPosition(1); 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 4d214f8..741db9c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java @@ -34,9 +34,12 @@ 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.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 @@ -63,7 +66,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="blue front", group="Robot") +@Autonomous(name="Blue (Backstage)", group="Robot") //@Disabled public class bluefront extends LinearOpMode { @@ -72,8 +75,8 @@ public class bluefront extends LinearOpMode { private DcMotor rightDrive = null; private DcMotor backrightDrive = null; private DcMotor backleftDrive = null; - private ColorSensor colorRight = null; - private ColorSensor colorLeft = null; + private DistanceSensor distanceRight = null; + private DistanceSensor distanceLeft = null; private Servo wrist = null; private Servo gripper = null; private DcMotor arm = null; @@ -177,27 +180,6 @@ public class bluefront extends LinearOpMode { 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); @@ -209,8 +191,8 @@ public class bluefront extends LinearOpMode { 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"); + 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"); @@ -256,62 +238,70 @@ public class bluefront extends LinearOpMode { { arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); driveForward(26); - int blueleft = readColorLeft(); - int blueright = readColorRight(); - double backboard = 29; - if (blueleft > 75) + sleep(500); + int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH); + int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH); + if (distanceleft < 7) { - //telemetry.addData("color sensor","left"); - if(blueleft > blueright) - telemetry.addData("color sensor","left"); - - straightLeft(13.5); + telemetry.addData("position", "left"); + telemetry.update(); + straightLeft(12); raisearm(80); arm.setPower(0); driveForward(-15.5); turnRight(90); straightRight(15); driveForward(8); - driveForward(-38); -// straightLeft(22.5); -// raisearm(80); -// wrist.setPosition(0); -// raisearm(100); -// driveForward(-5); -// gripper.setPosition(.25); + driveForward(-28.5); + straightLeft(19); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + gripper.setPosition(0); + sleep(500); + driveForward(5); + raisearm(-270); + raisearm(50); + wrist.setPosition(1); + driveForward(-5); terminateOpModeNow(); } - if (blueright > 75) + if (distanceright < 7) { - if(blueleft < blueright) - telemetry.addData("color sensor","right"); + telemetry.addData("position","right"); + telemetry.update(); turnRight(90); straightLeft(2); driveForward(6.5); raisearm(80); arm.setPower(0); - driveForward(-23); + driveForward(-21); straightRight(32); - turnRight(10); driveForward(18); - driveForward(-40); -// straightLeft(34); -// raisearm(80); -// wrist.setPosition(0); -// raisearm(100); -// driveForward(-1); -// gripper.setPosition(0.25); -// terminateOpModeNow(); + driveForward(-28); + straightLeft(33); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + gripper.setPosition(0); + sleep(500); + driveForward(5); + raisearm(-270); + raisearm(50); + wrist.setPosition(1); + driveForward(-5); + terminateOpModeNow(); } else telemetry.addData("position","center"); - driveForward(6.5); + telemetry.update(); + driveForward(3.5); raisearm(80); arm.setPower(0); driveForward(-8); @@ -325,11 +315,14 @@ public class bluefront extends LinearOpMode { raisearm(80); wrist.setPosition(0); raisearm(100); - gripper.setPosition(.25); + gripper.setPosition(0); sleep(500); driveForward(5); - telemetry.update(); - sleep(250); + raisearm(-270); + raisearm(50); + wrist.setPosition(1); + driveForward(-5); + terminateOpModeNow(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/colorRead.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/colorRead.java deleted file mode 100644 index 5194c86..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/colorRead.java +++ /dev/null @@ -1,93 +0,0 @@ -/* 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; - - -@Autonomous(name="Robot: colorRead", group="Robot") -//@Disabled -public class colorRead extends LinearOpMode { - - /* Declare OpMode members. */ - private ColorSensor colorRight = null; - private ColorSensor colorLeft = null; - - // 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. - @Override - public void runOpMode() { - hardwareinit(); - readColorRight(); - readColorLeft(); - - - } - - -// - - - public int readColorRight() { - telemetry.addData("Clear", colorRight.alpha()); - telemetry.addData("Red ", colorRight.red()); - telemetry.addData("Green", colorRight.green()); - telemetry.addData("Blue ", colorRight.blue()); - 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()); - - int bluenumber = colorLeft.blue(); - return bluenumber; - - - } - - public void hardwareinit() { - colorRight = hardwareMap.get(ColorSensor.class, "color right"); - colorLeft = hardwareMap.get(ColorSensor.class, "color left"); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/colorsense.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/colorsense.java deleted file mode 100644 index bf5c4d2..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/colorsense.java +++ /dev/null @@ -1,183 +0,0 @@ -/* 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. - - */ - - - -}