From a27996882bd6cf35106029bcc53490ba4aa89ed0 Mon Sep 17 00:00:00 2001 From: robotics2 Date: Thu, 15 Feb 2024 20:17:22 -0800 Subject: [PATCH] starighten added, center added, tweakes too both backstage code made but undtestested for some --- .../org/firstinspires/ftc/teamcode/Blue.java | 202 +++++++++++++----- .../ftc/teamcode/BlueBackStage.java | 169 ++++++++------- .../org/firstinspires/ftc/teamcode/Red.java | 102 ++++++++- .../ftc/teamcode/RedBackStage.java | 28 ++- 4 files changed, 360 insertions(+), 141 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Blue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Blue.java index 10402bf..1680db3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Blue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Blue.java @@ -100,6 +100,8 @@ public class Blue extends LinearOpMode { (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 DRIVE_SPEED_SLOW = 0.2; static final double TURN_SPEED = 0.4; static final double LONG_TIMEOUT = 1000; @@ -143,7 +145,7 @@ public class Blue extends LinearOpMode { 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 + encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, false); // S1: Forward 47 Inches with 5 Sec timeout } public void straightLeft(double distance) @@ -152,16 +154,28 @@ public class Blue extends LinearOpMode { rightDrive.setDirection(DcMotor.Direction.FORWARD); backrightDrive.setDirection(DcMotor.Direction.FORWARD); backleftDrive.setDirection(DcMotor.Direction.REVERSE); - encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); + encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, true); } + public void straightLeftOnPower(double speed) { + leftDrive.setDirection(DcMotor.Direction.FORWARD); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backrightDrive.setDirection(DcMotor.Direction.FORWARD); + backleftDrive.setDirection(DcMotor.Direction.REVERSE); + leftDrive.setPower(speed * 1.05); + rightDrive.setPower(speed * 1.05); + backrightDrive.setPower(speed); + backleftDrive.setPower(speed); + } + + 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); + encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, true); } public void turnLeft(double degrees) @@ -171,7 +185,7 @@ public class Blue extends LinearOpMode { 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); + encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT, false); } public void turnRight(double degrees) { @@ -180,9 +194,36 @@ public class Blue extends LinearOpMode { 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); + encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT, false); } + public void straighten(Double distance) + + { + driveForward(0); + double D1 = distanceLeft.getDistance(DistanceUnit.INCH); + driveForward(distance); + double D2 = distanceLeft.getDistance(DistanceUnit.INCH); + double rad = Math.atan2(D1 - D2, distance); + double degrees = Math.toDegrees(rad); + turnRight(degrees); + telemetry.addData("d1", D1); + telemetry.addData("d2", D2); + telemetry.addData("Calibration deg", degrees); + telemetry.update(); + sleep(1000); + } + + public void centerLeft() + { + double leftDistance = distanceLeft.getDistance(DistanceUnit.INCH); + straightLeft(leftDistance - 3); + telemetry.addData("leftDistance",leftDistance); + telemetry.addData("moving left x inches",leftDistance - 3); + telemetry.update(); + straightLeft(0.0); + sleep(1000); + } public void raisearm(int degrees) { armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT); @@ -238,61 +279,109 @@ public class Blue extends LinearOpMode { @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) + while (true) { - telemetry.addData("postion","left"); + int distright = (int)distanceRight.getDistance(DistanceUnit.INCH); + telemetry.addData("right dist", distright); telemetry.update(); - turnLeft(90); - straightLeft(2); - driveForward(6.5); - raisearm(45); - 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(); + +// 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(); - } - else - telemetry.addData("postion","center"); - telemetry.update(); - driveForward(3.5); - raisearm(80); - arm.setPower(0); - driveForward(-8); - terminateOpModeNow(); +// if (distanceleft < 7) +// { +// telemetry.addData("postion","left"); +// telemetry.update(); +// turnLeft(90); +// straightLeft(2); +// driveForward(6.5); +// raisearm(45); +// arm.setPower(0); +// sleep(500); +// driveForward(-20); +// +// do { +// straightLeftOnPower(DRIVE_SPEED_SLOW); +// distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH); +// +// } while (distanceleft >= 4); +// straighten(12.0); +// centerLeft(); +// driveForward(88); +// sleep(1000); +// wrist.setPosition(.465); +// gripper.setPosition(1); +// sleep(1000); +// driveForward(-3); +// terminateOpModeNow(); +// +// +// +// +// } +// if (distanceright < 7) +// { +// telemetry.addData("postion", "right"); +// telemetry.update(); +// straightRight(12); +// raisearm(80); +// arm.setPower(0); +// driveForward(-22); +// turnLeft(90); +// do { +// straightLeftOnPower(DRIVE_SPEED_SLOW); +// distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH); +// +// } while (distanceleft >= 6); +// straighten(12.0); +// centerLeft(); +// driveForward(98); +// sleep(1000); +// wrist.setPosition(.465); +// gripper.setPosition(1); +// sleep(1000); +// driveForward(-3); +// terminateOpModeNow(); +// +// +// } +// else +// telemetry.addData("postion","center"); +// telemetry.update(); +// driveForward(3.5); +// raisearm(80); +// arm.setPower(0); +// driveForward(-8); +// turnLeft(90); +// driveForward(-12); +// do { +// straightLeftOnPower(DRIVE_SPEED_SLOW); +// distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH); +// +// } while (distanceleft >= 6); +// straighten(12.0); +// centerLeft(); +// driveForward(88); +// sleep(1000); +// wrist.setPosition(.465); +// gripper.setPosition(1); +// sleep(1000); +// driveForward(-3); +// 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. + // pause to display final telemetry message. } @@ -311,7 +400,7 @@ public class Blue extends LinearOpMode { public void encoderDrive(double speed, double leftInches, double rightInches, - double timeoutS) { + double timeoutS, boolean addJuice) { int newLeftTarget; int newRightTarget; int newBackLeftTarget; @@ -338,8 +427,13 @@ public class Blue extends LinearOpMode { // reset the timeout time and start motion. runtime.reset(); - leftDrive.setPower(Math.abs(speed)); - rightDrive.setPower(Math.abs(speed)); + if(addJuice) { + leftDrive.setPower(Math.abs(speed * 1.05)); + rightDrive.setPower(Math.abs(speed * 1.05)); + } else { + leftDrive.setPower(Math.abs(speed)); + rightDrive.setPower(Math.abs(speed)); + } backrightDrive.setPower(Math.abs(speed)); backleftDrive.setPower(Math.abs(speed)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBackStage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBackStage.java index 49e1078..de78c70 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBackStage.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBackStage.java @@ -94,7 +94,9 @@ public class BlueBackStage extends LinearOpMode { 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; + double DRIVE_SPEED = 0.5; + + static final double DRIVE_SPEED_SLOW = .25; static final double TURN_SPEED = 0.4; static final double LONG_TIMEOUT = 1000; @@ -103,8 +105,7 @@ public class BlueBackStage extends LinearOpMode { static final double TICKS_TO_DEGREES = 0.07462686567; @Override - public void runOpMode() - { + public void runOpMode() { hardwareinit(); // Send telemetry message to indicate successful Encoder reset @@ -129,63 +130,56 @@ public class BlueBackStage extends LinearOpMode { } - // - public void driveForward(double distance) - { + 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 + encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, false); // S1: Forward 47 Inches with 5 Sec timeout } - public void straightLeft(double distance) - { + public void driveForwardSpeed(double distance, double speed) { + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backrightDrive.setDirection(DcMotor.Direction.REVERSE); + backleftDrive.setDirection(DcMotor.Direction.REVERSE); + encoderDrive(speed, distance, distance, LONG_TIMEOUT, false); // 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); + encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, true); } - public void straightRight(double distance) - { + 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); + encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, true); } - 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) - { + 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); + encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT, false); } public void raisearm(int degrees) { - armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT); + armEncoder(ARM_SPEED, degrees * TICKS_TO_DEGREES, LONG_TIMEOUT); } - public void hardwareinit() - { + + 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"); @@ -204,6 +198,10 @@ public class BlueBackStage extends LinearOpMode { backrightDrive.setDirection(DcMotor.Direction.REVERSE); backleftDrive.setDirection(DcMotor.Direction.REVERSE); arm.setDirection(DcMotor.Direction.REVERSE); + leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backleftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backrightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); @@ -218,30 +216,28 @@ public class BlueBackStage extends LinearOpMode { backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } - public void testWrist() - { + + public void testWrist() { wrist.setPosition(0); sleep(3000); wrist.setPosition(1); sleep(3000); } - public void testGripper() - { + + public void testGripper() { gripper.setPosition(0); sleep(3000); gripper.setPosition(1); sleep(3000); } - public void executeAuto() - { + 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) - { + int distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH); + int distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH); + if (distanceleft < 7) { telemetry.addData("position", "left"); telemetry.update(); straightLeft(13.5); @@ -251,7 +247,8 @@ public class BlueBackStage extends LinearOpMode { turnRight(90); straightRight(15); driveForward(-18); - straightLeft(20.25); + DRIVE_SPEED = .25; + straightLeft(22.25); driveForward(-.30); raisearm(80); wrist.setPosition(0); @@ -262,17 +259,14 @@ public class BlueBackStage extends LinearOpMode { raisearm(-50); wrist.setPosition(1); raisearm(-70); - straightLeft(29); - driveForward(-10); + straightLeft(35); + driveForward(-22); terminateOpModeNow(); - - } - if (distanceright < 7) - { - telemetry.addData("position","right"); + if (distanceright < 7) { + telemetry.addData("position", "right"); telemetry.update(); turnRight(88); driveForward(5.25); @@ -293,34 +287,33 @@ public class BlueBackStage extends LinearOpMode { terminateOpModeNow(); + } else { + telemetry.addData("position", "center"); + telemetry.update(); + driveForward(5); + raisearm(80); + arm.setPower(0); + driveForward(-8); + straightLeft(11.5); + driveForward(-15); + turnRight(90); + straightRight(15); + driveForward(-18); + DRIVE_SPEED = .25; + straightLeft(29); + driveForward(-2); + raisearm(80); + wrist.setPosition(0); + raisearm(100); + gripper.setPosition(1); + driveForward(6); + raisearm(-50); + wrist.setPosition(1); + raisearm(-70); + straightLeft(28); + driveForward(-25); + terminateOpModeNow(); } - else - telemetry.addData("position","center"); - telemetry.update(); - driveForward(3.5); - raisearm(80);; - arm.setPower(0); - driveForward(-8); - straightLeft(11.5); - driveForward(-15); - turnRight(90); - straightRight(15); - driveForward(-18); - straightLeft(29); - driveForward(-2); - raisearm(80); - wrist.setPosition(0); - raisearm(100); - gripper.setPosition(1); - driveForward(6); - raisearm(-50); - wrist.setPosition(1); - raisearm(-70); - straightLeft(19); - driveForward(-10); - terminateOpModeNow(); - - //Values were created from robot with wheel issues 9/28/23 @@ -343,16 +336,28 @@ public class BlueBackStage extends LinearOpMode { * 3) Driver stops the opmode running. */ +// public void encoderDrive(double speed, double leftInches, double rightInches, double timeout, boolean addJuice) { +// +// if(leftInches < 4) { +// encodedDriver(speed, leftInches, rightInches, timeout, addJuice); +// +// } else { +// // first n-4 is 50% speed (regional tournament speed) +// encodedDriver(DRIVE_SPEED, leftInches-10, rightInches-4, timeout, addJuice); +// // trailing result is 30% speed (base speed) +// encodedDriver(.3, 10, 10, timeout, addJuice); +// +// } +//} public void encoderDrive(double speed, double leftInches, double rightInches, - double timeoutS) { + double timeoutS, boolean addJuice) { int newLeftTarget; int newRightTarget; int newBackLeftTarget; int newbackRightTarget; - if (opModeIsActive()) { // Determine new target position, and pass to motor controller @@ -373,8 +378,13 @@ public class BlueBackStage extends LinearOpMode { // reset the timeout time and start motion. runtime.reset(); - leftDrive.setPower(Math.abs(speed)); - rightDrive.setPower(Math.abs(speed)); + if(addJuice) { + leftDrive.setPower(Math.abs(speed * 1.05)); + rightDrive.setPower(Math.abs(speed * 1.05)); + } else { + leftDrive.setPower(Math.abs(speed)); + rightDrive.setPower(Math.abs(speed)); + } backrightDrive.setPower(Math.abs(speed)); backleftDrive.setPower(Math.abs(speed)); @@ -395,7 +405,6 @@ public class BlueBackStage extends LinearOpMode { telemetry.update(); } - leftDrive.setPower(0); rightDrive.setPower(0); backrightDrive.setPower(0); @@ -408,7 +417,7 @@ public class BlueBackStage extends LinearOpMode { backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - sleep(250); // optional pause after each move. +// sleep(250); // optional pause after each move. } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Red.java index ddd1c5b..0792072 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Red.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Red.java @@ -97,6 +97,8 @@ public class Red extends LinearOpMode { (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 DRIVE_SPEED_SLOW = 0.25; static final double TURN_SPEED = 0.4; static final double LONG_TIMEOUT = 1000; @@ -219,6 +221,47 @@ public class Red extends LinearOpMode { backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } + + public void straightRightOnPower(double speed) { + speed *= -1; + leftDrive.setDirection(DcMotor.Direction.FORWARD); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backrightDrive.setDirection(DcMotor.Direction.FORWARD); + backleftDrive.setDirection(DcMotor.Direction.REVERSE); + leftDrive.setPower(speed * 1.05); + rightDrive.setPower(speed * 1.05); + backrightDrive.setPower(speed); + backleftDrive.setPower(speed); + } + + public void straighten(Double distance) + + { + driveForward(0); + double D1 = distanceRight.getDistance(DistanceUnit.INCH); + driveForward(distance); + double D2 = distanceRight.getDistance(DistanceUnit.INCH); + double rad = Math.atan2(D1 - D2, distance); + double degrees = Math.toDegrees(rad); + turnRight(-degrees); + telemetry.addData("d1", D1); + telemetry.addData("d2", D2); + telemetry.addData("Calibration deg", degrees); + telemetry.update(); + sleep(3000); + } + + public void centerRight() + { + double rightDistance = distanceRight.getDistance(DistanceUnit.INCH); + straightRight(rightDistance - 3); + telemetry.addData("rightDistance",rightDistance); + telemetry.addData("moving left x inches",rightDistance - 3); + telemetry.update(); + straightRight(0.0); + sleep(3000); + } + public void testWrist() { wrist.setPosition(0); @@ -235,6 +278,13 @@ public class Red extends LinearOpMode { } public void executeAuto() { +// while (true) +// { +// int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH); +// telemetry.addData("right", distanceright); +// telemetry.update(); +// sleep(500); +// } arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); driveForward(26); sleep(500); @@ -247,14 +297,27 @@ public class Red extends LinearOpMode { straightLeft(12); raisearm(80); arm.setPower(0); + sleep(500); + turnRight(90); driveForward(-10); + do { + straightRightOnPower(DRIVE_SPEED_SLOW); + distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH); + + } while (distanceright >= 4); + straighten(12.0); + centerRight(); + driveForward(88); + sleep(1000); + wrist.setPosition(.465); + gripper.setPosition(1); + sleep(1000); + driveForward(-3); terminateOpModeNow(); - - } - if (distanceright < 7) + if (distanceright < 7) //right { telemetry.addData("position","right"); telemetry.update(); @@ -263,7 +326,21 @@ public class Red extends LinearOpMode { driveForward(6.5); raisearm(45); arm.setPower(0); - driveForward(-10); + sleep(500); + driveForward(-6); + do { + straightRightOnPower(DRIVE_SPEED_SLOW); + distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH); + + } while (distanceright >= 6); + straighten(12.0); + centerRight(); + driveForward(88); + sleep(1000); + wrist.setPosition(.465); + gripper.setPosition(1); + sleep(1000); + driveForward(-3); terminateOpModeNow(); @@ -275,11 +352,28 @@ public class Red extends LinearOpMode { raisearm(80); arm.setPower(0); driveForward(-8); + turnRight(90); + driveForward(-12); + do { + straightRightOnPower(DRIVE_SPEED_SLOW); + distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH); + + } while (distanceright >= 6); + straighten(12.0); + centerRight(); + //straightLeft(3); + driveForward(88); + sleep(1000); + wrist.setPosition(.465); + gripper.setPosition(1); + sleep(1000); + driveForward(-3); terminateOpModeNow(); + //Values were created from robot with wheel issues 9/28/23 telemetry.addData("Path", "Complete"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStage.java index e553ff8..8fd0dd8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStage.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStage.java @@ -97,7 +97,7 @@ public class RedBackStage extends LinearOpMode { 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; + double DRIVE_SPEED = 0.5; static final double TURN_SPEED = 0.4; static final double LONG_TIMEOUT = 1000; @@ -234,7 +234,10 @@ public class RedBackStage extends LinearOpMode { @SuppressLint("SuspiciousIndentation") public void executeAuto() { - + backrightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backleftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); driveForward(26); sleep(500); @@ -257,6 +260,7 @@ public class RedBackStage extends LinearOpMode { driveForward(-21); straightLeft(34); driveForward(-10); + DRIVE_SPEED = .3; straightRight(35); driveForward(-5); raisearm(80); @@ -267,6 +271,11 @@ public class RedBackStage extends LinearOpMode { driveForward(4.5); sleep(500); driveForward(1.5); + raisearm(-50); + sleep(500); + wrist.setPosition(1); + raisearm(-70); + driveForward(-6); terminateOpModeNow(); @@ -284,13 +293,20 @@ public class RedBackStage extends LinearOpMode { turnLeft(90); straightLeft(15); driveForward(-20.5); + DRIVE_SPEED = .3; straightRight(19); driveForward(-1.5); raisearm(80); wrist.setPosition(0); raisearm(100); gripper.setPosition(1); + sleep(500); driveForward(8.5); + raisearm(-50); + sleep(500); + wrist.setPosition(1); + raisearm(-70); + driveForward(-6); terminateOpModeNow(); @@ -307,14 +323,20 @@ else turnLeft(90); straightLeft(15); driveForward(-18); + DRIVE_SPEED = .3; straightRight(29); - turnRight(10); + turnRight(5); raisearm(80); wrist.setPosition(0); raisearm(100); gripper.setPosition(1); sleep(500); driveForward(5); + raisearm(-50); + sleep(500); + wrist.setPosition(1); + raisearm(-70); + driveForward(-6); terminateOpModeNow();