From 2b638e1953790c31e1f356910cc2f5350e1ebae3 Mon Sep 17 00:00:00 2001 From: robotics2 Date: Fri, 3 Nov 2023 19:54:05 -0700 Subject: [PATCH 01/10] auto blue, and red front have been made not tested but values switched --- .../ftc/teamcode/Autonomoustest.java | 48 ++++++++-------- .../firstinspires/ftc/teamcode/bluefront.java | 56 ++++++++++--------- 2 files changed, 55 insertions(+), 49 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 1ab5777..8234096 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -93,7 +93,7 @@ public class Autonomoustest 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.2; + static final double DRIVE_SPEED = 0.3; static final double TURN_SPEED = 0.4; static final double LONG_TIMEOUT = 1000; @@ -105,6 +105,8 @@ 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", @@ -251,13 +253,11 @@ public class Autonomoustest extends LinearOpMode { public void executeAuto() { arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - gripper.setPosition(1); - sleep(1000); - driveForward(28); + driveForward(26); int blueleft = readColorLeft(); int blueright = readColorRight(); double backboard = 29; - if (blueleft > 75) + if (blueleft > 50 ) { //telemetry.addData("color sensor","left"); if(blueleft > blueright) @@ -271,44 +271,45 @@ public class Autonomoustest extends LinearOpMode { straightLeft(32); turnLeft(10); driveForward(18); - driveForward(-31); - straightRight(31.5); - raisearm(80); - wrist.setPosition(0); - raisearm(100); - driveForward(-1); - gripper.setPosition(0.25); + driveForward(-40); +// straightRight(31.5); +// raisearm(80); +// wrist.setPosition(0); +// raisearm(100); +// driveForward(-1); +// gripper.setPosition(0.25); terminateOpModeNow(); } - if (blueright > 175) + if (blueright > 50) { if(blueleft < blueright) telemetry.addData("color sensor","right"); straightRight(11); raisearm(80); arm.setPower(0); - driveForward(-13.5); + driveForward(-15.5); turnLeft(90); straightLeft(15); driveForward(8); - driveForward(-26); - straightRight(22.5); - raisearm(80); - wrist.setPosition(0); - raisearm(100); - driveForward(-5); - gripper.setPosition(.25); + driveForward(-38); +// straightRight(14.5); +// raisearm(80); +// wrist.setPosition(0); +// raisearm(100); +// gripper.setPosition(.25); +// driveForward(5); +// raisearm(-200); terminateOpModeNow(); } else telemetry.addData("position","center"); - driveForward(4); + driveForward(2.5); raisearm(80); arm.setPower(0); driveForward(-8); @@ -323,6 +324,9 @@ public class Autonomoustest extends LinearOpMode { wrist.setPosition(0); raisearm(100); gripper.setPosition(.25); + sleep(500); + driveForward(5); + raisearm(-270); telemetry.update(); sleep(250); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java index 49f5650..9f26774 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java @@ -93,7 +93,7 @@ public class bluefront 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.2; + static final double DRIVE_SPEED = 0.3; static final double TURN_SPEED = 0.4; static final double LONG_TIMEOUT = 1000; @@ -253,7 +253,7 @@ public class bluefront extends LinearOpMode { public void executeAuto() { arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - driveForward(28); + driveForward(26); int blueleft = readColorLeft(); int blueright = readColorRight(); double backboard = 29; @@ -263,20 +263,20 @@ public class bluefront extends LinearOpMode { if(blueleft > blueright) telemetry.addData("color sensor","left"); - straightLeft(8); - raisearm(30); + straightLeft(11); + raisearm(80); arm.setPower(0); - driveForward(-13.5); + driveForward(-15.5); turnRight(90); straightRight(15); driveForward(8); - driveForward(-26); - straightLeft(22.5); - raisearm(80); - wrist.setPosition(0); - raisearm(100); - driveForward(-5); - gripper.setPosition(.25); + driveForward(-38); +// straightLeft(22.5); +// raisearm(80); +// wrist.setPosition(0); +// raisearm(100); +// driveForward(-5); +// gripper.setPosition(.25); terminateOpModeNow(); @@ -289,31 +289,32 @@ public class bluefront extends LinearOpMode { telemetry.addData("color sensor","right"); turnRight(90); straightLeft(2); - raisearm(30); + driveForward(6.5); + raisearm(80); arm.setPower(0); - driveForward(-15); + driveForward(-23); straightRight(32); turnRight(10); driveForward(18); - driveForward(-31); - straightLeft(34); - raisearm(80); - wrist.setPosition(0); - raisearm(100); - driveForward(-1); - gripper.setPosition(0.25); - terminateOpModeNow(); + driveForward(-40); +// straightLeft(34); +// raisearm(80); +// wrist.setPosition(0); +// raisearm(100); +// driveForward(-1); +// gripper.setPosition(0.25); +// terminateOpModeNow(); } else telemetry.addData("position","center"); - driveForward(11); - raisearm(30); + driveForward(2.5); + raisearm(80); arm.setPower(0); - driveForward(-6); + driveForward(-8); straightLeft(11.5); - driveForward(-17); + driveForward(-15); turnRight(90); straightRight(15); driveForward(8); @@ -322,8 +323,9 @@ public class bluefront extends LinearOpMode { raisearm(80); wrist.setPosition(0); raisearm(100); - driveForward(-5); gripper.setPosition(.25); + sleep(500); + driveForward(5); telemetry.update(); sleep(250); From 7a861d562e4584ad1f87527caf8eaeb805f9dff7 Mon Sep 17 00:00:00 2001 From: robotics2 Date: Sat, 4 Nov 2023 08:54:36 -0700 Subject: [PATCH 02/10] auto blue, and red front have been made not tested but values switched --- .../main/java/org/firstinspires/ftc/teamcode/bluefront.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 9f26774..d0d29fa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java @@ -214,6 +214,7 @@ public class bluefront extends LinearOpMode { gripper = hardwareMap.get(Servo.class, "gripper"); arm = hardwareMap.get(DcMotor.class, "arm raise"); wrist = hardwareMap.get(Servo.class, "wrist"); + wrist.setPosition(1); // 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 @@ -257,7 +258,7 @@ public class bluefront extends LinearOpMode { int blueleft = readColorLeft(); int blueright = readColorRight(); double backboard = 29; - if (blueleft > 100) + if (blueleft > 75) { //telemetry.addData("color sensor","left"); if(blueleft > blueright) @@ -283,7 +284,7 @@ public class bluefront extends LinearOpMode { } - if (blueright > 100) + if (blueright > 75) { if(blueleft < blueright) telemetry.addData("color sensor","right"); From 17f4fa47dc5ed64489fda3f32178f9783018a9d5 Mon Sep 17 00:00:00 2001 From: robotics2 Date: Sat, 4 Nov 2023 08:56:52 -0700 Subject: [PATCH 03/10] autnomous first meet --- .../src/main/java/org/firstinspires/ftc/teamcode/bluefront.java | 1 + 1 file changed, 1 insertion(+) 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 d0d29fa..fd91263 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/bluefront.java @@ -215,6 +215,7 @@ public class bluefront extends LinearOpMode { 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 From 20680c3011951ea96f67918f748b5d676ad053bd Mon Sep 17 00:00:00 2001 From: robotics2 Date: Sat, 4 Nov 2023 08:58:18 -0700 Subject: [PATCH 04/10] autnomous first meet --- .../java/org/firstinspires/ftc/teamcode/Autonomoustest.java | 2 ++ 1 file changed, 2 insertions(+) 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 8234096..90da515 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -216,6 +216,8 @@ public class Autonomoustest extends LinearOpMode { 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 From e991c38b72c045404c8f996765c3e19e68b9326a Mon Sep 17 00:00:00 2001 From: Carlos Date: Thu, 9 Nov 2023 10:51:43 -0800 Subject: [PATCH 05/10] Commenting a line, is this a bug? --- .../java/org/firstinspires/ftc/teamcode/Autonomoustest.java | 2 +- 1 file changed, 1 insertion(+), 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 90da515..d7154b7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -370,7 +370,7 @@ public class Autonomoustest extends LinearOpMode { // 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); + newBackLeftTarget = backleftDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH); /* is this a bug ~ skewing issue? */ newbackRightTarget = backrightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH); leftDrive.setTargetPosition(newLeftTarget); rightDrive.setTargetPosition(newRightTarget); From 36d3af564ed2e88f8c8b04079e6e88cf453d1272 Mon Sep 17 00:00:00 2001 From: Carlos Date: Thu, 9 Nov 2023 14:20:34 -0800 Subject: [PATCH 06/10] Removing unused variables --- .../java/org/firstinspires/ftc/teamcode/Autonomoustest.java | 4 ++-- 1 file changed, 2 insertions(+), 2 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 d7154b7..03a07a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -258,7 +258,7 @@ public class Autonomoustest extends LinearOpMode { driveForward(26); int blueleft = readColorLeft(); int blueright = readColorRight(); - double backboard = 29; + // double backboard = 29; -- not used if (blueleft > 50 ) { //telemetry.addData("color sensor","left"); @@ -398,7 +398,7 @@ public class Autonomoustest extends LinearOpMode { // 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())) { + (leftDrive.isBusy() && rightDrive.isBusy() && backleftDrive.isBusy() && backrightDrive.isBusy())) { // Display it for the driver. telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget); From 8aef2037c5e3caea17f0410eb6864657670a3062 Mon Sep 17 00:00:00 2001 From: Carlos Date: Thu, 9 Nov 2023 14:21:15 -0800 Subject: [PATCH 07/10] Fixing variable bug, possible cause of strafing issue? --- .../java/org/firstinspires/ftc/teamcode/Autonomoustest.java | 2 +- 1 file changed, 1 insertion(+), 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 03a07a8..a376bb0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -370,7 +370,7 @@ public class Autonomoustest extends LinearOpMode { // 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); /* is this a bug ~ skewing issue? */ + newBackLeftTarget = backleftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH); newbackRightTarget = backrightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH); leftDrive.setTargetPosition(newLeftTarget); rightDrive.setTargetPosition(newRightTarget); From 4b6fd66770a14ae29ad88e10deecdeb42998ab92 Mon Sep 17 00:00:00 2001 From: Carlos Date: Thu, 9 Nov 2023 14:21:37 -0800 Subject: [PATCH 08/10] Adding sample code base, still work in progress --- .../ftc/teamcode/reorg/AutonomousCommand.java | 7 + .../teamcode/reorg/AutonomousConstant.java | 18 ++ .../teamcode/reorg/AutonomousTestFacade.java | 52 ++++ .../teamcode/reorg/AutonomousTestService.java | 240 ++++++++++++++++++ 4 files changed, 317 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousCommand.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousConstant.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestFacade.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestService.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousCommand.java new file mode 100644 index 0000000..9c2838e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousCommand.java @@ -0,0 +1,7 @@ +package org.firstinspires.ftc.teamcode.reorg; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public interface AutonomousCommand { + public void execute(boolean value, Telemetry telemetry); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousConstant.java new file mode 100644 index 0000000..c6f8eb1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousConstant.java @@ -0,0 +1,18 @@ +package org.firstinspires.ftc.teamcode.reorg; + +public class AutonomousConstant { + + 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; + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestFacade.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestFacade.java new file mode 100644 index 0000000..2a57a11 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestFacade.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode.reorg; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +@Autonomous(name="auto-test-facade", group="Robot") +public class AutonomousTestFacade extends LinearOpMode { + + @Override + public void runOpMode() { + /* + Class where all our initialization code lives + To use it, we just instantiate a copy of it! + */ + AutonomousTestService service = new AutonomousTestService(hardwareMap); + + /* + Initialize the hardware stuff + */ + service.initializeHardware(); // good + + /* + There's a sleep here, need to determine if it's needed + sleep(1000); + */ + + + /* + Initialize the direction stuff + */ + service.initializeDirection(); // good + + /* + Stop and reset all encoders + */ + service.initializeEncoders(); // good + + /* + Update telemetry + */ + telemetry.update(); + + /* + From the JavaDoc: Pauses the Linear Op Mode until start has been pressed + or until the current thread is interrupted. + */ + waitForStart(); + { + service.execute(opModeIsActive(), telemetry); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestService.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestService.java new file mode 100644 index 0000000..2d0f9b4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/reorg/AutonomousTestService.java @@ -0,0 +1,240 @@ +package org.firstinspires.ftc.teamcode.reorg; + +import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class AutonomousTestService implements AutonomousCommand { + + private DcMotor leftDrive; + private DcMotor rightDrive; + private DcMotor backRightDrive; + private DcMotor backLeftDrive; + private ColorSensor colorRight; + private ColorSensor colorLeft; + private Servo wrist; + private Servo gripper; + private DcMotor arm; + private ElapsedTime runTime; + + public AutonomousTestService(HardwareMap hwMap) { + this.runTime = new ElapsedTime(); + leftDrive = hwMap.get(DcMotor.class, "Drive front lt"); + rightDrive = hwMap.get(DcMotor.class, "Drive front rt"); + backLeftDrive = hwMap.get(DcMotor.class, "Drive back lt"); + backRightDrive = hwMap.get(DcMotor.class, "Drive back rt"); + colorRight = hwMap.get(ColorSensor.class, "color right"); + colorLeft = hwMap.get(ColorSensor.class, "color left"); + arm = hwMap.get(DcMotor.class, "arm raise"); + wrist = hwMap.get(Servo.class, "wrist"); + gripper = hwMap.get(Servo.class, "gripper"); + } + + public void initializeHardware() { + setWristPosition(1); + setGripperPosition(1); + arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + private void setWristPosition(int position) { + wrist.setPosition(position); + } + + private void setGripperPosition(int position) { + wrist.setPosition(position); + } + + public void initializeDirection() { + setDriveDirectionForward(); + setArmDirection(DcMotor.Direction.REVERSE); + } + + public void initializeEncoders() { + setDriveModeStopAndResetEncoder(); + setDriveModeRunUsingEncoder(); + setArmDriveModeStopAndResetEncoder(); + setArmDriveModeRunUsingEncoder(); + } + + private void setDriveDirectionForward() { + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); + backRightDrive.setDirection(DcMotor.Direction.REVERSE); + } + + private void setDriveDirectionLeft() { + leftDrive.setDirection(DcMotor.Direction.FORWARD); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backLeftDrive.setDirection(DcMotor.Direction.FORWARD); + backRightDrive.setDirection(DcMotor.Direction.REVERSE); + } + + private void setDriveDirectionStraightLeft() { + leftDrive.setDirection(DcMotor.Direction.FORWARD); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); + backRightDrive.setDirection(DcMotor.Direction.FORWARD); + } + + private void turn(double degrees, boolean isOpModeActive, Telemetry telemetry) { + double turning_distance = degrees * AutonomousConstant.DEGREE_TOO_DISTANCE; + move(turning_distance, isOpModeActive, telemetry); + } + + private void move(double degrees, boolean isOpModeActive, Telemetry telemetry) { + encoderDrive(AutonomousConstant.DRIVE_SPEED, degrees, degrees, AutonomousConstant.LONG_TIMEOUT, isOpModeActive, telemetry); + } + + private void setDriveMode(DcMotor.RunMode runMode) { + leftDrive.setMode(runMode); + rightDrive.setMode(runMode); + backLeftDrive.setMode(runMode); + backRightDrive.setMode(runMode); + } + private void setArmMode(DcMotor.RunMode runMode) { + arm.setMode(runMode); + } + + public void setArmDirection(DcMotor.Direction direction) { + arm.setDirection(direction); + } + + public void setDriveModeStopAndResetEncoder() { + setDriveMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + } + + public void setDriveModeRunUsingEncoder() { + setDriveMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + public void setArmDriveModeStopAndResetEncoder() { + setArmMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + } + + public void setArmDriveModeRunUsingEncoder() { + setArmMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + private void setDriveTargetPosition(int nlt, int nrt, int nbrt, int nblt) { + leftDrive.setTargetPosition(nlt); + rightDrive.setTargetPosition(nrt); + backRightDrive.setTargetPosition(nbrt); + backLeftDrive.setTargetPosition(nblt); + } + + public void setDriveModeRunToPosition() { + setDriveMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + public void setDrivePower(double speed) { + leftDrive.setPower(Math.abs(speed)); + rightDrive.setPower(Math.abs(speed)); + backRightDrive.setPower(Math.abs(speed)); + backLeftDrive.setPower(Math.abs(speed)); + } + + public int getColorSensorReadingOnRightSide(Telemetry telemetry) { + telemetry.addData("Clear Right", colorRight.alpha()); + telemetry.addData("Red Right", colorRight.red()); + telemetry.addData("Green Right", colorRight.green()); + telemetry.addData("Blue Right", colorRight.blue()); + telemetry.addData("Color Sensor","right"); + return colorRight.red(); + } + + public int getColorSensorReadingOnLeftSide(Telemetry telemetry) { + telemetry.addData("Clear Left", colorLeft.alpha()); + telemetry.addData("Red Left ", colorLeft.red()); + telemetry.addData("Green Left", colorLeft.green()); + telemetry.addData("Blue Left", colorLeft.blue()); + return colorLeft.red(); + } + + private int calculateCurrentPosition(int position, double distance) { + return position + (int) (distance * AutonomousConstant.COUNTS_PER_INCH); + } + + private void drive(double distance, boolean isOpModeActive, Telemetry telemetry) { + encoderDrive(AutonomousConstant.DRIVE_SPEED, distance, distance, AutonomousConstant.LONG_TIMEOUT, isOpModeActive, telemetry); + } + + private void encoderDrive(double speed, double leftInches, double rightInches, double timeout, boolean isOpModeActive, Telemetry telemetry) { + + if (isOpModeActive) { + + int ldgcp = leftDrive.getCurrentPosition(); + int rdgcp = leftDrive.getCurrentPosition(); + int bldgcp = leftDrive.getCurrentPosition(); + int brdgcp = leftDrive.getCurrentPosition(); + + int nlt = calculateCurrentPosition(ldgcp, leftInches); + int nrt = calculateCurrentPosition(rdgcp, rightInches); + int nblt = calculateCurrentPosition(bldgcp, leftInches); + int nbrt = calculateCurrentPosition(brdgcp, rightInches); + + setDriveTargetPosition(nlt, nrt, nblt, nbrt); + + setDriveModeRunToPosition(); + + runTime.reset(); + + setDrivePower(speed); + + while (isOpModeActive && (runTime.seconds() < timeout) && (leftDrive.isBusy() && rightDrive.isBusy() && backLeftDrive.isBusy() && backRightDrive.isBusy())) { + telemetry.addData("Running to", " %7d :%7d", nlt, nrt); + telemetry.addData("Currently at", " at %7d :%7d", ldgcp, rdgcp, bldgcp, brdgcp); + telemetry.update(); + } + + setDrivePower(0); + + setDriveModeRunUsingEncoder(); + + } + + } + + @Override + public void execute(boolean isOpModeActive, Telemetry telemetry) { + /* + Do we need this? Can it be part of the initialization? + */ + // arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + /* + Drive forward to left/right/center placement of item + */ + setDriveDirectionForward(); + drive(26, isOpModeActive, telemetry); + + + + if (getColorSensorReadingOnLeftSide(telemetry) > 50) { + + if(getColorSensorReadingOnLeftSide(telemetry) > getColorSensorReadingOnRightSide(telemetry)) + telemetry.addData("color sensor","left"); + + setDriveDirectionLeft(); + turn(90, isOpModeActive, telemetry); + + setDriveDirectionStraightLeft(); + move(8.5, isOpModeActive, telemetry); + + } + + + if (getColorSensorReadingOnRightSide(telemetry) > 50) { + + if(getColorSensorReadingOnRightSide(telemetry) > getColorSensorReadingOnLeftSide(telemetry)) + telemetry.addData("color sensor","right"); + + } + + + } +} From 7c1a0923e2d1aa8ff0862f3e060811232e5d154b Mon Sep 17 00:00:00 2001 From: Carlos Date: Thu, 9 Nov 2023 14:22:17 -0800 Subject: [PATCH 09/10] Fix camel casing --- .../java/org/firstinspires/ftc/teamcode/Autonomoustest.java | 6 +++--- 1 file changed, 3 insertions(+), 3 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 a376bb0..3ad5165 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -362,7 +362,7 @@ public class Autonomoustest extends LinearOpMode { int newLeftTarget; int newRightTarget; int newBackLeftTarget; - int newbackRightTarget; + int newBackRightTarget; if (opModeIsActive()) { @@ -371,10 +371,10 @@ public class Autonomoustest extends LinearOpMode { 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); + newBackRightTarget = backrightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH); leftDrive.setTargetPosition(newLeftTarget); rightDrive.setTargetPosition(newRightTarget); - backrightDrive.setTargetPosition(newbackRightTarget); + backrightDrive.setTargetPosition(newBackRightTarget); backleftDrive.setTargetPosition(newBackLeftTarget); // Turn On RUN_TO_POSITION From 58f738e28d69c7fde8d44807b04bcd48a9d4e64b Mon Sep 17 00:00:00 2001 From: Carlos Date: Thu, 9 Nov 2023 23:20:23 -0800 Subject: [PATCH 10/10] Fix camel casing --- .../ftc/teamcode/Autonomoustest.java | 80 +++++++++---------- 1 file changed, 40 insertions(+), 40 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 3ad5165..fe8fb02 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -70,8 +70,8 @@ public class Autonomoustest extends LinearOpMode { /* Declare OpMode members. */ private DcMotor leftDrive = null; private DcMotor rightDrive = null; - private DcMotor backrightDrive = null; - private DcMotor backleftDrive = null; + private DcMotor backRightDrive = null; + private DcMotor backLeftDrive = null; private ColorSensor colorRight = null; private ColorSensor colorLeft = null; private Servo wrist = null; @@ -112,8 +112,8 @@ public class Autonomoustest extends LinearOpMode { /* telemetry.addData("Starting at", "%7d :%7d", leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), - backleftDrive.getCurrentPosition(), - backrightDrive.getCurrentPosition());*/ + backLeftDrive.getCurrentPosition(), + backRightDrive.getCurrentPosition());*/ telemetry.update(); @@ -137,8 +137,8 @@ public class Autonomoustest extends LinearOpMode { { leftDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.FORWARD); - backrightDrive.setDirection(DcMotor.Direction.REVERSE); - backleftDrive.setDirection(DcMotor.Direction.REVERSE); + 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 } @@ -146,8 +146,8 @@ public class Autonomoustest extends LinearOpMode { { leftDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD); - backrightDrive.setDirection(DcMotor.Direction.FORWARD); - backleftDrive.setDirection(DcMotor.Direction.REVERSE); + backRightDrive.setDirection(DcMotor.Direction.FORWARD); + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); } @@ -155,8 +155,8 @@ public class Autonomoustest extends LinearOpMode { { leftDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.REVERSE); - backrightDrive.setDirection(DcMotor.Direction.REVERSE); - backleftDrive.setDirection(DcMotor.Direction.FORWARD); + backRightDrive.setDirection(DcMotor.Direction.REVERSE); + backLeftDrive.setDirection(DcMotor.Direction.FORWARD); encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); } @@ -164,8 +164,8 @@ public class Autonomoustest extends LinearOpMode { { leftDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD); - backrightDrive.setDirection(DcMotor.Direction.REVERSE); - backleftDrive.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); } @@ -173,8 +173,8 @@ public class Autonomoustest extends LinearOpMode { 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); + 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); } @@ -209,8 +209,8 @@ public class Autonomoustest extends LinearOpMode { { 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"); + 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"); @@ -223,21 +223,21 @@ public class Autonomoustest extends LinearOpMode { // 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); + 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); + 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); + backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } public void testWrist() @@ -370,25 +370,25 @@ public class Autonomoustest extends LinearOpMode { // 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); + 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); + 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); + 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)); + 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 @@ -398,27 +398,27 @@ public class Autonomoustest extends LinearOpMode { // onto the next step, use (isBusy() || isBusy()) in the loop test. while (opModeIsActive() && (runtime.seconds() < timeoutS) && - (leftDrive.isBusy() && rightDrive.isBusy() && backleftDrive.isBusy() && backrightDrive.isBusy())) { + (leftDrive.isBusy() && rightDrive.isBusy() && backLeftDrive.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()); + leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backRightDrive.getCurrentPosition(), backLeftDrive.getCurrentPosition()); telemetry.update(); } leftDrive.setPower(0); rightDrive.setPower(0); - backrightDrive.setPower(0); - backleftDrive.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); + backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); sleep(250); // optional pause after each move. } @@ -426,14 +426,14 @@ public class Autonomoustest extends LinearOpMode { public void armEncoder(double speed, double Inches, double timeoutS) { - int newarmTarget; + 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); + newArmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH); + arm.setTargetPosition(newArmTarget); // Turn On RUN_TO_POSITION arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); @@ -453,7 +453,7 @@ public class Autonomoustest extends LinearOpMode { (arm.isBusy())) { // Display it for the driver. - telemetry.addData("Running to", " %7d", newarmTarget); + telemetry.addData("Running to", " %7d", newArmTarget); telemetry.addData("Currently at", " at %7d", arm.getCurrentPosition()); telemetry.update();