From c3d8dc6b111adbdbffbd996adde2e0fa2c0bb437 Mon Sep 17 00:00:00 2001 From: robotics2 Date: Tue, 3 Oct 2023 17:05:29 -0700 Subject: [PATCH 1/3] color read up, not tested --- .../firstinspires/ftc/teamcode/Autonomoustest.java | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) 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 b092fd5..b3e66c4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -31,6 +31,7 @@ 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.util.ElapsedTime; @@ -69,6 +70,7 @@ public class Autonomoustest extends LinearOpMode { private DcMotor rightDrive = null; private DcMotor backrightDrive = null; private DcMotor backleftDrive = null; + private ColorSensor colorRight = null; @@ -97,6 +99,7 @@ public class Autonomoustest 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"); // 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 @@ -131,7 +134,7 @@ public class Autonomoustest extends LinearOpMode { // Note: Reverse movement is obtained by setting a negative distance (not speed) //driveForward(2); //driveForward(80); - driveForward(28); + readColor(); //turnRight(90); @@ -185,6 +188,13 @@ public class Autonomoustest extends LinearOpMode { double turning_distance = degrees * DEGREE_TOO_DISTANCE; encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT); } + public void readColor() + { + telemetry.addData("Clear", colorRight.alpha()); + telemetry.addData("Red ", colorRight.red()); + telemetry.addData("Green", colorRight.green()); + telemetry.addData("Blue ", colorRight.blue()); + } /* * Method to perform a relative move, based on encoder counts. From 9aab8fadf10587e4bd2904ae86927232a6cb7199 Mon Sep 17 00:00:00 2001 From: robotics2 Date: Thu, 5 Oct 2023 17:12:37 -0700 Subject: [PATCH 2/3] color read up and knows sides --- .../ftc/teamcode/Autonomoustest.java | 51 ++++++++++++++++--- 1 file changed, 45 insertions(+), 6 deletions(-) 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 b3e66c4..2e7a128 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -71,6 +71,7 @@ public class Autonomoustest extends LinearOpMode { private DcMotor backrightDrive = null; private DcMotor backleftDrive = null; private ColorSensor colorRight = null; + private ColorSensor colorLeft = null; @@ -100,6 +101,7 @@ public class Autonomoustest extends LinearOpMode { 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"); // 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 @@ -119,11 +121,11 @@ public class Autonomoustest extends LinearOpMode { backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); // Send telemetry message to indicate successful Encoder reset - telemetry.addData("Starting at", "%7d :%7d", + /* telemetry.addData("Starting at", "%7d :%7d", leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backleftDrive.getCurrentPosition(), - backrightDrive.getCurrentPosition()); + backrightDrive.getCurrentPosition());*/ telemetry.update(); @@ -133,9 +135,31 @@ public class Autonomoustest extends LinearOpMode { // Step through each leg of the path, // Note: Reverse movement is obtained by setting a negative distance (not speed) //driveForward(2); - //driveForward(80); - readColor(); - //turnRight(90); + loop(); + while(opModeIsActive()) + { + int blueleft = readColorLeft(); + int blueright = readColorRight(); + if (blueleft > 50) + { + //telemetry.addData("color sensor","left"); + if(blueleft > blueright) + telemetry.addData("color sensor","left"); + + + } + if (blueright > 50) + { + //telemetry.addData("color sensor", "right"); + if(blueleft < blueright) + telemetry.addData("color sensor","right"); + } + else + telemetry.addData("position","center"); + telemetry.update(); + sleep(250); + + } @@ -188,12 +212,27 @@ public class Autonomoustest extends LinearOpMode { double turning_distance = degrees * DEGREE_TOO_DISTANCE; encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT); } - public void readColor() + 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; + + + } /* From 780f5fdde91cf1efc9d00ba042da37f857e42609 Mon Sep 17 00:00:00 2001 From: robotics2 Date: Sat, 7 Oct 2023 12:07:18 -0700 Subject: [PATCH 3/3] arm holds and autonmous set wtihout the arm implemented --- .../ftc/teamcode/Autonomoustest.java | 124 +++++++++++++----- 1 file changed, 91 insertions(+), 33 deletions(-) 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 2e7a128..6212476 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -33,6 +33,8 @@ 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; /** @@ -72,6 +74,10 @@ public class Autonomoustest extends LinearOpMode { private DcMotor backleftDrive = null; private ColorSensor colorRight = null; private ColorSensor colorLeft = null; + private Servo wrist = null; + private Servo gripper = null; + private DcMotor arm = null; + @@ -95,13 +101,16 @@ public class Autonomoustest extends LinearOpMode { static final double DEGREE_TOO_DISTANCE = 0.21944444444; @Override public void runOpMode() { - // Initialize the drive system variables. + // Initialize the drive system variables 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 @@ -109,17 +118,20 @@ public class Autonomoustest extends LinearOpMode { 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); // Send telemetry message to indicate successful Encoder reset /* telemetry.addData("Starting at", "%7d :%7d", leftDrive.getCurrentPosition(), @@ -134,32 +146,67 @@ public class Autonomoustest extends LinearOpMode { // Step through each leg of the path, // Note: Reverse movement is obtained by setting a negative distance (not speed) + raisearm(1); + arm.setPower(0.001); + sleep(10000); //driveForward(2); - loop(); - while(opModeIsActive()) - { - int blueleft = readColorLeft(); - int blueright = readColorRight(); - if (blueleft > 50) - { - //telemetry.addData("color sensor","left"); - if(blueleft > blueright) - telemetry.addData("color sensor","left"); - - - } - if (blueright > 50) - { - //telemetry.addData("color sensor", "right"); - if(blueleft < blueright) - telemetry.addData("color sensor","right"); - } - else - telemetry.addData("position","center"); - telemetry.update(); - sleep(250); - - } +// driveForward(28); +// { +// int blueleft = readColorLeft(); +// int blueright = readColorRight(); +// if (blueleft > 75) +// { +// //telemetry.addData("color sensor","left"); +// if(blueleft > blueright) +// telemetry.addData("color sensor","left"); +// turnLeft(90); +// straightLeft(2); +// driveForward(11); +// driveForward(-30); +// turnLeft(180); +// driveForward(19); +// straightLeft(7); +// //use arm to place pixel +// straightRight(34); +// driveForward(11); +// terminateOpModeNow(); +// +// +// +// +// } +// if (blueright > 75) +// { +// //telemetry.addData("color sensor", "right"); +// if(blueleft < blueright) +// telemetry.addData("color sensor","right"); +// straightRight(11); +// driveForward(-17); +// turnRight(90); +// straightLeft(8); +// driveForward(28.5); +// //use arm to place pixel +// straightRight(20); +// driveForward(11); +// terminateOpModeNow(); +// +// +// } +// else +// telemetry.addData("position","center"); +// driveForward(11); +// driveForward(-17); +// turnRight(90); +// driveForward(39); +// straightRight(3.5); +// //use arm to place pixel +// straightRight(23.5); +// driveForward(11); +// +// telemetry.update(); +// sleep(250); +// +// } @@ -235,6 +282,14 @@ public class Autonomoustest extends LinearOpMode { } + public void raisearm(long seconds) + { + arm.setPower(.1); + sleep(seconds * 1000); + } + + + /* * Method to perform a relative move, based on encoder counts. * Encoders are not reset as the move is based on the current position. @@ -242,7 +297,9 @@ public class Autonomoustest extends LinearOpMode { * 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) { @@ -251,7 +308,6 @@ public class Autonomoustest extends LinearOpMode { int newBackLeftTarget; int newbackRightTarget; - // Ensure that the opmode is still active if (opModeIsActive()) { // Determine new target position, and pass to motor controller @@ -294,11 +350,13 @@ public class Autonomoustest extends LinearOpMode { telemetry.update(); } - // Stop all motion; - leftDrive.setPower(0); - rightDrive.setPower(0); - backrightDrive.setPower(0); - backleftDrive.setPower(0); + + + leftDrive.setPower(0); + rightDrive.setPower(0); + backrightDrive.setPower(0); + backleftDrive.setPower(0); + // Turn off RUN_TO_POSITION leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);