From f61b1d50550fd2124c5bbf11860a7d0fced4665b Mon Sep 17 00:00:00 2001 From: ftcwires <117595165+ftcwires@users.noreply.github.com> Date: Wed, 25 Oct 2023 12:03:55 -0500 Subject: [PATCH 001/154] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 319e735bbe..9d77cca484 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ # FTC WIRES Autonomous based on Road Runner 1.8 For instructions and documentation, please register at https://forms.gle/mBFYMgsE5pH3QBXa9 - +Details at http://www.ftcwires.org/softwareplatform From 841df09db1a69f4fdbce59d4c1e107f5f5ee90cf Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 28 Oct 2023 13:01:11 -0600 Subject: [PATCH 002/154] New island!!!! --- .../ftc/teamcode/Beginnings.java | 113 ++++++++++++++++++ .../ftc/teamcode/apriltagDimitri.java | 16 +++ 2 files changed, 129 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/apriltagDimitri.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java new file mode 100644 index 0000000000..0f4755323c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -0,0 +1,113 @@ +package org.firstinspires.ftc.teamcode; + +// imports + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; + +@TeleOp +public class Beginnings extends LinearOpMode { + // Declare vars + DcMotor BackLeft; + DcMotor BackRight; + DcMotor FrontLeft; + DcMotor FrontRight; + Servo LiftRight; + Servo LiftLeft; + + // Servo prep + + double MinLiftHeight = 0.05; + double MaxLiftHeight = 0.65; + double LiftLeftOffset = -0.08; + double LiftHeight; + private AndroidTextToSpeech androidTextToSpeech; + + + + // Functions \/ + + private void drive_code() { + + double Scale_Factor_of_Drive; + + if (gamepad1.right_bumper) { + Scale_Factor_of_Drive = 1; + } else { + Scale_Factor_of_Drive = 0.55; + } + // drive with joysticks + BackLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + BackRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); + FrontLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); + FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + telemetry.addData("FL Motor", FrontLeft.getPower()); + telemetry.addData("FR Motor", FrontRight.getPower()); + telemetry.addData("BL Motor", BackLeft.getPower()); + telemetry.addData("BR Motor", BackRight.getPower()); + + } + + private void servo_shenanigans() { + setLiftHeight(0.4); + sleep(10000); + setLiftHeight(0.05); + } + + private void setLiftHeight(double inputLiftHeight) { + if (inputLiftHeight < 0.05){ + inputLiftHeight = 0.05; + } + if (inputLiftHeight > 0.65){ + inputLiftHeight = 0.65; + } + LiftHeight = inputLiftHeight; + LiftLeft.setPosition(LiftLeftOffset + LiftHeight); + LiftRight.setPosition(LiftHeight); + + } + + + @Override + // NOT loop \/ - Or int of vars + public void runOpMode() throws InterruptedException { + + androidTextToSpeech = new AndroidTextToSpeech(); + androidTextToSpeech.initialize(); + + BackLeft = hardwareMap.get(DcMotor.class, "BackLeft"); + BackRight = hardwareMap.get(DcMotor.class, "BackRight"); + FrontLeft = hardwareMap.get(DcMotor.class, "FrontLeft"); + FrontRight = hardwareMap.get(DcMotor.class, "FrontRight"); + LiftRight = hardwareMap.get(Servo.class, "LiftRight"); + LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); + + LiftRight.setDirection(Servo.Direction.REVERSE); + + + // init servo hardware + // init drive hardware and variables + BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + FrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + FrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + BackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + + + + waitForStart(); + servo_shenanigans(); + // loop real + while(opModeIsActive()){ + //drive_code(); + telemetry.update(); + sleep(100); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/apriltagDimitri.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/apriltagDimitri.java new file mode 100644 index 0000000000..b9fe01ac11 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/apriltagDimitri.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +@TeleOp +public class apriltagDimitri extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + waitForStart(); + while (opModeIsActive()){} + + + + } +} From f2da2ba411b3f7cdc80ff8fec831d4528ce1b8f8 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Sat, 28 Oct 2023 13:16:06 -0600 Subject: [PATCH 003/154] its the tape code im working on --- .../ftc/teamcode/TheJettCode.java | 52 +++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java new file mode 100644 index 0000000000..2aeb8c347c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +@TeleOp +public class TheJettCode extends LinearOpMode { + private CRServo Tape; + double TapePower; + private void tapetroll() { + if (gamepad1.a) { + TapePower = 0.7; + Tape.setPower(TapePower); + sleep(570); + TapePower = 0; + Tape.setPower(TapePower); + } + + } + + private void taper() { + if (gamepad1.y) { + TapePower = -0.7; + } else if (gamepad1.x) { + TapePower = 0.7; + } else { + TapePower = 0; + } + Tape.setPower(TapePower); + telemetry.addData("TAPE", Tape.getPower()); + telemetry.addData("TAPE non var", TapePower); + } + @Override + public void runOpMode() throws InterruptedException { + + + Tape = hardwareMap.get(CRServo.class, "Tape"); + Tape.setDirection(DcMotorSimple.Direction.REVERSE); + waitForStart(); + while (opModeIsActive()) { + taper(); + tapetroll(); + telemetry.update(); + } + + } + + + +} \ No newline at end of file From 7952741e74571fef3f4b3b507a9fdcaa1964ac97 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 28 Oct 2023 13:42:59 -0600 Subject: [PATCH 004/154] Testing step 4.1 --- .../org/firstinspires/ftc/teamcode/MecanumDrive.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index cb21a18517..ab1d9d4bc2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -200,8 +200,8 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { //TODO Step 1 Drive Classes : get basic hardware configured. Update motor names to what is used in robot configuration leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftBack = hardwareMap.get(DcMotorEx.class, "leftBack"); - rightBack = hardwareMap.get(DcMotorEx.class, "rightBack"); + leftBack = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightBack = hardwareMap.get(DcMotorEx.class, "rightRear"); rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); //TODO End Step 1 @@ -221,10 +221,11 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); imu = hardwareMap.get(IMU.class, "imu"); + // //TODO Step 2 : Update direction of IMU by updating orientation of Driver Hub below IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.UP, // Change to UP / DOWN / LEFT / RIGHT / FORWARD / BACKWARD as in robot - RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)); //Change to UP / DOWN / LEFT / RIGHT / FORWARD / BACKWARD + RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, // Change to UP / DOWN / LEFT / RIGHT / FORWARD / BACKWARD as in robot + RevHubOrientationOnRobot.UsbFacingDirection.UP)); //Change to UP / DOWN / LEFT / RIGHT / FORWARD / BACKWARD imu.initialize(parameters); //TODO End Step 2 From 4d221e83a3711f52760bf6eeaf2af58486ed2892 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 28 Oct 2023 14:55:07 -0600 Subject: [PATCH 005/154] Orignal values for testing --- .../ftc/teamcode/MecanumDrive.java | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index ab1d9d4bc2..c3aee97468 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -51,22 +51,22 @@ public static class Params { // drive model parameters //TODO Step 5 Set value of inPerTick after running ForwardPushTest //TODO Step 14 Make value of inPerTick accurate after running LocalizationTest - public double inPerTick = 0; + public double inPerTick = -0.022622811513096; //TODO Step 6 (Only for DriveEncoder Localizer) Set value of lateralInPerTick after running LateralPushTest //TODO Step 8 (Only for DeadWheel Localizer) Set value of lateralInPerTick after running LateralRampLogger //TODO Step 14 Make value of lateralInPerTick accurate after running LocalizationTest - public double lateralInPerTick = 1; + public double lateralInPerTick = .022778; //TODO Step 10 (Only for DriveEncoder Localizer) Set value of trackWidthTicks after running AngularRampLogger //TODO Step 11 (Only for DeadWheel Localizer) Set value of trackWidthTicks after running AngularRampLogger - public double trackWidthTicks = 0; + public double trackWidthTicks = -1000.143754986; // feedforward parameters (in tick units) //TODO Step 7 (Only for DeadWheel Localizer) Set value for kS and KV after running ForwardRampLogger //TODO Step 9 (Only for DriveEncoder Localizer) Set value for kS and kV after running AngularRampLogger - public double kS = 0; - public double kV = 0; + public double kS = 1.0182010371577; + public double kV = -0.004333430259; //TODO Step 12 Set value of kA after running ManualFeedforwardTuner. In this emperical process update value in increments of 0.0001 public double kA = 0; @@ -132,10 +132,10 @@ public DriveLocalizer() { //TODO Step 4.2 Run MecanumDirectionDebugger Tuning OpMode to set motor direction correctly //Uncomment the lines for which the motorDirection need to be reversed to ensure all motors run forward in test - //leftFront.setDirection(DcMotorEx.Direction.REVERSE); - //leftBack.setDirection(DcMotorEx.Direction.REVERSE); - rightBack.setDirection(DcMotorEx.Direction.REVERSE); - rightFront.setDirection(DcMotorEx.Direction.REVERSE); + leftFront.setDirection(DcMotorEx.Direction.REVERSE); + leftBack.setDirection(DcMotorEx.Direction.REVERSE); + //rightBack.setDirection(DcMotorEx.Direction.REVERSE); + //rightFront.setDirection(DcMotorEx.Direction.REVERSE); //TODO End Step 4.2 lastLeftFrontPos = leftFront.getPositionAndVelocity().position; @@ -207,10 +207,10 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { //TODO Step 4.1 Run MecanumDirectionDebugger Tuning OpMode to set motor direction correctly //Uncomment the lines for which the motorDirection need to be reversed to ensure all motors run forward in test - //leftFront.setDirection(DcMotorEx.Direction.REVERSE); - //leftBack.setDirection(DcMotorEx.Direction.REVERSE); - rightFront.setDirection(DcMotorEx.Direction.REVERSE); - rightBack.setDirection(DcMotorEx.Direction.REVERSE); + leftFront.setDirection(DcMotorEx.Direction.REVERSE); + leftBack.setDirection(DcMotorEx.Direction.REVERSE); + //rightFront.setDirection(DcMotorEx.Direction.REVERSE); + //rightBack.setDirection(DcMotorEx.Direction.REVERSE); //TODO Make the same update in DriveLocalizer() function. Search for Step 4.2 //TODO End Step 4.1 From 3e2db99f0f3b4dd53eaf77e579fcfd10050ca71a Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 28 Oct 2023 16:11:20 -0600 Subject: [PATCH 006/154] New Ka value --- .../java/org/firstinspires/ftc/teamcode/MecanumDrive.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index c3aee97468..a393a0e6dc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -51,7 +51,7 @@ public static class Params { // drive model parameters //TODO Step 5 Set value of inPerTick after running ForwardPushTest //TODO Step 14 Make value of inPerTick accurate after running LocalizationTest - public double inPerTick = -0.022622811513096; + public double inPerTick = 0.022622811513096; //TODO Step 6 (Only for DriveEncoder Localizer) Set value of lateralInPerTick after running LateralPushTest //TODO Step 8 (Only for DeadWheel Localizer) Set value of lateralInPerTick after running LateralRampLogger @@ -60,16 +60,16 @@ public static class Params { //TODO Step 10 (Only for DriveEncoder Localizer) Set value of trackWidthTicks after running AngularRampLogger //TODO Step 11 (Only for DeadWheel Localizer) Set value of trackWidthTicks after running AngularRampLogger - public double trackWidthTicks = -1000.143754986; + public double trackWidthTicks = 1000.143754986; // feedforward parameters (in tick units) //TODO Step 7 (Only for DeadWheel Localizer) Set value for kS and KV after running ForwardRampLogger //TODO Step 9 (Only for DriveEncoder Localizer) Set value for kS and kV after running AngularRampLogger public double kS = 1.0182010371577; - public double kV = -0.004333430259; + public double kV = 0.004333430259; //TODO Step 12 Set value of kA after running ManualFeedforwardTuner. In this emperical process update value in increments of 0.0001 - public double kA = 0; + public double kA = 0.00025; // path profile parameters (in inches) public double maxWheelVel = 50; From 11ea6066fb99adb66d4602463c420f6625915578 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 3 Nov 2023 16:54:20 -0600 Subject: [PATCH 007/154] Saving before adding begginings --- .../main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index a393a0e6dc..77def0e455 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -56,7 +56,7 @@ public static class Params { //TODO Step 6 (Only for DriveEncoder Localizer) Set value of lateralInPerTick after running LateralPushTest //TODO Step 8 (Only for DeadWheel Localizer) Set value of lateralInPerTick after running LateralRampLogger //TODO Step 14 Make value of lateralInPerTick accurate after running LocalizationTest - public double lateralInPerTick = .022778; + public double lateralInPerTick = 0.022778; //TODO Step 10 (Only for DriveEncoder Localizer) Set value of trackWidthTicks after running AngularRampLogger //TODO Step 11 (Only for DeadWheel Localizer) Set value of trackWidthTicks after running AngularRampLogger From 02ea1ab34e5ed07cc79006b78e992757e5cfec5e Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 3 Nov 2023 17:30:12 -0600 Subject: [PATCH 008/154] Added Wires drive --- .../ftc/teamcode/Beginnings.java | 88 +++++++++++++++++-- 1 file changed, 82 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 0f4755323c..4ec6ae3161 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -2,6 +2,9 @@ // imports +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.PoseVelocity2d; +import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -9,6 +12,7 @@ import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; +import org.firstinspires.ftc.teamcode.tuning.TuningOpModes; @TeleOp public class Beginnings extends LinearOpMode { @@ -32,7 +36,7 @@ public class Beginnings extends LinearOpMode { // Functions \/ - private void drive_code() { + private void og_drive_code() { double Scale_Factor_of_Drive; @@ -53,6 +57,7 @@ private void drive_code() { } + private void servo_shenanigans() { setLiftHeight(0.4); sleep(10000); @@ -72,6 +77,76 @@ private void setLiftHeight(double inputLiftHeight) { } + private void intakeFunction() { + if (gamepad1.right_trigger > 0.5) { + // Intake.setPower(-1); + telemetry.addData("right trig pressed", "yes"); + } + else { + telemetry.addData("right trig not pressed", "yes"); + } + } + + private void driveCode() { + double SLOW_DOWN_FACTOR = 0.5; + telemetry.addData("Initializing FTC Wires (ftcwires.org) TeleOp adopted for Team:","TEAM NUMBER"); + telemetry.update(); + + if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + waitForStart(); + + while (opModeIsActive()) { + telemetry.addData("Running FTC Wires (ftcwires.org) TeleOp Mode adopted for Team:","TEAM NUMBER"); + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y * SLOW_DOWN_FACTOR, + -gamepad1.left_stick_x * SLOW_DOWN_FACTOR + ), + -gamepad1.right_stick_x * SLOW_DOWN_FACTOR + )); + + drive.updatePoseEstimate(); + + //telemetry.addData("LF Encoder", drive.leftFront.getCurrentPosition()); + //telemetry.addData("LB Encoder", drive.leftBack.getCurrentPosition()); + //telemetry.addData("RF Encoder", drive.rightFront.getCurrentPosition()); + //telemetry.addData("RB Encoder", drive.rightBack.getCurrentPosition()); + + telemetry.addLine("Current Pose"); + telemetry.addData("x", drive.pose.position.x); + telemetry.addData("y", drive.pose.position.y); + telemetry.addData("heading", Math.toDegrees(drive.pose.heading.log())); + telemetry.update(); + } + } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) { + TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); + + waitForStart(); + + while (opModeIsActive()) { + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y * SLOW_DOWN_FACTOR, + 0.0 + ), + -gamepad1.right_stick_x * SLOW_DOWN_FACTOR + )); + + drive.updatePoseEstimate(); + + telemetry.addData("x", drive.pose.position.x); + telemetry.addData("y", drive.pose.position.y); + telemetry.addData("heading", drive.pose.heading); + telemetry.update(); + } + } else { + throw new AssertionError(); + } + } + + @Override // NOT loop \/ - Or int of vars @@ -80,10 +155,10 @@ public void runOpMode() throws InterruptedException { androidTextToSpeech = new AndroidTextToSpeech(); androidTextToSpeech.initialize(); - BackLeft = hardwareMap.get(DcMotor.class, "BackLeft"); - BackRight = hardwareMap.get(DcMotor.class, "BackRight"); - FrontLeft = hardwareMap.get(DcMotor.class, "FrontLeft"); - FrontRight = hardwareMap.get(DcMotor.class, "FrontRight"); + BackLeft = hardwareMap.get(DcMotor.class, "leftRear"); + BackRight = hardwareMap.get(DcMotor.class, "rightRear"); + FrontLeft = hardwareMap.get(DcMotor.class, "leftFront"); + FrontRight = hardwareMap.get(DcMotor.class, "rightFront"); LiftRight = hardwareMap.get(Servo.class, "LiftRight"); LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); @@ -105,7 +180,8 @@ public void runOpMode() throws InterruptedException { servo_shenanigans(); // loop real while(opModeIsActive()){ - //drive_code(); + driveCode(); + intakeFunction(); telemetry.update(); sleep(100); } From 58a8492695344d9536257099e87eb7f3d0e00750 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Fri, 3 Nov 2023 17:42:45 -0600 Subject: [PATCH 009/154] i need to update the code for everyone yes yes --- .../ftc/teamcode/TheJettCode.java | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java index 2aeb8c347c..19c9dd29ac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java @@ -4,44 +4,44 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; @TeleOp public class TheJettCode extends LinearOpMode { - private CRServo Tape; - double TapePower; - private void tapetroll() { + private Servo LiftLeft; + double LiftHeight; + private void ServoNo() { if (gamepad1.a) { - TapePower = 0.7; - Tape.setPower(TapePower); + LiftHeight = 0.7; + LiftLeft.setPosition(LiftHeight); sleep(570); - TapePower = 0; - Tape.setPower(TapePower); + LiftHeight = 0; + LiftLeft.setPosition(LiftHeight); } } - private void taper() { + private void ServoYes() { if (gamepad1.y) { - TapePower = -0.7; + LiftHeight = -0.7; } else if (gamepad1.x) { - TapePower = 0.7; + LiftHeight = 0.7; } else { - TapePower = 0; + LiftHeight = 0; } - Tape.setPower(TapePower); - telemetry.addData("TAPE", Tape.getPower()); - telemetry.addData("TAPE non var", TapePower); + LiftLeft.setPosition(LiftHeight); + telemetry.addData("TAPE", LiftLeft.getPosition()); + telemetry.addData("TAPE non var", LiftHeight); } @Override public void runOpMode() throws InterruptedException { - Tape = hardwareMap.get(CRServo.class, "Tape"); - Tape.setDirection(DcMotorSimple.Direction.REVERSE); + LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); waitForStart(); while (opModeIsActive()) { - taper(); - tapetroll(); + ServoYes(); + ServoNo(); telemetry.update(); } From cd9acfd14d67a589c6f883ce98372c8f9df96819 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 3 Nov 2023 17:50:28 -0600 Subject: [PATCH 010/154] Made Wires drive into a function -goober --- .../ftc/teamcode/Beginnings.java | 87 +++++++------------ 1 file changed, 30 insertions(+), 57 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 4ec6ae3161..1a6a51f154 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -17,6 +17,8 @@ @TeleOp public class Beginnings extends LinearOpMode { // Declare vars + + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); DcMotor BackLeft; DcMotor BackRight; DcMotor FrontLeft; @@ -88,62 +90,31 @@ private void intakeFunction() { } private void driveCode() { - double SLOW_DOWN_FACTOR = 0.5; - telemetry.addData("Initializing FTC Wires (ftcwires.org) TeleOp adopted for Team:","TEAM NUMBER"); + + double SLOW_DOWN_FACTOR; + SLOW_DOWN_FACTOR = 0.5; + telemetry.addData("Running FTC Wires (ftcwires.org) TeleOp Mode adopted for Team:","TEAM NUMBER"); + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y * SLOW_DOWN_FACTOR, + -gamepad1.left_stick_x * SLOW_DOWN_FACTOR + ), + -gamepad1.right_stick_x * SLOW_DOWN_FACTOR + )); + + drive.updatePoseEstimate(); + + //telemetry.addData("LF Encoder", drive.leftFront.getCurrentPosition()); + //telemetry.addData("LB Encoder", drive.leftBack.getCurrentPosition()); + //telemetry.addData("RF Encoder", drive.rightFront.getCurrentPosition()); + //telemetry.addData("RB Encoder", drive.rightBack.getCurrentPosition()); + + telemetry.addLine("Current Pose"); + telemetry.addData("x", drive.pose.position.x); + telemetry.addData("y", drive.pose.position.y); + telemetry.addData("heading", Math.toDegrees(drive.pose.heading.log())); telemetry.update(); - if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { - MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); - - waitForStart(); - - while (opModeIsActive()) { - telemetry.addData("Running FTC Wires (ftcwires.org) TeleOp Mode adopted for Team:","TEAM NUMBER"); - drive.setDrivePowers(new PoseVelocity2d( - new Vector2d( - -gamepad1.left_stick_y * SLOW_DOWN_FACTOR, - -gamepad1.left_stick_x * SLOW_DOWN_FACTOR - ), - -gamepad1.right_stick_x * SLOW_DOWN_FACTOR - )); - - drive.updatePoseEstimate(); - - //telemetry.addData("LF Encoder", drive.leftFront.getCurrentPosition()); - //telemetry.addData("LB Encoder", drive.leftBack.getCurrentPosition()); - //telemetry.addData("RF Encoder", drive.rightFront.getCurrentPosition()); - //telemetry.addData("RB Encoder", drive.rightBack.getCurrentPosition()); - - telemetry.addLine("Current Pose"); - telemetry.addData("x", drive.pose.position.x); - telemetry.addData("y", drive.pose.position.y); - telemetry.addData("heading", Math.toDegrees(drive.pose.heading.log())); - telemetry.update(); - } - } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) { - TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); - - waitForStart(); - - while (opModeIsActive()) { - drive.setDrivePowers(new PoseVelocity2d( - new Vector2d( - -gamepad1.left_stick_y * SLOW_DOWN_FACTOR, - 0.0 - ), - -gamepad1.right_stick_x * SLOW_DOWN_FACTOR - )); - - drive.updatePoseEstimate(); - - telemetry.addData("x", drive.pose.position.x); - telemetry.addData("y", drive.pose.position.y); - telemetry.addData("heading", drive.pose.heading); - telemetry.update(); - } - } else { - throw new AssertionError(); - } } @@ -162,7 +133,7 @@ public void runOpMode() throws InterruptedException { LiftRight = hardwareMap.get(Servo.class, "LiftRight"); LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); - LiftRight.setDirection(Servo.Direction.REVERSE); + // LiftRight.setDirection(Servo.Direction.REVERSE); only for og drive code // init servo hardware @@ -173,11 +144,13 @@ public void runOpMode() throws InterruptedException { FrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); BackRight.setDirection(DcMotorSimple.Direction.REVERSE); - + double SLOW_DOWN_FACTOR = 0.5; + telemetry.addData("Initializing TeleOp",""); + telemetry.update(); waitForStart(); - servo_shenanigans(); + // servo_shenanigans(); // loop real while(opModeIsActive()){ driveCode(); From 9c1690b0a89d6671b6cd595e772da320f273b344 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 3 Nov 2023 18:15:56 -0600 Subject: [PATCH 011/154] Goodbye Scorpion, you have served us well. We appreciated you teaching us the ways of roadrunner and learning android studio. We will never forget your sacrifice, and you will always be remembered as our favorite bug. We could not have gotten to the point where we are without you. You will be missed. We are moving on to Lightning McGreen. Goodbye. o7 -goober --- .../java/org/firstinspires/ftc/teamcode/Beginnings.java | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 1a6a51f154..bc89bc983b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -18,7 +18,7 @@ public class Beginnings extends LinearOpMode { // Declare vars - MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + MecanumDrive drive; DcMotor BackLeft; DcMotor BackRight; DcMotor FrontLeft; @@ -126,6 +126,9 @@ public void runOpMode() throws InterruptedException { androidTextToSpeech = new AndroidTextToSpeech(); androidTextToSpeech.initialize(); + // for wires driving + drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + /* all for OG driving BackLeft = hardwareMap.get(DcMotor.class, "leftRear"); BackRight = hardwareMap.get(DcMotor.class, "rightRear"); FrontLeft = hardwareMap.get(DcMotor.class, "leftFront"); @@ -133,7 +136,8 @@ public void runOpMode() throws InterruptedException { LiftRight = hardwareMap.get(Servo.class, "LiftRight"); LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); - // LiftRight.setDirection(Servo.Direction.REVERSE); only for og drive code + + LiftRight.setDirection(Servo.Direction.REVERSE); only for og drive code // init servo hardware @@ -143,6 +147,7 @@ public void runOpMode() throws InterruptedException { FrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); FrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); BackRight.setDirection(DcMotorSimple.Direction.REVERSE); + */ double SLOW_DOWN_FACTOR = 0.5; telemetry.addData("Initializing TeleOp",""); From ca1a9eeaba9c492118efceeb5b867f96656fb22d Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 3 Nov 2023 18:54:03 -0600 Subject: [PATCH 012/154] Adding Servos for new robot -goober --- .../ftc/teamcode/Beginnings.java | 29 ++++++++++++++++--- .../ftc/teamcode/MecanumDrive.java | 4 +-- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index bc89bc983b..6f1afadb32 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -18,13 +18,18 @@ public class Beginnings extends LinearOpMode { // Declare vars + private Servo launcher; + private Servo rightLift; + private Servo leftLift; + private Servo shoulder; + private Servo wrist; + private Servo hopper; MecanumDrive drive; DcMotor BackLeft; DcMotor BackRight; DcMotor FrontLeft; DcMotor FrontRight; - Servo LiftRight; - Servo LiftLeft; + // Servo prep @@ -74,8 +79,8 @@ private void setLiftHeight(double inputLiftHeight) { inputLiftHeight = 0.65; } LiftHeight = inputLiftHeight; - LiftLeft.setPosition(LiftLeftOffset + LiftHeight); - LiftRight.setPosition(LiftHeight); + leftLift.setPosition(LiftLeftOffset + LiftHeight); + rightLift.setPosition(LiftHeight); } @@ -128,6 +133,13 @@ public void runOpMode() throws InterruptedException { // for wires driving drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + launcher = hardwareMap.get(Servo.class, "launcher"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + shoulder = hardwareMap.get(Servo.class, "shoulder"); + wrist = hardwareMap.get(Servo.class, "wrist"); + hopper = hardwareMap.get(Servo.class, "hopper"); /* all for OG driving BackLeft = hardwareMap.get(DcMotor.class, "leftRear"); BackRight = hardwareMap.get(DcMotor.class, "rightRear"); @@ -149,6 +161,15 @@ public void runOpMode() throws InterruptedException { BackRight.setDirection(DcMotorSimple.Direction.REVERSE); */ + // servos + launcher.setPosition(0.8); + leftLift.setPosition(0); + rightLift.setPosition(0); + shoulder.setPosition(0.1); + wrist.setPosition(-0.04); + hopper.setPosition(0.01); + + double SLOW_DOWN_FACTOR = 0.5; telemetry.addData("Initializing TeleOp",""); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 77def0e455..55110c6484 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -200,8 +200,8 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { //TODO Step 1 Drive Classes : get basic hardware configured. Update motor names to what is used in robot configuration leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftBack = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightBack = hardwareMap.get(DcMotorEx.class, "rightRear"); + leftBack = hardwareMap.get(DcMotorEx.class, "leftBack"); + rightBack = hardwareMap.get(DcMotorEx.class, "rightBack"); rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); //TODO End Step 1 From 811270288b68b54fdbda0204cce952c05e374edb Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 3 Nov 2023 19:13:56 -0600 Subject: [PATCH 013/154] Added Intake code -goober --- .../ftc/teamcode/Beginnings.java | 24 +++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 6f1afadb32..981bafd395 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -29,6 +29,8 @@ public class Beginnings extends LinearOpMode { DcMotor BackRight; DcMotor FrontLeft; DcMotor FrontRight; + DcMotor frontIntake; + DcMotor rearIntake; // Servo prep @@ -86,11 +88,14 @@ private void setLiftHeight(double inputLiftHeight) { private void intakeFunction() { if (gamepad1.right_trigger > 0.5) { - // Intake.setPower(-1); - telemetry.addData("right trig pressed", "yes"); + frontIntake.setPower(0.5); + rearIntake.setPower(0.5); + telemetry.addData("Intake", "yes"); } else { - telemetry.addData("right trig not pressed", "yes"); + telemetry.addData("Intake", "no"); + frontIntake.setPower(0); + rearIntake.setPower(0); } } @@ -118,7 +123,7 @@ private void driveCode() { telemetry.addData("x", drive.pose.position.x); telemetry.addData("y", drive.pose.position.y); telemetry.addData("heading", Math.toDegrees(drive.pose.heading.log())); - telemetry.update(); + // telemetry.update(); } @@ -140,6 +145,17 @@ public void runOpMode() throws InterruptedException { shoulder = hardwareMap.get(Servo.class, "shoulder"); wrist = hardwareMap.get(Servo.class, "wrist"); hopper = hardwareMap.get(Servo.class, "hopper"); + + frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); + rearIntake = hardwareMap.get(DcMotor.class, "rearIntake"); + + frontIntake.setDirection(DcMotorSimple.Direction.FORWARD); + rearIntake.setDirection(DcMotorSimple.Direction.REVERSE); + frontIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + /* all for OG driving BackLeft = hardwareMap.get(DcMotor.class, "leftRear"); BackRight = hardwareMap.get(DcMotor.class, "rightRear"); From 2481f68a2e9b5f6c44f6b202427c61a9cb430931 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 3 Nov 2023 19:24:04 -0600 Subject: [PATCH 014/154] Fixed intaking out outputing code -goober --- .../firstinspires/ftc/teamcode/Beginnings.java | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 981bafd395..b2da714d3d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -88,17 +88,24 @@ private void setLiftHeight(double inputLiftHeight) { private void intakeFunction() { if (gamepad1.right_trigger > 0.5) { - frontIntake.setPower(0.5); - rearIntake.setPower(0.5); - telemetry.addData("Intake", "yes"); + frontIntake.setPower(1); + rearIntake.setPower(1); + telemetry.addData("Intake", "in"); + } + else if (gamepad1.right_bumper) { + frontIntake.setPower(-1); + rearIntake.setPower(-1); + telemetry.addData("Intake", "out"); } else { - telemetry.addData("Intake", "no"); + telemetry.addData("Intake", "stopped"); frontIntake.setPower(0); rearIntake.setPower(0); } + } + private void driveCode() { double SLOW_DOWN_FACTOR; From 4c39f86d8cc29416f05371ad466bee2694bf9097 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 4 Nov 2023 10:48:52 -0600 Subject: [PATCH 015/154] Refactored og drive -goober --- .../ftc/teamcode/Beginnings.java | 52 ++-------------- .../firstinspires/ftc/teamcode/OgDrive.java | 62 +++++++++++++++++++ 2 files changed, 68 insertions(+), 46 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index b2da714d3d..7f64f0b2ab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -9,6 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; @@ -25,10 +26,7 @@ public class Beginnings extends LinearOpMode { private Servo wrist; private Servo hopper; MecanumDrive drive; - DcMotor BackLeft; - DcMotor BackRight; - DcMotor FrontLeft; - DcMotor FrontRight; + OgDrive ogDrive; DcMotor frontIntake; DcMotor rearIntake; @@ -45,27 +43,6 @@ public class Beginnings extends LinearOpMode { // Functions \/ - private void og_drive_code() { - - double Scale_Factor_of_Drive; - - if (gamepad1.right_bumper) { - Scale_Factor_of_Drive = 1; - } else { - Scale_Factor_of_Drive = 0.55; - } - // drive with joysticks - BackLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); - BackRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); - FrontLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); - FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); - telemetry.addData("FL Motor", FrontLeft.getPower()); - telemetry.addData("FR Motor", FrontRight.getPower()); - telemetry.addData("BL Motor", BackLeft.getPower()); - telemetry.addData("BR Motor", BackRight.getPower()); - - } - private void servo_shenanigans() { setLiftHeight(0.4); @@ -146,6 +123,9 @@ public void runOpMode() throws InterruptedException { // for wires driving drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + // for Og driving (the best driving) + ogDrive = new OgDrive(hardwareMap); + launcher = hardwareMap.get(Servo.class, "launcher"); rightLift = hardwareMap.get(Servo.class, "rightLift"); leftLift = hardwareMap.get(Servo.class, "leftLift"); @@ -163,27 +143,6 @@ public void runOpMode() throws InterruptedException { rearIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); frontIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - /* all for OG driving - BackLeft = hardwareMap.get(DcMotor.class, "leftRear"); - BackRight = hardwareMap.get(DcMotor.class, "rightRear"); - FrontLeft = hardwareMap.get(DcMotor.class, "leftFront"); - FrontRight = hardwareMap.get(DcMotor.class, "rightFront"); - LiftRight = hardwareMap.get(Servo.class, "LiftRight"); - LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); - - - LiftRight.setDirection(Servo.Direction.REVERSE); only for og drive code - - - // init servo hardware - // init drive hardware and variables - BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - FrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - FrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - BackRight.setDirection(DcMotorSimple.Direction.REVERSE); - */ - // servos launcher.setPosition(0.8); leftLift.setPosition(0); @@ -203,6 +162,7 @@ public void runOpMode() throws InterruptedException { // loop real while(opModeIsActive()){ driveCode(); + // ogDrive.og_drive_code(gamepad1, telemetry); intakeFunction(); telemetry.update(); sleep(100); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java new file mode 100644 index 0000000000..b2c5f279eb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + + +public class OgDrive { + DcMotor BackLeft; + DcMotor BackRight; + DcMotor FrontLeft; + DcMotor FrontRight; + Servo LiftRight; + Servo LiftLeft; + + public OgDrive(HardwareMap hardwareMap) { + + BackLeft = hardwareMap.get(DcMotor.class, "leftRear"); + BackRight = hardwareMap.get(DcMotor.class, "rightRear"); + FrontLeft = hardwareMap.get(DcMotor.class, "leftFront"); + FrontRight = hardwareMap.get(DcMotor.class, "rightFront"); + LiftRight = hardwareMap.get(Servo.class, "LiftRight"); + LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); + + + LiftRight.setDirection(Servo.Direction.REVERSE); + + + // init servo hardware + // init drive hardware and variables + BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + FrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + FrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + BackRight.setDirection(DcMotorSimple.Direction.REVERSE); + } + public void og_drive_code(Gamepad gamepad1, Telemetry telemetry) { + + double Scale_Factor_of_Drive; + + if (gamepad1.right_bumper) { + Scale_Factor_of_Drive = 1; + } else { + Scale_Factor_of_Drive = 0.55; + } + // drive with joysticks + BackLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + BackRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); + FrontLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); + FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + telemetry.addData("FL Motor", FrontLeft.getPower()); + telemetry.addData("FR Motor", FrontRight.getPower()); + telemetry.addData("BL Motor", BackLeft.getPower()); + telemetry.addData("BR Motor", BackRight.getPower()); + + } +} From 50e189a1e433c0bf35e05c4b7e060a02d2719603 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 4 Nov 2023 11:54:43 -0600 Subject: [PATCH 016/154] Lift and airplane code -goober --- .../ftc/teamcode/Beginnings.java | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 7f64f0b2ab..7ab48dea61 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -37,6 +37,7 @@ public class Beginnings extends LinearOpMode { double MaxLiftHeight = 0.65; double LiftLeftOffset = -0.08; double LiftHeight; + double LiftOffset = 0; private AndroidTextToSpeech androidTextToSpeech; @@ -63,6 +64,32 @@ private void setLiftHeight(double inputLiftHeight) { } + private void airplane() { + if (gamepad2.dpad_down = true) { + launcher.setPosition(0.3); + } + else { + launcher.setPosition(0); + } + } + private void liftFunction() { + if (gamepad2.y) { + leftLift.setPosition(0.6 + LiftOffset); + rightLift.setPosition(0.6); + } + else if (gamepad2.x) { + leftLift.setPosition(0.4 + LiftOffset); + rightLift.setPosition(0.4); + } + else if (gamepad2.b) { + leftLift.setPosition(0.2 + LiftOffset); + rightLift.setPosition(0.2); + } + else if (gamepad2.a) { + leftLift.setPosition(0 + LiftOffset); + rightLift.setPosition(0); + } + } private void intakeFunction() { if (gamepad1.right_trigger > 0.5) { frontIntake.setPower(1); @@ -133,6 +160,8 @@ public void runOpMode() throws InterruptedException { wrist = hardwareMap.get(Servo.class, "wrist"); hopper = hardwareMap.get(Servo.class, "hopper"); + launcher.setDirection(Servo.Direction.REVERSE); + frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); rearIntake = hardwareMap.get(DcMotor.class, "rearIntake"); @@ -162,8 +191,11 @@ public void runOpMode() throws InterruptedException { // loop real while(opModeIsActive()){ driveCode(); + airplane(); // ogDrive.og_drive_code(gamepad1, telemetry); intakeFunction(); + liftFunction(); + telemetry.update(); sleep(100); } From 25c5db65ee11c818b4024a50769551ff8da1e91c Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 4 Nov 2023 12:47:17 -0600 Subject: [PATCH 017/154] this code works on McGreen -goober --- .../ftc/teamcode/Beginnings.java | 22 +++++++++---------- .../ftc/teamcode/MecanumDrive.java | 2 +- .../firstinspires/ftc/teamcode/OgDrive.java | 11 ++-------- 3 files changed, 14 insertions(+), 21 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 7ab48dea61..c365f0f5ea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -74,20 +74,20 @@ private void airplane() { } private void liftFunction() { if (gamepad2.y) { - leftLift.setPosition(0.6 + LiftOffset); - rightLift.setPosition(0.6); + leftLift.setPosition(0.9 + LiftOffset); + rightLift.setPosition(0.9); } else if (gamepad2.x) { - leftLift.setPosition(0.4 + LiftOffset); - rightLift.setPosition(0.4); + leftLift.setPosition(0.8 + LiftOffset); + rightLift.setPosition(0.8); } else if (gamepad2.b) { - leftLift.setPosition(0.2 + LiftOffset); - rightLift.setPosition(0.2); + leftLift.setPosition(0.7 + LiftOffset); + rightLift.setPosition(0.7); } else if (gamepad2.a) { - leftLift.setPosition(0 + LiftOffset); - rightLift.setPosition(0); + leftLift.setPosition(0.5 + LiftOffset); + rightLift.setPosition(0.5); } } private void intakeFunction() { @@ -156,9 +156,9 @@ public void runOpMode() throws InterruptedException { launcher = hardwareMap.get(Servo.class, "launcher"); rightLift = hardwareMap.get(Servo.class, "rightLift"); leftLift = hardwareMap.get(Servo.class, "leftLift"); - shoulder = hardwareMap.get(Servo.class, "shoulder"); wrist = hardwareMap.get(Servo.class, "wrist"); hopper = hardwareMap.get(Servo.class, "hopper"); + shoulder = hardwareMap.get(Servo.class, "shoulder"); launcher.setDirection(Servo.Direction.REVERSE); @@ -174,8 +174,8 @@ public void runOpMode() throws InterruptedException { // servos launcher.setPosition(0.8); - leftLift.setPosition(0); - rightLift.setPosition(0); + leftLift.setPosition(0.5); + rightLift.setPosition(0.5); shoulder.setPosition(0.1); wrist.setPosition(-0.04); hopper.setPosition(0.01); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 55110c6484..6bd3224a1b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -210,7 +210,7 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { leftFront.setDirection(DcMotorEx.Direction.REVERSE); leftBack.setDirection(DcMotorEx.Direction.REVERSE); //rightFront.setDirection(DcMotorEx.Direction.REVERSE); - //rightBack.setDirection(DcMotorEx.Direction.REVERSE); + rightBack.setDirection(DcMotorEx.Direction.REVERSE); //TODO Make the same update in DriveLocalizer() function. Search for Step 4.2 //TODO End Step 4.1 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java index b2c5f279eb..05bfa1e1b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java @@ -15,20 +15,13 @@ public class OgDrive { DcMotor BackRight; DcMotor FrontLeft; DcMotor FrontRight; - Servo LiftRight; - Servo LiftLeft; public OgDrive(HardwareMap hardwareMap) { - BackLeft = hardwareMap.get(DcMotor.class, "leftRear"); - BackRight = hardwareMap.get(DcMotor.class, "rightRear"); + BackLeft = hardwareMap.get(DcMotor.class, "leftBack"); + BackRight = hardwareMap.get(DcMotor.class, "rightBack"); FrontLeft = hardwareMap.get(DcMotor.class, "leftFront"); FrontRight = hardwareMap.get(DcMotor.class, "rightFront"); - LiftRight = hardwareMap.get(Servo.class, "LiftRight"); - LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); - - - LiftRight.setDirection(Servo.Direction.REVERSE); // init servo hardware From 8829534e0220eb488cdacc2b6b63f47cd34f399b Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Sat, 4 Nov 2023 12:57:44 -0600 Subject: [PATCH 018/154] i need to push so i can upload code --- .../ftc/teamcode/TheJettCode.java | 46 +++++++++---------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java index 19c9dd29ac..d4bb3cf759 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java @@ -9,44 +9,44 @@ @TeleOp public class TheJettCode extends LinearOpMode { private Servo LiftLeft; + private Servo Hopper; double LiftHeight; - private void ServoNo() { - if (gamepad1.a) { - LiftHeight = 0.7; - LiftLeft.setPosition(LiftHeight); - sleep(570); - LiftHeight = 0; - LiftLeft.setPosition(LiftHeight); - } + boolean LiftMax; + private void ServoNo() { } - private void ServoYes() { - if (gamepad1.y) { - LiftHeight = -0.7; - } else if (gamepad1.x) { - LiftHeight = 0.7; - } else { - LiftHeight = 0; - } - LiftLeft.setPosition(LiftHeight); - telemetry.addData("TAPE", LiftLeft.getPosition()); - telemetry.addData("TAPE non var", LiftHeight); - } @Override public void runOpMode() throws InterruptedException { - LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); + Hopper = hardwareMap.get(Servo.class, "Hopper"); + + LiftLeft.getController().pwmEnable(); + Hopper.getController().pwmEnable(); + waitForStart(); while (opModeIsActive()) { - ServoYes(); + if (gamepad1.y) { + LiftHeight = 0.1; + } else if (gamepad1.x) { + LiftHeight = 0.5; + } else { + LiftHeight = 0; + } + + + LiftLeft.setPosition(LiftHeight); + Hopper.setPosition(LiftHeight); + telemetry.addData("LiftLeft", LiftLeft.getPosition()); + telemetry.addData("Hopper", Hopper.getPosition()); + telemetry.addData("Lift non var", LiftHeight); ServoNo(); telemetry.update(); + sleep(100); } } - } \ No newline at end of file From d8efd3fa2a01bb81138cf631d490ad72543d9e9d Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 4 Nov 2023 13:17:55 -0600 Subject: [PATCH 019/154] hardware issue???? code works on mcgreen, laucnger broken -goober --- .../org/firstinspires/ftc/teamcode/Beginnings.java | 14 +++++++++++++- .../firstinspires/ftc/teamcode/MecanumDrive.java | 4 ++-- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index c365f0f5ea..9ab0fce877 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -69,7 +69,15 @@ private void airplane() { launcher.setPosition(0.3); } else { - launcher.setPosition(0); + launcher.setPosition(0.8); + } + } + private void extralift() { + if (gamepad2.right_bumper) { + shoulder.setPosition(0.25); + } + else { + shoulder.setPosition(0.1); } } private void liftFunction() { @@ -160,6 +168,8 @@ public void runOpMode() throws InterruptedException { hopper = hardwareMap.get(Servo.class, "hopper"); shoulder = hardwareMap.get(Servo.class, "shoulder"); + // shoulder.setDirection(Servo.Direction.REVERSE); + launcher.setDirection(Servo.Direction.REVERSE); frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); @@ -181,6 +191,7 @@ public void runOpMode() throws InterruptedException { hopper.setPosition(0.01); + double SLOW_DOWN_FACTOR = 0.5; telemetry.addData("Initializing TeleOp",""); telemetry.update(); @@ -192,6 +203,7 @@ public void runOpMode() throws InterruptedException { while(opModeIsActive()){ driveCode(); airplane(); + extralift(); // ogDrive.og_drive_code(gamepad1, telemetry); intakeFunction(); liftFunction(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 6bd3224a1b..fde6b4c694 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -209,8 +209,8 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { //Uncomment the lines for which the motorDirection need to be reversed to ensure all motors run forward in test leftFront.setDirection(DcMotorEx.Direction.REVERSE); leftBack.setDirection(DcMotorEx.Direction.REVERSE); - //rightFront.setDirection(DcMotorEx.Direction.REVERSE); - rightBack.setDirection(DcMotorEx.Direction.REVERSE); + rightFront.setDirection(DcMotorEx.Direction.REVERSE); + //rightBack.setDirection(DcMotorEx.Direction.REVERSE); //TODO Make the same update in DriveLocalizer() function. Search for Step 4.2 //TODO End Step 4.1 From 14a12ab4eba3f7eb8beb4eeee3b74a7075445265 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Sat, 4 Nov 2023 15:10:15 -0600 Subject: [PATCH 020/154] i need to push so i can upload code v2 --- .../ftc/teamcode/TheJettCode.java | 46 +++++++++---------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java index 19c9dd29ac..d4bb3cf759 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java @@ -9,44 +9,44 @@ @TeleOp public class TheJettCode extends LinearOpMode { private Servo LiftLeft; + private Servo Hopper; double LiftHeight; - private void ServoNo() { - if (gamepad1.a) { - LiftHeight = 0.7; - LiftLeft.setPosition(LiftHeight); - sleep(570); - LiftHeight = 0; - LiftLeft.setPosition(LiftHeight); - } + boolean LiftMax; + private void ServoNo() { } - private void ServoYes() { - if (gamepad1.y) { - LiftHeight = -0.7; - } else if (gamepad1.x) { - LiftHeight = 0.7; - } else { - LiftHeight = 0; - } - LiftLeft.setPosition(LiftHeight); - telemetry.addData("TAPE", LiftLeft.getPosition()); - telemetry.addData("TAPE non var", LiftHeight); - } @Override public void runOpMode() throws InterruptedException { - LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); + Hopper = hardwareMap.get(Servo.class, "Hopper"); + + LiftLeft.getController().pwmEnable(); + Hopper.getController().pwmEnable(); + waitForStart(); while (opModeIsActive()) { - ServoYes(); + if (gamepad1.y) { + LiftHeight = 0.1; + } else if (gamepad1.x) { + LiftHeight = 0.5; + } else { + LiftHeight = 0; + } + + + LiftLeft.setPosition(LiftHeight); + Hopper.setPosition(LiftHeight); + telemetry.addData("LiftLeft", LiftLeft.getPosition()); + telemetry.addData("Hopper", Hopper.getPosition()); + telemetry.addData("Lift non var", LiftHeight); ServoNo(); telemetry.update(); + sleep(100); } } - } \ No newline at end of file From a4e62ad3e348e54d3bee37a3c7580189c63c6448 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Sat, 4 Nov 2023 17:48:39 -0600 Subject: [PATCH 021/154] update v3 --- .../org/firstinspires/ftc/teamcode/TheJettCode.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java index d4bb3cf759..84898dc9ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java @@ -8,7 +8,7 @@ @TeleOp public class TheJettCode extends LinearOpMode { - private Servo LiftLeft; + private Servo Wrist; private Servo Hopper; double LiftHeight; boolean LiftMax; @@ -19,10 +19,10 @@ private void ServoNo() { @Override public void runOpMode() throws InterruptedException { - LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); + Wrist = hardwareMap.get(Servo.class, "Wrist"); Hopper = hardwareMap.get(Servo.class, "Hopper"); - LiftLeft.getController().pwmEnable(); + Wrist.getController().pwmEnable(); Hopper.getController().pwmEnable(); waitForStart(); @@ -36,9 +36,9 @@ public void runOpMode() throws InterruptedException { } - LiftLeft.setPosition(LiftHeight); + Wrist.setPosition(LiftHeight); Hopper.setPosition(LiftHeight); - telemetry.addData("LiftLeft", LiftLeft.getPosition()); + telemetry.addData("LiftLeft", Wrist.getPosition()); telemetry.addData("Hopper", Hopper.getPosition()); telemetry.addData("Lift non var", LiftHeight); ServoNo(); From 29ae9525c06340182df5d73bd13d140692295a63 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 4 Nov 2023 17:50:17 -0600 Subject: [PATCH 022/154] ok jett go merge -goober --- .../ftc/teamcode/Beginnings.java | 72 ++++++++++++++----- .../ftc/teamcode/MecanumDrive.java | 2 +- .../firstinspires/ftc/teamcode/OgDrive.java | 3 +- 3 files changed, 58 insertions(+), 19 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 9ab0fce877..2a6d2e963a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -64,22 +64,55 @@ private void setLiftHeight(double inputLiftHeight) { } + private void worm() { + if(gamepad2.left_bumper) { + shoulder.setPosition(0.445); + } + if(gamepad2.right_bumper) { + shoulder.setPosition(0.92); + } + } + private void airplane() { - if (gamepad2.dpad_down = true) { - launcher.setPosition(0.3); + if (gamepad2.dpad_down) { + launcher.setPosition(0.1); } else { launcher.setPosition(0.8); } } - private void extralift() { + private void tuneshoulder() { if (gamepad2.right_bumper) { - shoulder.setPosition(0.25); + shoulder.setPosition(shoulder.getPosition() + .01); + } else if (gamepad2.right_trigger > .5){ + shoulder.setPosition(shoulder.getPosition() - .01); + } else { + //shoulder.setPosition(0.1); } - else { - shoulder.setPosition(0.1); + telemetry.addData("shoulder", shoulder.getPosition()); + } + /* + private void tuneWrist() { + if (gamepad2.left_bumper) { + wrist.setPosition(wrist.getPosition() + .01); + } else if (gamepad2.left_trigger > .5){ + wrist.setPosition(wrist.getPosition() - .01); + } else { + //shoulder.setPosition(0.1); } + telemetry.addData("wrist", wrist.getPosition()); } + + private void jukeBeta() { + if (gamepad2.dpad_up) { + shoulder.setPosition(.48); + wrist.setPosition(.31); + } else if (gamepad2.dpad_down) { + shoulder.setPosition(.445); + wrist.setPosition(.26); + } + } + */ private void liftFunction() { if (gamepad2.y) { leftLift.setPosition(0.9 + LiftOffset); @@ -168,7 +201,9 @@ public void runOpMode() throws InterruptedException { hopper = hardwareMap.get(Servo.class, "hopper"); shoulder = hardwareMap.get(Servo.class, "shoulder"); - // shoulder.setDirection(Servo.Direction.REVERSE); + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + launcher.setDirection(Servo.Direction.REVERSE); @@ -184,12 +219,17 @@ public void runOpMode() throws InterruptedException { // servos launcher.setPosition(0.8); - leftLift.setPosition(0.5); - rightLift.setPosition(0.5); - shoulder.setPosition(0.1); - wrist.setPosition(-0.04); - hopper.setPosition(0.01); - + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + shoulder.setPosition(0.445); + wrist.setPosition(0.26); + hopper.setPosition(0.0); + /* + sleep(500); + leftLift.setPosition(0.8); + rightLift.setPosition(0.8); + wrist.setPosition(0.7); + */ double SLOW_DOWN_FACTOR = 0.5; @@ -201,13 +241,11 @@ public void runOpMode() throws InterruptedException { // servo_shenanigans(); // loop real while(opModeIsActive()){ - driveCode(); + //driveCode(); airplane(); - extralift(); - // ogDrive.og_drive_code(gamepad1, telemetry); + ogDrive.og_drive_code(gamepad1, telemetry); intakeFunction(); liftFunction(); - telemetry.update(); sleep(100); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index fde6b4c694..55110c6484 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -209,7 +209,7 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { //Uncomment the lines for which the motorDirection need to be reversed to ensure all motors run forward in test leftFront.setDirection(DcMotorEx.Direction.REVERSE); leftBack.setDirection(DcMotorEx.Direction.REVERSE); - rightFront.setDirection(DcMotorEx.Direction.REVERSE); + //rightFront.setDirection(DcMotorEx.Direction.REVERSE); //rightBack.setDirection(DcMotorEx.Direction.REVERSE); //TODO Make the same update in DriveLocalizer() function. Search for Step 4.2 //TODO End Step 4.1 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java index 05bfa1e1b2..a1353a1bc5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java @@ -30,7 +30,8 @@ public OgDrive(HardwareMap hardwareMap) { BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); FrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); FrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - BackRight.setDirection(DcMotorSimple.Direction.REVERSE); + //BackRight.setDirection(DcMotorSimple.Direction.REVERSE); + FrontRight.setDirection(DcMotorSimple.Direction.REVERSE); } public void og_drive_code(Gamepad gamepad1, Telemetry telemetry) { From 37bc2886ab90e4435492547b3353f51e2cfe5878 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Sat, 4 Nov 2023 17:55:52 -0600 Subject: [PATCH 023/154] update v4 --- .../main/java/org/firstinspires/ftc/teamcode/TheJettCode.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java index 84898dc9ad..b4ff2447a5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java @@ -19,8 +19,8 @@ private void ServoNo() { @Override public void runOpMode() throws InterruptedException { - Wrist = hardwareMap.get(Servo.class, "Wrist"); - Hopper = hardwareMap.get(Servo.class, "Hopper"); + Wrist = hardwareMap.get(Servo.class, "wrist"); + Hopper = hardwareMap.get(Servo.class, "hopper"); Wrist.getController().pwmEnable(); Hopper.getController().pwmEnable(); From fc4a87898771735373c123fa9edb08599e870e5e Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 4 Nov 2023 17:56:08 -0600 Subject: [PATCH 024/154] added wrist fix -goober --- .../main/java/org/firstinspires/ftc/teamcode/Beginnings.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 2a6d2e963a..f9ed7a69a0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -67,6 +67,7 @@ private void setLiftHeight(double inputLiftHeight) { private void worm() { if(gamepad2.left_bumper) { shoulder.setPosition(0.445); + wrist.setPosition(0.26); } if(gamepad2.right_bumper) { shoulder.setPosition(0.92); @@ -246,6 +247,7 @@ public void runOpMode() throws InterruptedException { ogDrive.og_drive_code(gamepad1, telemetry); intakeFunction(); liftFunction(); + worm(); telemetry.update(); sleep(100); } From 458aeec5018281760f8ed8d3e92493b5705aa8c9 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 5 Nov 2023 09:15:03 -0700 Subject: [PATCH 025/154] =?UTF-8?q?EL=20MC=20TUNERR=20=F0=9F=A7=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit -goober --- .../ftc/teamcode/Beginnings.java | 20 ++++---- .../ftc/teamcode/McTuner2000.java | 47 +++++++++++++++++++ 2 files changed, 57 insertions(+), 10 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index f9ed7a69a0..596c3ad76d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -84,9 +84,9 @@ private void airplane() { } private void tuneshoulder() { if (gamepad2.right_bumper) { - shoulder.setPosition(shoulder.getPosition() + .01); - } else if (gamepad2.right_trigger > .5){ - shoulder.setPosition(shoulder.getPosition() - .01); + shoulder.setPosition(shoulder.getPosition() + 0.01); + } else if (gamepad2.right_trigger > 0.5){ + shoulder.setPosition(shoulder.getPosition() - 0.01); } else { //shoulder.setPosition(0.1); } @@ -95,9 +95,9 @@ private void tuneshoulder() { /* private void tuneWrist() { if (gamepad2.left_bumper) { - wrist.setPosition(wrist.getPosition() + .01); - } else if (gamepad2.left_trigger > .5){ - wrist.setPosition(wrist.getPosition() - .01); + wrist.setPosition(wrist.getPosition() + 0.01); + } else if (gamepad2.left_trigger > 0.5){ + wrist.setPosition(wrist.getPosition() - 0.01); } else { //shoulder.setPosition(0.1); } @@ -106,11 +106,11 @@ private void tuneWrist() { private void jukeBeta() { if (gamepad2.dpad_up) { - shoulder.setPosition(.48); - wrist.setPosition(.31); + shoulder.setPosition(0.48); + wrist.setPosition(0.31); } else if (gamepad2.dpad_down) { - shoulder.setPosition(.445); - wrist.setPosition(.26); + shoulder.setPosition(0.445); + wrist.setPosition(0.26); } } */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java new file mode 100644 index 0000000000..51ab4aee5a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +@TeleOp +public class McTuner2000 extends LinearOpMode { + Servo wrist; + Servo hopper; + + private void wristTuner() { + if (gamepad1.y) { + wrist.setPosition(wrist.getPosition() + 0.02); + } + else if (gamepad1.a) { + wrist.setPosition(wrist.getPosition() - 0.02); + } + telemetry.addData("Wrist Position", wrist.getPosition()); + } + private void hopperTuner() { + if (gamepad1.x) { + hopper.setPosition(hopper.getPosition() + 0.02); + } + else if (gamepad1.b) { + hopper.setPosition(hopper.getPosition() - 0.02); + } + telemetry.addData("Hopper Position", hopper.getPosition()); + } + @Override + public void runOpMode() throws InterruptedException { + + wrist = hardwareMap.get(Servo.class, "wrist"); + hopper = hardwareMap.get(Servo.class, "hopper"); + telemetry.update(); + + telemetry.addData("Wrist Position", wrist.getPosition()); + telemetry.addData("Hopper Position", hopper.getPosition()); + waitForStart(); + + while(opModeIsActive()) { + wristTuner(); + hopperTuner(); + telemetry.update(); + } + } +} From 159bdf6113ded71ac05165dbe8abbb379a165dad Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 5 Nov 2023 10:30:01 -0700 Subject: [PATCH 026/154] =?UTF-8?q?Mc=20tuner,=20but=20like,=20cooler=20by?= =?UTF-8?q?=20like=201000x,=20and=20in=20the=20year=203000,=20=F0=9F=8D=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit -goober --- .../ftc/teamcode/McTuner2000.java | 6 +- .../ftc/teamcode/McTuner3000.java | 128 ++++++++++++++++++ 2 files changed, 133 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java index 51ab4aee5a..004c0d2417 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java @@ -8,6 +8,7 @@ public class McTuner2000 extends LinearOpMode { Servo wrist; Servo hopper; + Servo shoulder; private void wristTuner() { if (gamepad1.y) { @@ -32,10 +33,13 @@ public void runOpMode() throws InterruptedException { wrist = hardwareMap.get(Servo.class, "wrist"); hopper = hardwareMap.get(Servo.class, "hopper"); - telemetry.update(); + + wrist.setDirection(Servo.Direction.REVERSE); telemetry.addData("Wrist Position", wrist.getPosition()); telemetry.addData("Hopper Position", hopper.getPosition()); + + telemetry.update(); waitForStart(); while(opModeIsActive()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java new file mode 100644 index 0000000000..3f2437082b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -0,0 +1,128 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.vision.apriltag.AprilTagCanvasAnnotator; + +@TeleOp +public class McTuner3000 extends LinearOpMode { + //TODO: Step 1, Replace all "wrist","hopper", etc with your servos + Servo wrist; + Servo hopper; + Servo shoulder; + Servo leftLift; + Servo rightLift; + + double speedAmount; + + enum ServoTypes{ + SHOULDER, + HOPPER, + WRIST, + LIFT; + } + ServoTypes which; + + private void masterTuner() { + if (gamepad1.left_bumper) { + if (which == ServoTypes.SHOULDER) { + shoulder.setPosition(shoulder.getPosition() - speedAmount); + } + else if (which == ServoTypes.HOPPER) { + hopper.setPosition(hopper.getPosition() - speedAmount); + } + else if (which == ServoTypes.WRIST) { + wrist.setPosition(wrist.getPosition() - speedAmount); + } + else if (which == ServoTypes.LIFT) { + leftLift.setPosition(leftLift.getPosition() - speedAmount); + rightLift.setPosition(rightLift.getPosition() - speedAmount); + } + } + else if (gamepad1.right_bumper) { + if (which == ServoTypes.SHOULDER) { + shoulder.setPosition(shoulder.getPosition() + speedAmount); + } + else if (which == ServoTypes.HOPPER) { + hopper.setPosition(hopper.getPosition() + speedAmount); + } + else if (which == ServoTypes.WRIST) { + wrist.setPosition(wrist.getPosition() + speedAmount); + } + else if (which == ServoTypes.LIFT) { + leftLift.setPosition(leftLift.getPosition() + speedAmount); + rightLift.setPosition(rightLift.getPosition() + speedAmount); + } + } + } + private void setServo() { + if (gamepad1.y) { + which = ServoTypes.SHOULDER; + } + else if (gamepad1.x) { + which = ServoTypes.HOPPER; + } + else if (gamepad1.b) { + which = ServoTypes.WRIST; + } + else if (gamepad1.a) { + which = ServoTypes.LIFT; + } + telemetry.addData("Selected Servo = ", which.toString()); + } + private void setSpeed() { + if (gamepad1.dpad_down) { + speedAmount = 0; + } + else if (gamepad1.dpad_left) { + speedAmount = 0.01; + } + else if (gamepad1.dpad_right) { + speedAmount = 0.02; + } + else if (gamepad1.dpad_up) { + speedAmount = 0.05; + } + telemetry.addData("Speed = ", speedAmount); + } + private void whatServoAt() { + telemetry.addData("Shoulder = ", shoulder.getPosition()); + telemetry.addData("Hopper = ", hopper.getPosition()); + telemetry.addData("Wrist = ", wrist.getPosition()); + telemetry.addData("Lift Left = ",leftLift.getPosition()); + telemetry.addData("Lift Right = ",rightLift.getPosition()); + } + @Override + public void runOpMode() throws InterruptedException { + which = ServoTypes.SHOULDER; + speedAmount = 0.01; + + //TODO: Step 2, Replace the device names with your 4 (or more if you use two servos for one task) + wrist = hardwareMap.get(Servo.class, "wrist"); + hopper = hardwareMap.get(Servo.class, "hopper"); + shoulder = hardwareMap.get(Servo.class, "shoulder"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + + telemetry.addData("Wrist Position", wrist.getPosition()); + telemetry.addData("Hopper Position", hopper.getPosition()); + + telemetry.update(); + waitForStart(); + + while(opModeIsActive()) { + setServo(); + masterTuner(); + setSpeed(); + whatServoAt(); + telemetry.addData("Selected", which.toString()); + telemetry.addLine("Y = Shoulder - X = Hopper - B = Wrist A = Lift"); + telemetry.update(); + } + } +} From f3e47b9b7693b521f354c63aa1afcd9bf2ee932a Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 5 Nov 2023 10:38:03 -0700 Subject: [PATCH 027/154] Making the todo list for McTuner3000 was unnecessary, fun, but unnecessary. -goober --- .../java/org/firstinspires/ftc/teamcode/McTuner3000.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java index 3f2437082b..cc456e884b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -17,6 +17,7 @@ public class McTuner3000 extends LinearOpMode { double speedAmount; + //TODO: Step 3, set all of your servo names below enum ServoTypes{ SHOULDER, HOPPER, @@ -24,6 +25,7 @@ enum ServoTypes{ LIFT; } ServoTypes which; + //TODO: Step 4, replace all names of Servos with yours, and replace all capitals with what you set them to from step 3 private void masterTuner() { if (gamepad1.left_bumper) { @@ -57,6 +59,7 @@ else if (which == ServoTypes.LIFT) { } } } + //TODO: Step 5, replace all of your Servo functions below private void setServo() { if (gamepad1.y) { which = ServoTypes.SHOULDER; @@ -87,6 +90,8 @@ else if (gamepad1.dpad_up) { } telemetry.addData("Speed = ", speedAmount); } + //TODO: Step 6 replace all of the xyz.getPosition()0; with your servos and replace "xyz" with what that servo is + //TODO: Step 7, your done! This was written by Goober on 11/5/23 slouching in a chair at 10:35 in the morning. And as I'm writing this I wonder if anybody will actually use this. Problably not, but idk what to do while I wait for the robot to be ready for calibration. It's now 10:36 am. I wonder if future me will use this and or remember doing this. Goober out. private void whatServoAt() { telemetry.addData("Shoulder = ", shoulder.getPosition()); telemetry.addData("Hopper = ", hopper.getPosition()); From 0c128f527025d5683ac8d985ccd73893ea4973ed Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 5 Nov 2023 10:40:47 -0700 Subject: [PATCH 028/154] Small change. Made really long text into multible lines -goober --- .../java/org/firstinspires/ftc/teamcode/McTuner3000.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java index cc456e884b..8655bb0f78 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -90,8 +90,13 @@ else if (gamepad1.dpad_up) { } telemetry.addData("Speed = ", speedAmount); } + + //TODO: Step 7, your done! This was written by Goober on 11/5/23 slouching in a chair at 10:35 in the morning. + // And as I'm writing this I wonder if anybody will actually use this. Problably not, + // but idk what to do while I wait for the robot to be ready for calibration. It's now 10:36 am. + // I wonder if future me will use this and or remember doing this. Goober out. + //TODO: Step 6 replace all of the xyz.getPosition()0; with your servos and replace "xyz" with what that servo is - //TODO: Step 7, your done! This was written by Goober on 11/5/23 slouching in a chair at 10:35 in the morning. And as I'm writing this I wonder if anybody will actually use this. Problably not, but idk what to do while I wait for the robot to be ready for calibration. It's now 10:36 am. I wonder if future me will use this and or remember doing this. Goober out. private void whatServoAt() { telemetry.addData("Shoulder = ", shoulder.getPosition()); telemetry.addData("Hopper = ", hopper.getPosition()); From 550dd67ec34b5d25e8f364a678f538eac6779eec Mon Sep 17 00:00:00 2001 From: Gabriel Date: Sun, 5 Nov 2023 10:45:41 -0700 Subject: [PATCH 029/154] remerge last year's code for budster's ogdrive --- .../java/org/firstinspires/ftc/teamcode/OgDrive.java | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java index a1353a1bc5..df2e420c09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java @@ -43,10 +43,17 @@ public void og_drive_code(Gamepad gamepad1, Telemetry telemetry) { Scale_Factor_of_Drive = 0.55; } // drive with joysticks - BackLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); - BackRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); + /* FrontLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + BackLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + BackRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); + */ + FrontLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (-gamepad1.left_stick_x + gamepad1.left_stick_y)); + BackLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (-gamepad1.left_stick_x + gamepad1.left_stick_y)); + BackRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + telemetry.addData("FL Motor", FrontLeft.getPower()); telemetry.addData("FR Motor", FrontRight.getPower()); telemetry.addData("BL Motor", BackLeft.getPower()); From f04f24c4477dfe41385c187ff6736ec7f0e5056e Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 5 Nov 2023 13:04:45 -0700 Subject: [PATCH 030/154] Starting stop motion and fixed drive -goober --- .../org/firstinspires/ftc/teamcode/Beginnings.java | 13 +++++++------ .../org/firstinspires/ftc/teamcode/McTuner3000.java | 9 +++++++++ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 596c3ad76d..e621a8f8b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -5,6 +5,7 @@ import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -220,11 +221,11 @@ public void runOpMode() throws InterruptedException { // servos launcher.setPosition(0.8); + wrist.setPosition(0.51); + shoulder.setPosition(0.44); + hopper.setPosition(0); leftLift.setPosition(0.42); rightLift.setPosition(0.42); - shoulder.setPosition(0.445); - wrist.setPosition(0.26); - hopper.setPosition(0.0); /* sleep(500); leftLift.setPosition(0.8); @@ -243,11 +244,11 @@ public void runOpMode() throws InterruptedException { // loop real while(opModeIsActive()){ //driveCode(); - airplane(); + //airplane(); ogDrive.og_drive_code(gamepad1, telemetry); intakeFunction(); - liftFunction(); - worm(); + //liftFunction(); + //worm(); telemetry.update(); sleep(100); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java index 8655bb0f78..5fe3e22f17 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -122,6 +122,14 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Wrist Position", wrist.getPosition()); telemetry.addData("Hopper Position", hopper.getPosition()); + wrist.setPosition(0.51); + shoulder.setPosition(0.44); + hopper.setPosition(0); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + + + telemetry.update(); waitForStart(); @@ -133,6 +141,7 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Selected", which.toString()); telemetry.addLine("Y = Shoulder - X = Hopper - B = Wrist A = Lift"); telemetry.update(); + sleep(100); } } } From 3444b6c47136081056fae172878926c3fb846de4 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 5 Nov 2023 13:04:53 -0700 Subject: [PATCH 031/154] Starting stop motion and fixed drive -goober --- .../firstinspires/ftc/teamcode/OgDrive.java | 11 ++-- .../ftc/teamcode/StopMotionTester.java | 50 +++++++++++++++++++ 2 files changed, 56 insertions(+), 5 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java index df2e420c09..c14cd21883 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java @@ -43,16 +43,17 @@ public void og_drive_code(Gamepad gamepad1, Telemetry telemetry) { Scale_Factor_of_Drive = 0.55; } // drive with joysticks - /* - FrontLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); - FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); - BackLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + + FrontLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); + FrontRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + BackLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); BackRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); - */ + /* FrontLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (-gamepad1.left_stick_x + gamepad1.left_stick_y)); BackLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (-gamepad1.left_stick_x + gamepad1.left_stick_y)); BackRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); +*/ telemetry.addData("FL Motor", FrontLeft.getPower()); telemetry.addData("FR Motor", FrontRight.getPower()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java new file mode 100644 index 0000000000..592187bc54 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode; + +import static android.os.SystemClock.sleep; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +public class StopMotionTester { + + static public void Dominator(HardwareMap hardwareMap) { + Servo wrist; + Servo hopper; + Servo shoulder; + Servo leftLift; + Servo rightLift; + + wrist = hardwareMap.get(Servo.class, "wrist"); + hopper = hardwareMap.get(Servo.class, "hopper"); + shoulder = hardwareMap.get(Servo.class, "shoulder"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + + wrist.setPosition(0.51); + shoulder.setPosition(0.44); + hopper.setPosition(0); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + + sleep(2000); + + wrist.setPosition(0.74); + shoulder.setPosition(0.7); + hopper.setPosition(0); + + sleep(2000); + + wrist.setPosition(0.73); + shoulder.setPosition(0.75); + hopper.setPosition(0.16); + + sleep(2000); + + + + } +} From a7baea4f7b7e24ec09edf9ca915e4d9e1db3f3cf Mon Sep 17 00:00:00 2001 From: digitalbreadllc Date: Mon, 6 Nov 2023 10:24:35 -0700 Subject: [PATCH 032/154] Adding OdoMec --- .../src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java new file mode 100644 index 0000000000..01b5990353 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode; + +public class OdoMec { +} From 35a9c2dbd79f7c1200438e8382dc61cbbe8d3455 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Mon, 6 Nov 2023 17:19:53 -0700 Subject: [PATCH 033/154] update git pls -goober --- .../main/java/org/firstinspires/ftc/teamcode/Beginnings.java | 2 +- .../main/java/org/firstinspires/ftc/teamcode/McTuner2000.java | 1 + .../main/java/org/firstinspires/ftc/teamcode/McTuner3000.java | 1 + .../src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java | 1 + .../java/org/firstinspires/ftc/teamcode/StopMotionTester.java | 1 + 5 files changed, 5 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index e621a8f8b4..d6c9d6aec1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -233,7 +233,7 @@ public void runOpMode() throws InterruptedException { wrist.setPosition(0.7); */ - + //this is a coment to mAKE git update double SLOW_DOWN_FACTOR = 0.5; telemetry.addData("Initializing TeleOp",""); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java index 004c0d2417..1d6981c9b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java @@ -41,6 +41,7 @@ public void runOpMode() throws InterruptedException { telemetry.update(); waitForStart(); + //this is a coment to mAKE git update while(opModeIsActive()) { wristTuner(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java index 5fe3e22f17..8667f76b98 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -108,6 +108,7 @@ private void whatServoAt() { public void runOpMode() throws InterruptedException { which = ServoTypes.SHOULDER; speedAmount = 0.01; + //this is a coment to mAKE git update //TODO: Step 2, Replace the device names with your 4 (or more if you use two servos for one task) wrist = hardwareMap.get(Servo.class, "wrist"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java index c14cd21883..bea317ac23 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java @@ -42,6 +42,7 @@ public void og_drive_code(Gamepad gamepad1, Telemetry telemetry) { } else { Scale_Factor_of_Drive = 0.55; } + //this is a coment to mAKE git update // drive with joysticks FrontLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java index 592187bc54..3be96b4afd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java @@ -43,6 +43,7 @@ static public void Dominator(HardwareMap hardwareMap) { hopper.setPosition(0.16); sleep(2000); + //this is a coment to mAKE git update From 483c2a90218114d60f6326dff16b8a66b40125cd Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Fri, 3 Nov 2023 17:42:45 -0600 Subject: [PATCH 034/154] i need to update the code for everyone yes yes --- .../ftc/teamcode/TheJettCode.java | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java index 2aeb8c347c..19c9dd29ac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java @@ -4,44 +4,44 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; @TeleOp public class TheJettCode extends LinearOpMode { - private CRServo Tape; - double TapePower; - private void tapetroll() { + private Servo LiftLeft; + double LiftHeight; + private void ServoNo() { if (gamepad1.a) { - TapePower = 0.7; - Tape.setPower(TapePower); + LiftHeight = 0.7; + LiftLeft.setPosition(LiftHeight); sleep(570); - TapePower = 0; - Tape.setPower(TapePower); + LiftHeight = 0; + LiftLeft.setPosition(LiftHeight); } } - private void taper() { + private void ServoYes() { if (gamepad1.y) { - TapePower = -0.7; + LiftHeight = -0.7; } else if (gamepad1.x) { - TapePower = 0.7; + LiftHeight = 0.7; } else { - TapePower = 0; + LiftHeight = 0; } - Tape.setPower(TapePower); - telemetry.addData("TAPE", Tape.getPower()); - telemetry.addData("TAPE non var", TapePower); + LiftLeft.setPosition(LiftHeight); + telemetry.addData("TAPE", LiftLeft.getPosition()); + telemetry.addData("TAPE non var", LiftHeight); } @Override public void runOpMode() throws InterruptedException { - Tape = hardwareMap.get(CRServo.class, "Tape"); - Tape.setDirection(DcMotorSimple.Direction.REVERSE); + LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); waitForStart(); while (opModeIsActive()) { - taper(); - tapetroll(); + ServoYes(); + ServoNo(); telemetry.update(); } From 4fe730c7de3854f48b86bf632c407b17a34c140f Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 3 Nov 2023 18:15:56 -0600 Subject: [PATCH 035/154] Goodbye Scorpion, you have served us well. We appreciated you teaching us the ways of roadrunner and learning android studio. We will never forget your sacrifice, and you will always be remembered as our favorite bug. We could not have gotten to the point where we are without you. You will be missed. We are moving on to Lightning McGreen. Goodbye. o7 -goober --- .../java/org/firstinspires/ftc/teamcode/Beginnings.java | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 1a6a51f154..bc89bc983b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -18,7 +18,7 @@ public class Beginnings extends LinearOpMode { // Declare vars - MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + MecanumDrive drive; DcMotor BackLeft; DcMotor BackRight; DcMotor FrontLeft; @@ -126,6 +126,9 @@ public void runOpMode() throws InterruptedException { androidTextToSpeech = new AndroidTextToSpeech(); androidTextToSpeech.initialize(); + // for wires driving + drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + /* all for OG driving BackLeft = hardwareMap.get(DcMotor.class, "leftRear"); BackRight = hardwareMap.get(DcMotor.class, "rightRear"); FrontLeft = hardwareMap.get(DcMotor.class, "leftFront"); @@ -133,7 +136,8 @@ public void runOpMode() throws InterruptedException { LiftRight = hardwareMap.get(Servo.class, "LiftRight"); LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); - // LiftRight.setDirection(Servo.Direction.REVERSE); only for og drive code + + LiftRight.setDirection(Servo.Direction.REVERSE); only for og drive code // init servo hardware @@ -143,6 +147,7 @@ public void runOpMode() throws InterruptedException { FrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); FrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); BackRight.setDirection(DcMotorSimple.Direction.REVERSE); + */ double SLOW_DOWN_FACTOR = 0.5; telemetry.addData("Initializing TeleOp",""); From 1289db310e99f59797061c4a942d5f216cd7ccb8 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 3 Nov 2023 18:54:03 -0600 Subject: [PATCH 036/154] Adding Servos for new robot -goober --- .../ftc/teamcode/Beginnings.java | 29 ++++++++++++++++--- .../ftc/teamcode/MecanumDrive.java | 4 +-- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index bc89bc983b..6f1afadb32 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -18,13 +18,18 @@ public class Beginnings extends LinearOpMode { // Declare vars + private Servo launcher; + private Servo rightLift; + private Servo leftLift; + private Servo shoulder; + private Servo wrist; + private Servo hopper; MecanumDrive drive; DcMotor BackLeft; DcMotor BackRight; DcMotor FrontLeft; DcMotor FrontRight; - Servo LiftRight; - Servo LiftLeft; + // Servo prep @@ -74,8 +79,8 @@ private void setLiftHeight(double inputLiftHeight) { inputLiftHeight = 0.65; } LiftHeight = inputLiftHeight; - LiftLeft.setPosition(LiftLeftOffset + LiftHeight); - LiftRight.setPosition(LiftHeight); + leftLift.setPosition(LiftLeftOffset + LiftHeight); + rightLift.setPosition(LiftHeight); } @@ -128,6 +133,13 @@ public void runOpMode() throws InterruptedException { // for wires driving drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + launcher = hardwareMap.get(Servo.class, "launcher"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + shoulder = hardwareMap.get(Servo.class, "shoulder"); + wrist = hardwareMap.get(Servo.class, "wrist"); + hopper = hardwareMap.get(Servo.class, "hopper"); /* all for OG driving BackLeft = hardwareMap.get(DcMotor.class, "leftRear"); BackRight = hardwareMap.get(DcMotor.class, "rightRear"); @@ -149,6 +161,15 @@ public void runOpMode() throws InterruptedException { BackRight.setDirection(DcMotorSimple.Direction.REVERSE); */ + // servos + launcher.setPosition(0.8); + leftLift.setPosition(0); + rightLift.setPosition(0); + shoulder.setPosition(0.1); + wrist.setPosition(-0.04); + hopper.setPosition(0.01); + + double SLOW_DOWN_FACTOR = 0.5; telemetry.addData("Initializing TeleOp",""); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 77def0e455..55110c6484 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -200,8 +200,8 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { //TODO Step 1 Drive Classes : get basic hardware configured. Update motor names to what is used in robot configuration leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftBack = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightBack = hardwareMap.get(DcMotorEx.class, "rightRear"); + leftBack = hardwareMap.get(DcMotorEx.class, "leftBack"); + rightBack = hardwareMap.get(DcMotorEx.class, "rightBack"); rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); //TODO End Step 1 From b0f983182ec2e859aec87600d148c6efb3fb1c1e Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 3 Nov 2023 19:13:56 -0600 Subject: [PATCH 037/154] Added Intake code -goober --- .../ftc/teamcode/Beginnings.java | 24 +++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 6f1afadb32..981bafd395 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -29,6 +29,8 @@ public class Beginnings extends LinearOpMode { DcMotor BackRight; DcMotor FrontLeft; DcMotor FrontRight; + DcMotor frontIntake; + DcMotor rearIntake; // Servo prep @@ -86,11 +88,14 @@ private void setLiftHeight(double inputLiftHeight) { private void intakeFunction() { if (gamepad1.right_trigger > 0.5) { - // Intake.setPower(-1); - telemetry.addData("right trig pressed", "yes"); + frontIntake.setPower(0.5); + rearIntake.setPower(0.5); + telemetry.addData("Intake", "yes"); } else { - telemetry.addData("right trig not pressed", "yes"); + telemetry.addData("Intake", "no"); + frontIntake.setPower(0); + rearIntake.setPower(0); } } @@ -118,7 +123,7 @@ private void driveCode() { telemetry.addData("x", drive.pose.position.x); telemetry.addData("y", drive.pose.position.y); telemetry.addData("heading", Math.toDegrees(drive.pose.heading.log())); - telemetry.update(); + // telemetry.update(); } @@ -140,6 +145,17 @@ public void runOpMode() throws InterruptedException { shoulder = hardwareMap.get(Servo.class, "shoulder"); wrist = hardwareMap.get(Servo.class, "wrist"); hopper = hardwareMap.get(Servo.class, "hopper"); + + frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); + rearIntake = hardwareMap.get(DcMotor.class, "rearIntake"); + + frontIntake.setDirection(DcMotorSimple.Direction.FORWARD); + rearIntake.setDirection(DcMotorSimple.Direction.REVERSE); + frontIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + /* all for OG driving BackLeft = hardwareMap.get(DcMotor.class, "leftRear"); BackRight = hardwareMap.get(DcMotor.class, "rightRear"); From 81fad2adf6bdf08fcbcee49e48d50fc0100e0fe1 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 3 Nov 2023 19:24:04 -0600 Subject: [PATCH 038/154] Fixed intaking out outputing code -goober --- .../firstinspires/ftc/teamcode/Beginnings.java | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 981bafd395..b2da714d3d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -88,17 +88,24 @@ private void setLiftHeight(double inputLiftHeight) { private void intakeFunction() { if (gamepad1.right_trigger > 0.5) { - frontIntake.setPower(0.5); - rearIntake.setPower(0.5); - telemetry.addData("Intake", "yes"); + frontIntake.setPower(1); + rearIntake.setPower(1); + telemetry.addData("Intake", "in"); + } + else if (gamepad1.right_bumper) { + frontIntake.setPower(-1); + rearIntake.setPower(-1); + telemetry.addData("Intake", "out"); } else { - telemetry.addData("Intake", "no"); + telemetry.addData("Intake", "stopped"); frontIntake.setPower(0); rearIntake.setPower(0); } + } + private void driveCode() { double SLOW_DOWN_FACTOR; From 08e7c21c280510f43d17534d49956aeeb21334ed Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 4 Nov 2023 10:48:52 -0600 Subject: [PATCH 039/154] Refactored og drive -goober --- .../ftc/teamcode/Beginnings.java | 52 ++-------------- .../firstinspires/ftc/teamcode/OgDrive.java | 62 +++++++++++++++++++ 2 files changed, 68 insertions(+), 46 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index b2da714d3d..7f64f0b2ab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -9,6 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; @@ -25,10 +26,7 @@ public class Beginnings extends LinearOpMode { private Servo wrist; private Servo hopper; MecanumDrive drive; - DcMotor BackLeft; - DcMotor BackRight; - DcMotor FrontLeft; - DcMotor FrontRight; + OgDrive ogDrive; DcMotor frontIntake; DcMotor rearIntake; @@ -45,27 +43,6 @@ public class Beginnings extends LinearOpMode { // Functions \/ - private void og_drive_code() { - - double Scale_Factor_of_Drive; - - if (gamepad1.right_bumper) { - Scale_Factor_of_Drive = 1; - } else { - Scale_Factor_of_Drive = 0.55; - } - // drive with joysticks - BackLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); - BackRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); - FrontLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); - FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); - telemetry.addData("FL Motor", FrontLeft.getPower()); - telemetry.addData("FR Motor", FrontRight.getPower()); - telemetry.addData("BL Motor", BackLeft.getPower()); - telemetry.addData("BR Motor", BackRight.getPower()); - - } - private void servo_shenanigans() { setLiftHeight(0.4); @@ -146,6 +123,9 @@ public void runOpMode() throws InterruptedException { // for wires driving drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + // for Og driving (the best driving) + ogDrive = new OgDrive(hardwareMap); + launcher = hardwareMap.get(Servo.class, "launcher"); rightLift = hardwareMap.get(Servo.class, "rightLift"); leftLift = hardwareMap.get(Servo.class, "leftLift"); @@ -163,27 +143,6 @@ public void runOpMode() throws InterruptedException { rearIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); frontIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - /* all for OG driving - BackLeft = hardwareMap.get(DcMotor.class, "leftRear"); - BackRight = hardwareMap.get(DcMotor.class, "rightRear"); - FrontLeft = hardwareMap.get(DcMotor.class, "leftFront"); - FrontRight = hardwareMap.get(DcMotor.class, "rightFront"); - LiftRight = hardwareMap.get(Servo.class, "LiftRight"); - LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); - - - LiftRight.setDirection(Servo.Direction.REVERSE); only for og drive code - - - // init servo hardware - // init drive hardware and variables - BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - FrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - FrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - BackRight.setDirection(DcMotorSimple.Direction.REVERSE); - */ - // servos launcher.setPosition(0.8); leftLift.setPosition(0); @@ -203,6 +162,7 @@ public void runOpMode() throws InterruptedException { // loop real while(opModeIsActive()){ driveCode(); + // ogDrive.og_drive_code(gamepad1, telemetry); intakeFunction(); telemetry.update(); sleep(100); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java new file mode 100644 index 0000000000..b2c5f279eb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + + +public class OgDrive { + DcMotor BackLeft; + DcMotor BackRight; + DcMotor FrontLeft; + DcMotor FrontRight; + Servo LiftRight; + Servo LiftLeft; + + public OgDrive(HardwareMap hardwareMap) { + + BackLeft = hardwareMap.get(DcMotor.class, "leftRear"); + BackRight = hardwareMap.get(DcMotor.class, "rightRear"); + FrontLeft = hardwareMap.get(DcMotor.class, "leftFront"); + FrontRight = hardwareMap.get(DcMotor.class, "rightFront"); + LiftRight = hardwareMap.get(Servo.class, "LiftRight"); + LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); + + + LiftRight.setDirection(Servo.Direction.REVERSE); + + + // init servo hardware + // init drive hardware and variables + BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + FrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + FrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + BackRight.setDirection(DcMotorSimple.Direction.REVERSE); + } + public void og_drive_code(Gamepad gamepad1, Telemetry telemetry) { + + double Scale_Factor_of_Drive; + + if (gamepad1.right_bumper) { + Scale_Factor_of_Drive = 1; + } else { + Scale_Factor_of_Drive = 0.55; + } + // drive with joysticks + BackLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + BackRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); + FrontLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); + FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + telemetry.addData("FL Motor", FrontLeft.getPower()); + telemetry.addData("FR Motor", FrontRight.getPower()); + telemetry.addData("BL Motor", BackLeft.getPower()); + telemetry.addData("BR Motor", BackRight.getPower()); + + } +} From 6c8c0860183a27b5ca86897ccd82d27aea115141 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 4 Nov 2023 11:54:43 -0600 Subject: [PATCH 040/154] Lift and airplane code -goober --- .../ftc/teamcode/Beginnings.java | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 7f64f0b2ab..7ab48dea61 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -37,6 +37,7 @@ public class Beginnings extends LinearOpMode { double MaxLiftHeight = 0.65; double LiftLeftOffset = -0.08; double LiftHeight; + double LiftOffset = 0; private AndroidTextToSpeech androidTextToSpeech; @@ -63,6 +64,32 @@ private void setLiftHeight(double inputLiftHeight) { } + private void airplane() { + if (gamepad2.dpad_down = true) { + launcher.setPosition(0.3); + } + else { + launcher.setPosition(0); + } + } + private void liftFunction() { + if (gamepad2.y) { + leftLift.setPosition(0.6 + LiftOffset); + rightLift.setPosition(0.6); + } + else if (gamepad2.x) { + leftLift.setPosition(0.4 + LiftOffset); + rightLift.setPosition(0.4); + } + else if (gamepad2.b) { + leftLift.setPosition(0.2 + LiftOffset); + rightLift.setPosition(0.2); + } + else if (gamepad2.a) { + leftLift.setPosition(0 + LiftOffset); + rightLift.setPosition(0); + } + } private void intakeFunction() { if (gamepad1.right_trigger > 0.5) { frontIntake.setPower(1); @@ -133,6 +160,8 @@ public void runOpMode() throws InterruptedException { wrist = hardwareMap.get(Servo.class, "wrist"); hopper = hardwareMap.get(Servo.class, "hopper"); + launcher.setDirection(Servo.Direction.REVERSE); + frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); rearIntake = hardwareMap.get(DcMotor.class, "rearIntake"); @@ -162,8 +191,11 @@ public void runOpMode() throws InterruptedException { // loop real while(opModeIsActive()){ driveCode(); + airplane(); // ogDrive.og_drive_code(gamepad1, telemetry); intakeFunction(); + liftFunction(); + telemetry.update(); sleep(100); } From f4f3677de18082a5b6ccdf21f45b458d0da654e2 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Sat, 4 Nov 2023 15:10:15 -0600 Subject: [PATCH 041/154] i need to push so i can upload code v2 --- .../ftc/teamcode/TheJettCode.java | 46 +++++++++---------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java index 19c9dd29ac..d4bb3cf759 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java @@ -9,44 +9,44 @@ @TeleOp public class TheJettCode extends LinearOpMode { private Servo LiftLeft; + private Servo Hopper; double LiftHeight; - private void ServoNo() { - if (gamepad1.a) { - LiftHeight = 0.7; - LiftLeft.setPosition(LiftHeight); - sleep(570); - LiftHeight = 0; - LiftLeft.setPosition(LiftHeight); - } + boolean LiftMax; + private void ServoNo() { } - private void ServoYes() { - if (gamepad1.y) { - LiftHeight = -0.7; - } else if (gamepad1.x) { - LiftHeight = 0.7; - } else { - LiftHeight = 0; - } - LiftLeft.setPosition(LiftHeight); - telemetry.addData("TAPE", LiftLeft.getPosition()); - telemetry.addData("TAPE non var", LiftHeight); - } @Override public void runOpMode() throws InterruptedException { - LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); + Hopper = hardwareMap.get(Servo.class, "Hopper"); + + LiftLeft.getController().pwmEnable(); + Hopper.getController().pwmEnable(); + waitForStart(); while (opModeIsActive()) { - ServoYes(); + if (gamepad1.y) { + LiftHeight = 0.1; + } else if (gamepad1.x) { + LiftHeight = 0.5; + } else { + LiftHeight = 0; + } + + + LiftLeft.setPosition(LiftHeight); + Hopper.setPosition(LiftHeight); + telemetry.addData("LiftLeft", LiftLeft.getPosition()); + telemetry.addData("Hopper", Hopper.getPosition()); + telemetry.addData("Lift non var", LiftHeight); ServoNo(); telemetry.update(); + sleep(100); } } - } \ No newline at end of file From 6a75dda33042c30b1f3c0a5233231ced028cfcf6 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 4 Nov 2023 12:47:17 -0600 Subject: [PATCH 042/154] this code works on McGreen -goober --- .../ftc/teamcode/Beginnings.java | 22 +++++++++---------- .../ftc/teamcode/MecanumDrive.java | 2 +- .../firstinspires/ftc/teamcode/OgDrive.java | 11 ++-------- 3 files changed, 14 insertions(+), 21 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 7ab48dea61..c365f0f5ea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -74,20 +74,20 @@ private void airplane() { } private void liftFunction() { if (gamepad2.y) { - leftLift.setPosition(0.6 + LiftOffset); - rightLift.setPosition(0.6); + leftLift.setPosition(0.9 + LiftOffset); + rightLift.setPosition(0.9); } else if (gamepad2.x) { - leftLift.setPosition(0.4 + LiftOffset); - rightLift.setPosition(0.4); + leftLift.setPosition(0.8 + LiftOffset); + rightLift.setPosition(0.8); } else if (gamepad2.b) { - leftLift.setPosition(0.2 + LiftOffset); - rightLift.setPosition(0.2); + leftLift.setPosition(0.7 + LiftOffset); + rightLift.setPosition(0.7); } else if (gamepad2.a) { - leftLift.setPosition(0 + LiftOffset); - rightLift.setPosition(0); + leftLift.setPosition(0.5 + LiftOffset); + rightLift.setPosition(0.5); } } private void intakeFunction() { @@ -156,9 +156,9 @@ public void runOpMode() throws InterruptedException { launcher = hardwareMap.get(Servo.class, "launcher"); rightLift = hardwareMap.get(Servo.class, "rightLift"); leftLift = hardwareMap.get(Servo.class, "leftLift"); - shoulder = hardwareMap.get(Servo.class, "shoulder"); wrist = hardwareMap.get(Servo.class, "wrist"); hopper = hardwareMap.get(Servo.class, "hopper"); + shoulder = hardwareMap.get(Servo.class, "shoulder"); launcher.setDirection(Servo.Direction.REVERSE); @@ -174,8 +174,8 @@ public void runOpMode() throws InterruptedException { // servos launcher.setPosition(0.8); - leftLift.setPosition(0); - rightLift.setPosition(0); + leftLift.setPosition(0.5); + rightLift.setPosition(0.5); shoulder.setPosition(0.1); wrist.setPosition(-0.04); hopper.setPosition(0.01); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 55110c6484..6bd3224a1b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -210,7 +210,7 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { leftFront.setDirection(DcMotorEx.Direction.REVERSE); leftBack.setDirection(DcMotorEx.Direction.REVERSE); //rightFront.setDirection(DcMotorEx.Direction.REVERSE); - //rightBack.setDirection(DcMotorEx.Direction.REVERSE); + rightBack.setDirection(DcMotorEx.Direction.REVERSE); //TODO Make the same update in DriveLocalizer() function. Search for Step 4.2 //TODO End Step 4.1 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java index b2c5f279eb..05bfa1e1b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java @@ -15,20 +15,13 @@ public class OgDrive { DcMotor BackRight; DcMotor FrontLeft; DcMotor FrontRight; - Servo LiftRight; - Servo LiftLeft; public OgDrive(HardwareMap hardwareMap) { - BackLeft = hardwareMap.get(DcMotor.class, "leftRear"); - BackRight = hardwareMap.get(DcMotor.class, "rightRear"); + BackLeft = hardwareMap.get(DcMotor.class, "leftBack"); + BackRight = hardwareMap.get(DcMotor.class, "rightBack"); FrontLeft = hardwareMap.get(DcMotor.class, "leftFront"); FrontRight = hardwareMap.get(DcMotor.class, "rightFront"); - LiftRight = hardwareMap.get(Servo.class, "LiftRight"); - LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); - - - LiftRight.setDirection(Servo.Direction.REVERSE); // init servo hardware From 3fdb4a5c96750a046b48de26ce373c58a7e6bba4 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 4 Nov 2023 13:17:55 -0600 Subject: [PATCH 043/154] hardware issue???? code works on mcgreen, laucnger broken -goober --- .../org/firstinspires/ftc/teamcode/Beginnings.java | 14 +++++++++++++- .../firstinspires/ftc/teamcode/MecanumDrive.java | 4 ++-- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index c365f0f5ea..9ab0fce877 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -69,7 +69,15 @@ private void airplane() { launcher.setPosition(0.3); } else { - launcher.setPosition(0); + launcher.setPosition(0.8); + } + } + private void extralift() { + if (gamepad2.right_bumper) { + shoulder.setPosition(0.25); + } + else { + shoulder.setPosition(0.1); } } private void liftFunction() { @@ -160,6 +168,8 @@ public void runOpMode() throws InterruptedException { hopper = hardwareMap.get(Servo.class, "hopper"); shoulder = hardwareMap.get(Servo.class, "shoulder"); + // shoulder.setDirection(Servo.Direction.REVERSE); + launcher.setDirection(Servo.Direction.REVERSE); frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); @@ -181,6 +191,7 @@ public void runOpMode() throws InterruptedException { hopper.setPosition(0.01); + double SLOW_DOWN_FACTOR = 0.5; telemetry.addData("Initializing TeleOp",""); telemetry.update(); @@ -192,6 +203,7 @@ public void runOpMode() throws InterruptedException { while(opModeIsActive()){ driveCode(); airplane(); + extralift(); // ogDrive.og_drive_code(gamepad1, telemetry); intakeFunction(); liftFunction(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 6bd3224a1b..fde6b4c694 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -209,8 +209,8 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { //Uncomment the lines for which the motorDirection need to be reversed to ensure all motors run forward in test leftFront.setDirection(DcMotorEx.Direction.REVERSE); leftBack.setDirection(DcMotorEx.Direction.REVERSE); - //rightFront.setDirection(DcMotorEx.Direction.REVERSE); - rightBack.setDirection(DcMotorEx.Direction.REVERSE); + rightFront.setDirection(DcMotorEx.Direction.REVERSE); + //rightBack.setDirection(DcMotorEx.Direction.REVERSE); //TODO Make the same update in DriveLocalizer() function. Search for Step 4.2 //TODO End Step 4.1 From 1824f62d5a5673c2a652fc08bede44ffd998be40 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 4 Nov 2023 17:50:17 -0600 Subject: [PATCH 044/154] ok jett go merge -goober --- .../ftc/teamcode/Beginnings.java | 72 ++++++++++++++----- .../ftc/teamcode/MecanumDrive.java | 2 +- .../firstinspires/ftc/teamcode/OgDrive.java | 3 +- 3 files changed, 58 insertions(+), 19 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 9ab0fce877..2a6d2e963a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -64,22 +64,55 @@ private void setLiftHeight(double inputLiftHeight) { } + private void worm() { + if(gamepad2.left_bumper) { + shoulder.setPosition(0.445); + } + if(gamepad2.right_bumper) { + shoulder.setPosition(0.92); + } + } + private void airplane() { - if (gamepad2.dpad_down = true) { - launcher.setPosition(0.3); + if (gamepad2.dpad_down) { + launcher.setPosition(0.1); } else { launcher.setPosition(0.8); } } - private void extralift() { + private void tuneshoulder() { if (gamepad2.right_bumper) { - shoulder.setPosition(0.25); + shoulder.setPosition(shoulder.getPosition() + .01); + } else if (gamepad2.right_trigger > .5){ + shoulder.setPosition(shoulder.getPosition() - .01); + } else { + //shoulder.setPosition(0.1); } - else { - shoulder.setPosition(0.1); + telemetry.addData("shoulder", shoulder.getPosition()); + } + /* + private void tuneWrist() { + if (gamepad2.left_bumper) { + wrist.setPosition(wrist.getPosition() + .01); + } else if (gamepad2.left_trigger > .5){ + wrist.setPosition(wrist.getPosition() - .01); + } else { + //shoulder.setPosition(0.1); } + telemetry.addData("wrist", wrist.getPosition()); } + + private void jukeBeta() { + if (gamepad2.dpad_up) { + shoulder.setPosition(.48); + wrist.setPosition(.31); + } else if (gamepad2.dpad_down) { + shoulder.setPosition(.445); + wrist.setPosition(.26); + } + } + */ private void liftFunction() { if (gamepad2.y) { leftLift.setPosition(0.9 + LiftOffset); @@ -168,7 +201,9 @@ public void runOpMode() throws InterruptedException { hopper = hardwareMap.get(Servo.class, "hopper"); shoulder = hardwareMap.get(Servo.class, "shoulder"); - // shoulder.setDirection(Servo.Direction.REVERSE); + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + launcher.setDirection(Servo.Direction.REVERSE); @@ -184,12 +219,17 @@ public void runOpMode() throws InterruptedException { // servos launcher.setPosition(0.8); - leftLift.setPosition(0.5); - rightLift.setPosition(0.5); - shoulder.setPosition(0.1); - wrist.setPosition(-0.04); - hopper.setPosition(0.01); - + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + shoulder.setPosition(0.445); + wrist.setPosition(0.26); + hopper.setPosition(0.0); + /* + sleep(500); + leftLift.setPosition(0.8); + rightLift.setPosition(0.8); + wrist.setPosition(0.7); + */ double SLOW_DOWN_FACTOR = 0.5; @@ -201,13 +241,11 @@ public void runOpMode() throws InterruptedException { // servo_shenanigans(); // loop real while(opModeIsActive()){ - driveCode(); + //driveCode(); airplane(); - extralift(); - // ogDrive.og_drive_code(gamepad1, telemetry); + ogDrive.og_drive_code(gamepad1, telemetry); intakeFunction(); liftFunction(); - telemetry.update(); sleep(100); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index fde6b4c694..55110c6484 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -209,7 +209,7 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { //Uncomment the lines for which the motorDirection need to be reversed to ensure all motors run forward in test leftFront.setDirection(DcMotorEx.Direction.REVERSE); leftBack.setDirection(DcMotorEx.Direction.REVERSE); - rightFront.setDirection(DcMotorEx.Direction.REVERSE); + //rightFront.setDirection(DcMotorEx.Direction.REVERSE); //rightBack.setDirection(DcMotorEx.Direction.REVERSE); //TODO Make the same update in DriveLocalizer() function. Search for Step 4.2 //TODO End Step 4.1 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java index 05bfa1e1b2..a1353a1bc5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java @@ -30,7 +30,8 @@ public OgDrive(HardwareMap hardwareMap) { BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); FrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); FrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - BackRight.setDirection(DcMotorSimple.Direction.REVERSE); + //BackRight.setDirection(DcMotorSimple.Direction.REVERSE); + FrontRight.setDirection(DcMotorSimple.Direction.REVERSE); } public void og_drive_code(Gamepad gamepad1, Telemetry telemetry) { From b7e295d04bac65730adb51f9d24e0863cfbfbd25 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Sat, 4 Nov 2023 17:48:39 -0600 Subject: [PATCH 045/154] update v3 --- .../org/firstinspires/ftc/teamcode/TheJettCode.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java index d4bb3cf759..84898dc9ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java @@ -8,7 +8,7 @@ @TeleOp public class TheJettCode extends LinearOpMode { - private Servo LiftLeft; + private Servo Wrist; private Servo Hopper; double LiftHeight; boolean LiftMax; @@ -19,10 +19,10 @@ private void ServoNo() { @Override public void runOpMode() throws InterruptedException { - LiftLeft = hardwareMap.get(Servo.class, "LiftLeft"); + Wrist = hardwareMap.get(Servo.class, "Wrist"); Hopper = hardwareMap.get(Servo.class, "Hopper"); - LiftLeft.getController().pwmEnable(); + Wrist.getController().pwmEnable(); Hopper.getController().pwmEnable(); waitForStart(); @@ -36,9 +36,9 @@ public void runOpMode() throws InterruptedException { } - LiftLeft.setPosition(LiftHeight); + Wrist.setPosition(LiftHeight); Hopper.setPosition(LiftHeight); - telemetry.addData("LiftLeft", LiftLeft.getPosition()); + telemetry.addData("LiftLeft", Wrist.getPosition()); telemetry.addData("Hopper", Hopper.getPosition()); telemetry.addData("Lift non var", LiftHeight); ServoNo(); From d0953b19e0fc33cd22381f1c7e05ae234d9a9b40 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 4 Nov 2023 17:56:08 -0600 Subject: [PATCH 046/154] added wrist fix -goober --- .../main/java/org/firstinspires/ftc/teamcode/Beginnings.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 2a6d2e963a..f9ed7a69a0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -67,6 +67,7 @@ private void setLiftHeight(double inputLiftHeight) { private void worm() { if(gamepad2.left_bumper) { shoulder.setPosition(0.445); + wrist.setPosition(0.26); } if(gamepad2.right_bumper) { shoulder.setPosition(0.92); @@ -246,6 +247,7 @@ public void runOpMode() throws InterruptedException { ogDrive.og_drive_code(gamepad1, telemetry); intakeFunction(); liftFunction(); + worm(); telemetry.update(); sleep(100); } From e7f3d50c72f92a8e4826d51e0e8b393ce7043d9e Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Sat, 4 Nov 2023 17:55:52 -0600 Subject: [PATCH 047/154] update v4 --- .../main/java/org/firstinspires/ftc/teamcode/TheJettCode.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java index 84898dc9ad..b4ff2447a5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java @@ -19,8 +19,8 @@ private void ServoNo() { @Override public void runOpMode() throws InterruptedException { - Wrist = hardwareMap.get(Servo.class, "Wrist"); - Hopper = hardwareMap.get(Servo.class, "Hopper"); + Wrist = hardwareMap.get(Servo.class, "wrist"); + Hopper = hardwareMap.get(Servo.class, "hopper"); Wrist.getController().pwmEnable(); Hopper.getController().pwmEnable(); From 3f64a19301ee75615ec724fa25448c005c59f10b Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 5 Nov 2023 09:15:03 -0700 Subject: [PATCH 048/154] =?UTF-8?q?EL=20MC=20TUNERR=20=F0=9F=A7=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit -goober --- .../ftc/teamcode/Beginnings.java | 20 ++++---- .../ftc/teamcode/McTuner2000.java | 47 +++++++++++++++++++ 2 files changed, 57 insertions(+), 10 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index f9ed7a69a0..596c3ad76d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -84,9 +84,9 @@ private void airplane() { } private void tuneshoulder() { if (gamepad2.right_bumper) { - shoulder.setPosition(shoulder.getPosition() + .01); - } else if (gamepad2.right_trigger > .5){ - shoulder.setPosition(shoulder.getPosition() - .01); + shoulder.setPosition(shoulder.getPosition() + 0.01); + } else if (gamepad2.right_trigger > 0.5){ + shoulder.setPosition(shoulder.getPosition() - 0.01); } else { //shoulder.setPosition(0.1); } @@ -95,9 +95,9 @@ private void tuneshoulder() { /* private void tuneWrist() { if (gamepad2.left_bumper) { - wrist.setPosition(wrist.getPosition() + .01); - } else if (gamepad2.left_trigger > .5){ - wrist.setPosition(wrist.getPosition() - .01); + wrist.setPosition(wrist.getPosition() + 0.01); + } else if (gamepad2.left_trigger > 0.5){ + wrist.setPosition(wrist.getPosition() - 0.01); } else { //shoulder.setPosition(0.1); } @@ -106,11 +106,11 @@ private void tuneWrist() { private void jukeBeta() { if (gamepad2.dpad_up) { - shoulder.setPosition(.48); - wrist.setPosition(.31); + shoulder.setPosition(0.48); + wrist.setPosition(0.31); } else if (gamepad2.dpad_down) { - shoulder.setPosition(.445); - wrist.setPosition(.26); + shoulder.setPosition(0.445); + wrist.setPosition(0.26); } } */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java new file mode 100644 index 0000000000..51ab4aee5a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +@TeleOp +public class McTuner2000 extends LinearOpMode { + Servo wrist; + Servo hopper; + + private void wristTuner() { + if (gamepad1.y) { + wrist.setPosition(wrist.getPosition() + 0.02); + } + else if (gamepad1.a) { + wrist.setPosition(wrist.getPosition() - 0.02); + } + telemetry.addData("Wrist Position", wrist.getPosition()); + } + private void hopperTuner() { + if (gamepad1.x) { + hopper.setPosition(hopper.getPosition() + 0.02); + } + else if (gamepad1.b) { + hopper.setPosition(hopper.getPosition() - 0.02); + } + telemetry.addData("Hopper Position", hopper.getPosition()); + } + @Override + public void runOpMode() throws InterruptedException { + + wrist = hardwareMap.get(Servo.class, "wrist"); + hopper = hardwareMap.get(Servo.class, "hopper"); + telemetry.update(); + + telemetry.addData("Wrist Position", wrist.getPosition()); + telemetry.addData("Hopper Position", hopper.getPosition()); + waitForStart(); + + while(opModeIsActive()) { + wristTuner(); + hopperTuner(); + telemetry.update(); + } + } +} From 7f0d7256c343d3f4610ca2079bcf0e0abb36c267 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 5 Nov 2023 10:30:01 -0700 Subject: [PATCH 049/154] =?UTF-8?q?Mc=20tuner,=20but=20like,=20cooler=20by?= =?UTF-8?q?=20like=201000x,=20and=20in=20the=20year=203000,=20=F0=9F=8D=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit -goober --- .../ftc/teamcode/McTuner2000.java | 6 +- .../ftc/teamcode/McTuner3000.java | 128 ++++++++++++++++++ 2 files changed, 133 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java index 51ab4aee5a..004c0d2417 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java @@ -8,6 +8,7 @@ public class McTuner2000 extends LinearOpMode { Servo wrist; Servo hopper; + Servo shoulder; private void wristTuner() { if (gamepad1.y) { @@ -32,10 +33,13 @@ public void runOpMode() throws InterruptedException { wrist = hardwareMap.get(Servo.class, "wrist"); hopper = hardwareMap.get(Servo.class, "hopper"); - telemetry.update(); + + wrist.setDirection(Servo.Direction.REVERSE); telemetry.addData("Wrist Position", wrist.getPosition()); telemetry.addData("Hopper Position", hopper.getPosition()); + + telemetry.update(); waitForStart(); while(opModeIsActive()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java new file mode 100644 index 0000000000..3f2437082b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -0,0 +1,128 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.vision.apriltag.AprilTagCanvasAnnotator; + +@TeleOp +public class McTuner3000 extends LinearOpMode { + //TODO: Step 1, Replace all "wrist","hopper", etc with your servos + Servo wrist; + Servo hopper; + Servo shoulder; + Servo leftLift; + Servo rightLift; + + double speedAmount; + + enum ServoTypes{ + SHOULDER, + HOPPER, + WRIST, + LIFT; + } + ServoTypes which; + + private void masterTuner() { + if (gamepad1.left_bumper) { + if (which == ServoTypes.SHOULDER) { + shoulder.setPosition(shoulder.getPosition() - speedAmount); + } + else if (which == ServoTypes.HOPPER) { + hopper.setPosition(hopper.getPosition() - speedAmount); + } + else if (which == ServoTypes.WRIST) { + wrist.setPosition(wrist.getPosition() - speedAmount); + } + else if (which == ServoTypes.LIFT) { + leftLift.setPosition(leftLift.getPosition() - speedAmount); + rightLift.setPosition(rightLift.getPosition() - speedAmount); + } + } + else if (gamepad1.right_bumper) { + if (which == ServoTypes.SHOULDER) { + shoulder.setPosition(shoulder.getPosition() + speedAmount); + } + else if (which == ServoTypes.HOPPER) { + hopper.setPosition(hopper.getPosition() + speedAmount); + } + else if (which == ServoTypes.WRIST) { + wrist.setPosition(wrist.getPosition() + speedAmount); + } + else if (which == ServoTypes.LIFT) { + leftLift.setPosition(leftLift.getPosition() + speedAmount); + rightLift.setPosition(rightLift.getPosition() + speedAmount); + } + } + } + private void setServo() { + if (gamepad1.y) { + which = ServoTypes.SHOULDER; + } + else if (gamepad1.x) { + which = ServoTypes.HOPPER; + } + else if (gamepad1.b) { + which = ServoTypes.WRIST; + } + else if (gamepad1.a) { + which = ServoTypes.LIFT; + } + telemetry.addData("Selected Servo = ", which.toString()); + } + private void setSpeed() { + if (gamepad1.dpad_down) { + speedAmount = 0; + } + else if (gamepad1.dpad_left) { + speedAmount = 0.01; + } + else if (gamepad1.dpad_right) { + speedAmount = 0.02; + } + else if (gamepad1.dpad_up) { + speedAmount = 0.05; + } + telemetry.addData("Speed = ", speedAmount); + } + private void whatServoAt() { + telemetry.addData("Shoulder = ", shoulder.getPosition()); + telemetry.addData("Hopper = ", hopper.getPosition()); + telemetry.addData("Wrist = ", wrist.getPosition()); + telemetry.addData("Lift Left = ",leftLift.getPosition()); + telemetry.addData("Lift Right = ",rightLift.getPosition()); + } + @Override + public void runOpMode() throws InterruptedException { + which = ServoTypes.SHOULDER; + speedAmount = 0.01; + + //TODO: Step 2, Replace the device names with your 4 (or more if you use two servos for one task) + wrist = hardwareMap.get(Servo.class, "wrist"); + hopper = hardwareMap.get(Servo.class, "hopper"); + shoulder = hardwareMap.get(Servo.class, "shoulder"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + + telemetry.addData("Wrist Position", wrist.getPosition()); + telemetry.addData("Hopper Position", hopper.getPosition()); + + telemetry.update(); + waitForStart(); + + while(opModeIsActive()) { + setServo(); + masterTuner(); + setSpeed(); + whatServoAt(); + telemetry.addData("Selected", which.toString()); + telemetry.addLine("Y = Shoulder - X = Hopper - B = Wrist A = Lift"); + telemetry.update(); + } + } +} From bed6aa367c06ab7c8a17c61af713de1713af586e Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 5 Nov 2023 10:38:03 -0700 Subject: [PATCH 050/154] Making the todo list for McTuner3000 was unnecessary, fun, but unnecessary. -goober --- .../java/org/firstinspires/ftc/teamcode/McTuner3000.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java index 3f2437082b..cc456e884b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -17,6 +17,7 @@ public class McTuner3000 extends LinearOpMode { double speedAmount; + //TODO: Step 3, set all of your servo names below enum ServoTypes{ SHOULDER, HOPPER, @@ -24,6 +25,7 @@ enum ServoTypes{ LIFT; } ServoTypes which; + //TODO: Step 4, replace all names of Servos with yours, and replace all capitals with what you set them to from step 3 private void masterTuner() { if (gamepad1.left_bumper) { @@ -57,6 +59,7 @@ else if (which == ServoTypes.LIFT) { } } } + //TODO: Step 5, replace all of your Servo functions below private void setServo() { if (gamepad1.y) { which = ServoTypes.SHOULDER; @@ -87,6 +90,8 @@ else if (gamepad1.dpad_up) { } telemetry.addData("Speed = ", speedAmount); } + //TODO: Step 6 replace all of the xyz.getPosition()0; with your servos and replace "xyz" with what that servo is + //TODO: Step 7, your done! This was written by Goober on 11/5/23 slouching in a chair at 10:35 in the morning. And as I'm writing this I wonder if anybody will actually use this. Problably not, but idk what to do while I wait for the robot to be ready for calibration. It's now 10:36 am. I wonder if future me will use this and or remember doing this. Goober out. private void whatServoAt() { telemetry.addData("Shoulder = ", shoulder.getPosition()); telemetry.addData("Hopper = ", hopper.getPosition()); From 9660073ca31d5d672c170684762c9c08cd375a71 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 5 Nov 2023 10:40:47 -0700 Subject: [PATCH 051/154] Small change. Made really long text into multible lines -goober --- .../java/org/firstinspires/ftc/teamcode/McTuner3000.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java index cc456e884b..8655bb0f78 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -90,8 +90,13 @@ else if (gamepad1.dpad_up) { } telemetry.addData("Speed = ", speedAmount); } + + //TODO: Step 7, your done! This was written by Goober on 11/5/23 slouching in a chair at 10:35 in the morning. + // And as I'm writing this I wonder if anybody will actually use this. Problably not, + // but idk what to do while I wait for the robot to be ready for calibration. It's now 10:36 am. + // I wonder if future me will use this and or remember doing this. Goober out. + //TODO: Step 6 replace all of the xyz.getPosition()0; with your servos and replace "xyz" with what that servo is - //TODO: Step 7, your done! This was written by Goober on 11/5/23 slouching in a chair at 10:35 in the morning. And as I'm writing this I wonder if anybody will actually use this. Problably not, but idk what to do while I wait for the robot to be ready for calibration. It's now 10:36 am. I wonder if future me will use this and or remember doing this. Goober out. private void whatServoAt() { telemetry.addData("Shoulder = ", shoulder.getPosition()); telemetry.addData("Hopper = ", hopper.getPosition()); From 1c7266cb4ebf90e9dbb5f0dce01c35f297b86deb Mon Sep 17 00:00:00 2001 From: Gabriel Date: Sun, 5 Nov 2023 10:45:41 -0700 Subject: [PATCH 052/154] remerge last year's code for budster's ogdrive --- .../java/org/firstinspires/ftc/teamcode/OgDrive.java | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java index a1353a1bc5..df2e420c09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java @@ -43,10 +43,17 @@ public void og_drive_code(Gamepad gamepad1, Telemetry telemetry) { Scale_Factor_of_Drive = 0.55; } // drive with joysticks - BackLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); - BackRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); + /* FrontLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + BackLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + BackRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); + */ + FrontLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (-gamepad1.left_stick_x + gamepad1.left_stick_y)); + BackLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (-gamepad1.left_stick_x + gamepad1.left_stick_y)); + BackRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + telemetry.addData("FL Motor", FrontLeft.getPower()); telemetry.addData("FR Motor", FrontRight.getPower()); telemetry.addData("BL Motor", BackLeft.getPower()); From 4ecf4ac88324435dada9e4a4fad1f7239a76dfe4 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 5 Nov 2023 13:04:45 -0700 Subject: [PATCH 053/154] Starting stop motion and fixed drive -goober --- .../org/firstinspires/ftc/teamcode/Beginnings.java | 13 +++++++------ .../org/firstinspires/ftc/teamcode/McTuner3000.java | 9 +++++++++ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 596c3ad76d..e621a8f8b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -5,6 +5,7 @@ import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -220,11 +221,11 @@ public void runOpMode() throws InterruptedException { // servos launcher.setPosition(0.8); + wrist.setPosition(0.51); + shoulder.setPosition(0.44); + hopper.setPosition(0); leftLift.setPosition(0.42); rightLift.setPosition(0.42); - shoulder.setPosition(0.445); - wrist.setPosition(0.26); - hopper.setPosition(0.0); /* sleep(500); leftLift.setPosition(0.8); @@ -243,11 +244,11 @@ public void runOpMode() throws InterruptedException { // loop real while(opModeIsActive()){ //driveCode(); - airplane(); + //airplane(); ogDrive.og_drive_code(gamepad1, telemetry); intakeFunction(); - liftFunction(); - worm(); + //liftFunction(); + //worm(); telemetry.update(); sleep(100); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java index 8655bb0f78..5fe3e22f17 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -122,6 +122,14 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Wrist Position", wrist.getPosition()); telemetry.addData("Hopper Position", hopper.getPosition()); + wrist.setPosition(0.51); + shoulder.setPosition(0.44); + hopper.setPosition(0); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + + + telemetry.update(); waitForStart(); @@ -133,6 +141,7 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Selected", which.toString()); telemetry.addLine("Y = Shoulder - X = Hopper - B = Wrist A = Lift"); telemetry.update(); + sleep(100); } } } From e03ef293778310ea29e856e7047e2a43d36707d7 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 5 Nov 2023 13:04:53 -0700 Subject: [PATCH 054/154] Starting stop motion and fixed drive -goober --- .../firstinspires/ftc/teamcode/OgDrive.java | 11 ++-- .../ftc/teamcode/StopMotionTester.java | 50 +++++++++++++++++++ 2 files changed, 56 insertions(+), 5 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java index df2e420c09..c14cd21883 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java @@ -43,16 +43,17 @@ public void og_drive_code(Gamepad gamepad1, Telemetry telemetry) { Scale_Factor_of_Drive = 0.55; } // drive with joysticks - /* - FrontLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); - FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); - BackLeft.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + + FrontLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); + FrontRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); + BackLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); BackRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); - */ + /* FrontLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (-gamepad1.left_stick_x + gamepad1.left_stick_y)); BackLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (-gamepad1.left_stick_x + gamepad1.left_stick_y)); BackRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); +*/ telemetry.addData("FL Motor", FrontLeft.getPower()); telemetry.addData("FR Motor", FrontRight.getPower()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java new file mode 100644 index 0000000000..592187bc54 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode; + +import static android.os.SystemClock.sleep; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +public class StopMotionTester { + + static public void Dominator(HardwareMap hardwareMap) { + Servo wrist; + Servo hopper; + Servo shoulder; + Servo leftLift; + Servo rightLift; + + wrist = hardwareMap.get(Servo.class, "wrist"); + hopper = hardwareMap.get(Servo.class, "hopper"); + shoulder = hardwareMap.get(Servo.class, "shoulder"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + + wrist.setPosition(0.51); + shoulder.setPosition(0.44); + hopper.setPosition(0); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + + sleep(2000); + + wrist.setPosition(0.74); + shoulder.setPosition(0.7); + hopper.setPosition(0); + + sleep(2000); + + wrist.setPosition(0.73); + shoulder.setPosition(0.75); + hopper.setPosition(0.16); + + sleep(2000); + + + + } +} From 7111892885f60f072116919b65677b97780a6885 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Mon, 6 Nov 2023 17:19:53 -0700 Subject: [PATCH 055/154] update git pls -goober --- .../main/java/org/firstinspires/ftc/teamcode/Beginnings.java | 2 +- .../main/java/org/firstinspires/ftc/teamcode/McTuner2000.java | 1 + .../main/java/org/firstinspires/ftc/teamcode/McTuner3000.java | 1 + .../src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java | 1 + .../java/org/firstinspires/ftc/teamcode/StopMotionTester.java | 1 + 5 files changed, 5 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index e621a8f8b4..d6c9d6aec1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -233,7 +233,7 @@ public void runOpMode() throws InterruptedException { wrist.setPosition(0.7); */ - + //this is a coment to mAKE git update double SLOW_DOWN_FACTOR = 0.5; telemetry.addData("Initializing TeleOp",""); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java index 004c0d2417..1d6981c9b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java @@ -41,6 +41,7 @@ public void runOpMode() throws InterruptedException { telemetry.update(); waitForStart(); + //this is a coment to mAKE git update while(opModeIsActive()) { wristTuner(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java index 5fe3e22f17..8667f76b98 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -108,6 +108,7 @@ private void whatServoAt() { public void runOpMode() throws InterruptedException { which = ServoTypes.SHOULDER; speedAmount = 0.01; + //this is a coment to mAKE git update //TODO: Step 2, Replace the device names with your 4 (or more if you use two servos for one task) wrist = hardwareMap.get(Servo.class, "wrist"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java index c14cd21883..bea317ac23 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java @@ -42,6 +42,7 @@ public void og_drive_code(Gamepad gamepad1, Telemetry telemetry) { } else { Scale_Factor_of_Drive = 0.55; } + //this is a coment to mAKE git update // drive with joysticks FrontLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java index 592187bc54..3be96b4afd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java @@ -43,6 +43,7 @@ static public void Dominator(HardwareMap hardwareMap) { hopper.setPosition(0.16); sleep(2000); + //this is a coment to mAKE git update From 9c6fcd8a49e49f5aab84b9c2b0984c28feed47eb Mon Sep 17 00:00:00 2001 From: Gabriel Date: Tue, 7 Nov 2023 17:37:23 -0700 Subject: [PATCH 056/154] isabels drive code --- .../firstinspires/ftc/teamcode/OdoMec.java | 88 +++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java new file mode 100644 index 0000000000..2cf3101819 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -0,0 +1,88 @@ +package org.firstinspires.ftc.teamcode; +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.rev.RevTouchSensor; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +@TeleOp(name="TeleMec") + +public class OdoMec extends LinearOpMode { + private ElapsedTime runtime = new ElapsedTime(); + //Motors + //Drivetrain + private DcMotor rightFront; //front right 0 + private DcMotor leftFront; //front left 2 + private DcMotor rightBack; //rear right 1 + private DcMotor leftBack; //rear left 3 + + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + rightFront = hardwareMap.get(DcMotor.class, "rightFront"); + leftFront = hardwareMap.get(DcMotor.class, "leftFront"); + rightBack = hardwareMap.get(DcMotor.class, "rightBack"); + leftBack = hardwareMap.get(DcMotor.class, "leftBack"); + + rightFront.setDirection(DcMotor.Direction.REVERSE); + leftFront.setDirection(DcMotor.Direction.FORWARD); + rightBack.setDirection(DcMotor.Direction.REVERSE); + leftBack.setDirection(DcMotor.Direction.FORWARD); + + //Set motor modes + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + + telemetry.addData("Status", "OdoMec2 is ready to run!"); + telemetry.update(); + + //Wait for press play + waitForStart(); + runtime.reset(); + while (opModeIsActive()) { + //Drivetrain + double forward = gamepad1.left_stick_y; + double strafe = -gamepad1.left_stick_x; + double turn = -gamepad1.right_stick_x; + + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); + + double rightFrontPower = (forward - strafe - turn) / denominator; + double leftFrontPower = (forward + strafe + turn) / denominator; + double rightBackPower = (forward + strafe - turn) / denominator; + double leftBackPower = (forward - strafe + turn) / denominator; + + if (gamepad1.left_bumper) { + rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); + leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); + rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); + leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); + } else { + rightFrontPower = Range.clip(rightFrontPower, -0.8, 0.8); + leftFrontPower = Range.clip(leftFrontPower, -0.8, 0.8); + rightBackPower = Range.clip(rightBackPower, -0.8, 0.8); + leftBackPower = Range.clip(leftBackPower, -0.8, 0.8); + } + + + rightFront.setPower(rightFrontPower); + leftFront.setPower(leftFrontPower); + rightBack.setPower(rightBackPower); + leftBack.setPower(leftBackPower); + + telemetry.addData("Status", "Run " + runtime.toString()); + telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); + telemetry.update(); + + + // run until the end of + } + } +} From b750289ce5c63d53fb2ec511393e760bad5cfc2d Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 7 Nov 2023 17:45:25 -0700 Subject: [PATCH 057/154] Adding Russ's drive. -goober --- .../ftc/teamcode/Beginnings.java | 88 +++++++++++++++++-- .../firstinspires/ftc/teamcode/IsDrive.java | 68 ++++++++++++++ .../firstinspires/ftc/teamcode/OgDrive.java | 10 ++- .../ftc/teamcode/StopMotionTester.java | 7 +- 4 files changed, 160 insertions(+), 13 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/IsDrive.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index d6c9d6aec1..8225b4ce30 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -5,6 +5,7 @@ import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -12,6 +13,8 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; import org.firstinspires.ftc.teamcode.tuning.TuningOpModes; @@ -19,15 +22,25 @@ @TeleOp public class Beginnings extends LinearOpMode { // Declare vars - + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; private Servo launcher; private Servo rightLift; private Servo leftLift; private Servo shoulder; private Servo wrist; private Servo hopper; - MecanumDrive drive; - OgDrive ogDrive; + //MecanumDrive drive; + //OgDrive ogDrive; + private ElapsedTime runtime = new ElapsedTime(); + //Motors + //Drivetrain + private DcMotor rightFront; //front right 0 + private DcMotor leftFront; //front left 2 + private DcMotor rightBack; //rear right 1 + private DcMotor leftBack; //rear left 3 DcMotor frontIntake; DcMotor rearIntake; @@ -152,7 +165,7 @@ else if (gamepad1.right_bumper) { } - +/* private void driveCode() { double SLOW_DOWN_FACTOR; @@ -180,7 +193,7 @@ private void driveCode() { // telemetry.update(); } - +*/ @Override @@ -191,10 +204,34 @@ public void runOpMode() throws InterruptedException { androidTextToSpeech.initialize(); // for wires driving - drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + //drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); // for Og driving (the best driving) - ogDrive = new OgDrive(hardwareMap); + //ogDrive = new OgDrive(hardwareMap); + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + rightFront = hardwareMap.get(DcMotor.class, "rightFront"); + leftFront = hardwareMap.get(DcMotor.class, "leftFront"); + rightBack = hardwareMap.get(DcMotor.class, "rightBack"); + leftBack = hardwareMap.get(DcMotor.class, "leftBack"); + + rightFront.setDirection(DcMotor.Direction.REVERSE); + leftFront.setDirection(DcMotor.Direction.FORWARD); + rightBack.setDirection(DcMotor.Direction.REVERSE); + leftBack.setDirection(DcMotor.Direction.FORWARD); + + //Set motor modes + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + + telemetry.addData("Status", "OdoMec2 is ready to run!"); + telemetry.update(); + + launcher = hardwareMap.get(Servo.class, "launcher"); rightLift = hardwareMap.get(Servo.class, "rightLift"); @@ -240,12 +277,47 @@ public void runOpMode() throws InterruptedException { waitForStart(); + runtime.reset(); // servo_shenanigans(); // loop real while(opModeIsActive()){ + //Drivetrain + double forward = gamepad1.left_stick_y; + double strafe = -gamepad1.left_stick_x; + double turn = -gamepad1.right_stick_x; + + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); + + double rightFrontPower = (forward - strafe - turn) / denominator; + double leftFrontPower = (forward + strafe + turn) / denominator; + double rightBackPower = (forward + strafe - turn) / denominator; + double leftBackPower = (forward - strafe + turn) / denominator; + + if (gamepad1.left_bumper) { + rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); + leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); + rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); + leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); + } else { + rightFrontPower = Range.clip(rightFrontPower, -0.8, 0.8); + leftFrontPower = Range.clip(leftFrontPower, -0.8, 0.8); + rightBackPower = Range.clip(rightBackPower, -0.8, 0.8); + leftBackPower = Range.clip(leftBackPower, -0.8, 0.8); + } + + + rightFront.setPower(rightFrontPower); + leftFront.setPower(leftFrontPower); + rightBack.setPower(rightBackPower); + leftBack.setPower(leftBackPower); + + telemetry.addData("Status", "Run " + runtime.toString()); + telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); + //driveCode(); //airplane(); - ogDrive.og_drive_code(gamepad1, telemetry); + //ogDrive.og_drive_code(gamepad1, telemetry); + //IsDrive.is_drive_code(gamepad1, telemetry); intakeFunction(); //liftFunction(); //worm(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/IsDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/IsDrive.java new file mode 100644 index 0000000000..3776977d59 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/IsDrive.java @@ -0,0 +1,68 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + + +public class IsDrive { + DcMotor BackLeft; + DcMotor BackRight; + DcMotor FrontLeft; + DcMotor FrontRight; + + public IsDrive(HardwareMap hardwareMap) { + + BackLeft = hardwareMap.get(DcMotor.class, "leftBack"); + BackRight = hardwareMap.get(DcMotor.class, "rightBack"); + FrontLeft = hardwareMap.get(DcMotor.class, "leftFront"); + FrontRight = hardwareMap.get(DcMotor.class, "rightFront"); + + + // init servo hardware + // init drive hardware and variables + BackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + FrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + FrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + //FrontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + //BackLeft.setDirection(DcMotorSimple.Direction.REVERSE); + BackRight.setDirection(DcMotorSimple.Direction.REVERSE); + FrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + } + + public void is_drive_code(Gamepad gamepad1, Telemetry telemetry) { + double forward = gamepad1.left_stick_y; + double strafe = -gamepad1.left_stick_x; + double turn = -gamepad1.right_stick_x; + + double denominator = Math.max(Math.abs(forward)+Math.abs(strafe)+Math.abs(turn), 1); + + double rightFrontPower = (forward - strafe - turn) / denominator; + double leftFrontPower = (forward + strafe + turn) / denominator; + double rightRearPower = (forward + strafe - turn) / denominator; + double leftRearPower = (forward - strafe + turn) / denominator; + + if (gamepad1.left_bumper) { + rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); + leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); + rightRearPower = Range.clip(rightRearPower, -0.4, 0.4); + leftRearPower = Range.clip(leftRearPower, -0.4, 0.4); + } else { + rightFrontPower = Range.clip(rightFrontPower, -0.8, 0.8); + leftFrontPower = Range.clip(leftFrontPower, -0.8, 0.8); + rightRearPower = Range.clip(rightRearPower, -0.8, 0.8); + leftRearPower = Range.clip(leftRearPower, -0.8, 0.8); + } + + + FrontRight.setPower(rightFrontPower); + FrontLeft.setPower(leftFrontPower); + BackRight.setPower(rightRearPower); + BackLeft.setPower(leftRearPower); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java index bea317ac23..72c82c7822 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OgDrive.java @@ -30,7 +30,9 @@ public OgDrive(HardwareMap hardwareMap) { BackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); FrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); FrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - //BackRight.setDirection(DcMotorSimple.Direction.REVERSE); + //FrontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + //BackLeft.setDirection(DcMotorSimple.Direction.REVERSE); + BackRight.setDirection(DcMotorSimple.Direction.REVERSE); FrontRight.setDirection(DcMotorSimple.Direction.REVERSE); } public void og_drive_code(Gamepad gamepad1, Telemetry telemetry) { @@ -44,17 +46,17 @@ public void og_drive_code(Gamepad gamepad1, Telemetry telemetry) { } //this is a coment to mAKE git update // drive with joysticks - + /* FrontLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); FrontRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); BackLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); BackRight.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x - -0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x - gamepad1.left_stick_y)); - /* + */ FrontLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); FrontRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (-gamepad1.left_stick_x + gamepad1.left_stick_y)); BackLeft.setPower(0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (-gamepad1.left_stick_x + gamepad1.left_stick_y)); BackRight.setPower(-0.8 * Scale_Factor_of_Drive * gamepad1.right_stick_x + 0.8 * Scale_Factor_of_Drive * (gamepad1.left_stick_x + gamepad1.left_stick_y)); -*/ + telemetry.addData("FL Motor", FrontLeft.getPower()); telemetry.addData("FR Motor", FrontRight.getPower()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java index 3be96b4afd..1412b1634b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java @@ -5,9 +5,14 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.hardware.rev.RevTouchSensor; public class StopMotionTester { - + //sensors + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; static public void Dominator(HardwareMap hardwareMap) { Servo wrist; Servo hopper; From 1805d81df1c4870a2166d712c15e53fa92ff6c8c Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 7 Nov 2023 19:13:08 -0700 Subject: [PATCH 058/154] Home and drive button -goober --- .../ftc/teamcode/Beginnings.java | 48 +++++++++++++++++++ .../ftc/teamcode/StopMotionTester.java | 11 +++-- 2 files changed, 56 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 8225b4ce30..b453543fda 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -2,6 +2,8 @@ // imports +import static android.os.SystemClock.sleep; + import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.Vector2d; @@ -164,6 +166,49 @@ else if (gamepad1.right_bumper) { } } + private void aroundthetop() { + if (gamepad2.start) { + shoulder.setPosition(0.56); + wrist.setPosition(0.5); + hopper.setPosition(0.4); + } + } + private void driveAroundPos() { + if (gamepad1.a) { + shoulder.setPosition(0.455); + sleep(653); + shoulder.setPosition(0.47); + wrist.setPosition(0.55); + sleep(531); + shoulder.setPosition(0.49); + wrist.setPosition(0.59); + } + if (gamepad2.dpad_up) { + wrist.setPosition(0.51); + shoulder.setPosition(0.44); + } + } + private void stopMotion() { + if(gamepad1.dpad_left) { + wrist.setPosition(0.51); + shoulder.setPosition(0.44); + hopper.setPosition(0); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + + sleep(2000); + + wrist.setPosition(0.74); + shoulder.setPosition(0.7); + hopper.setPosition(0); + + sleep(2000); + + wrist.setPosition(0.73); + shoulder.setPosition(0.75); + hopper.setPosition(0.16); + } + } /* private void driveCode() { @@ -319,8 +364,11 @@ public void runOpMode() throws InterruptedException { //ogDrive.og_drive_code(gamepad1, telemetry); //IsDrive.is_drive_code(gamepad1, telemetry); intakeFunction(); + driveAroundPos(); //liftFunction(); //worm(); + stopMotion(); + aroundthetop(); telemetry.update(); sleep(100); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java index 1412b1634b..671c02fcfc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StopMotionTester.java @@ -7,6 +7,7 @@ import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.hardware.rev.RevTouchSensor; + public class StopMotionTester { //sensors private RevTouchSensor rightUpper; @@ -35,19 +36,23 @@ static public void Dominator(HardwareMap hardwareMap) { leftLift.setPosition(0.42); rightLift.setPosition(0.42); - sleep(2000); + sleep(1200); wrist.setPosition(0.74); shoulder.setPosition(0.7); hopper.setPosition(0); - sleep(2000); + sleep(1200); wrist.setPosition(0.73); shoulder.setPosition(0.75); hopper.setPosition(0.16); - sleep(2000); + sleep(1200); + + wrist.setPosition(0.94); + shoulder.setPosition(0.91); + hopper.setPosition(0.54); //this is a coment to mAKE git update From a68aee73dfe33cc3edac5bcddf23cd528c0124b0 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Tue, 7 Nov 2023 19:23:42 -0700 Subject: [PATCH 059/154] update v5 --- .../org/firstinspires/ftc/teamcode/Beginnings.java | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index b453543fda..49a255a75b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -168,9 +168,14 @@ else if (gamepad1.right_bumper) { } private void aroundthetop() { if (gamepad2.start) { - shoulder.setPosition(0.56); - wrist.setPosition(0.5); - hopper.setPosition(0.4); + shoulder.setPosition(0.44); + sleep(400); + wrist.setPosition(0.53); + hopper.setPosition(0.33); + sleep(500); + wrist.setPosition(0.93); + hopper.setPosition(0.57); + } } private void driveAroundPos() { From ca86288a396b611a02da8bbeb001e5eb82afa590 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 7 Nov 2023 19:27:25 -0700 Subject: [PATCH 060/154] dump prep -goober --- .../org/firstinspires/ftc/teamcode/Beginnings.java | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index b453543fda..9fc4852f09 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -173,6 +173,15 @@ private void aroundthetop() { hopper.setPosition(0.4); } } + private void dumpPrep() { + if (gamepad2.back) { + wrist.setPosition(0.37); + shoulder.setPosition(0.91); + sleep(2345); + hopper.setPosition(0.57); + wrist.setPosition(0.93); + } + } private void driveAroundPos() { if (gamepad1.a) { shoulder.setPosition(0.455); @@ -365,6 +374,7 @@ public void runOpMode() throws InterruptedException { //IsDrive.is_drive_code(gamepad1, telemetry); intakeFunction(); driveAroundPos(); + dumpPrep(); //liftFunction(); //worm(); stopMotion(); From 592bdc80fbad422cb87d409e06231479432ec4b6 Mon Sep 17 00:00:00 2001 From: digitalbreadllc Date: Wed, 8 Nov 2023 09:24:08 -0700 Subject: [PATCH 061/154] Adding OdoMec --- .../src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java | 4 ---- 1 file changed, 4 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java deleted file mode 100644 index 01b5990353..0000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -public class OdoMec { -} From 530f8bf27634e98bbf5ab9ba17c5c8d41a60ba28 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 8 Nov 2023 13:43:45 -0700 Subject: [PATCH 062/154] Enigma Auto Work --- .../ftc/teamcode/EnigmaAuto.java | 389 ++++++++++++++++++ 1 file changed, 389 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java new file mode 100644 index 0000000000..4f543bfdb8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -0,0 +1,389 @@ +/* Copyright (c) 2019 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 static com.qualcomm.robotcore.util.ElapsedTime.Resolution.SECONDS; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.tfod.TfodProcessor; + +import java.util.List; + +/** + * FTC WIRES Autonomous Example for only vision detection using tensorflow and park + */ +@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "ENIGMA TeleOp") +public class EnigmaAuto extends LinearOpMode { + + public static String TEAM_NAME = "ENIGMA"; //TODO: Enter team Name + public static int TEAM_NUMBER = 16265; //TODO: Enter team Number + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + //Vision parameters + private TfodProcessor tfod; + private VisionPortal visionPortal; + + //Define and declare Robot Starting Locations + public enum START_POSITION{ + BLUE_LEFT, + BLUE_RIGHT, + RED_LEFT, + RED_RIGHT + } + public static START_POSITION startPosition; + + public enum IDENTIFIED_SPIKE_MARK_LOCATION { + LEFT, + MIDDLE, + RIGHT + } + public static IDENTIFIED_SPIKE_MARK_LOCATION identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; + + @Override + public void runOpMode() throws InterruptedException { + + //Key Pay inputs to selecting Starting Position of robot + selectStartingPosition(); + telemetry.addData("Selected Starting Position", startPosition); + + //Activate Camera Vision that uses TensorFlow for pixel detection + initTfod(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.update(); + //waitForStart(); + + while (!isStopRequested() && !opModeIsActive()) { + telemetry.addData("Selected Starting Position", startPosition); + + //Run Vuforia Tensor Flow and keep watching for the identifier in the Signal Cone. + runTfodTensorFlow(); + telemetry.addData("Vision identified Parking Location", identifiedSpikeMarkLocation); + telemetry.update(); + } + + //Game Play Button is pressed + if (opModeIsActive() && !isStopRequested()) { + //Build parking trajectory based on last detected target by vision + runAutonoumousMode(); + } + } // end runOpMode() + + public void runAutonoumousMode() { + //Initialize Pose2d as desired + Pose2d initPose = new Pose2d(0, 0, 0); // Starting Pose + Pose2d moveBeyondTrussPose = new Pose2d(0,0,0); + Pose2d dropPurplePixelPose = new Pose2d(0, 0, 0); + Pose2d midwayPose1 = new Pose2d(0,0,0); + Pose2d midwayPose1a = new Pose2d(0,0,0); + Pose2d intakeStack = new Pose2d(0,0,0); + Pose2d midwayPose2 = new Pose2d(0,0,0); + Pose2d dropYellowPixelPose = new Pose2d(0, 0, 0); + Pose2d parkPose = new Pose2d(0,0, 0); + double waitSecondsBeforeDrop = 0; + MecanumDrive drive = new MecanumDrive(hardwareMap, initPose); + + initPose = new Pose2d(0, 0, Math.toRadians(0)); //Starting pose + moveBeyondTrussPose = new Pose2d(15,0,0); + + switch (startPosition) { + case BLUE_LEFT: + drive = new MecanumDrive(hardwareMap, initPose); + switch(identifiedSpikeMarkLocation){ + case LEFT: + dropPurplePixelPose = new Pose2d(26, 8, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(23, 36, Math.toRadians(-90)); + break; + case MIDDLE: + dropPurplePixelPose = new Pose2d(30, 3, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(30, 36, Math.toRadians(-90)); + break; + case RIGHT: + dropPurplePixelPose = new Pose2d(30, -9, Math.toRadians(-45)); + dropYellowPixelPose = new Pose2d(37, 36, Math.toRadians(-90)); + break; + } + midwayPose1 = new Pose2d(14, 13, Math.toRadians(-45)); + waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + parkPose = new Pose2d(8, 30, Math.toRadians(-90)); + break; + + case RED_RIGHT: + drive = new MecanumDrive(hardwareMap, initPose); + switch(identifiedSpikeMarkLocation){ + case LEFT: + dropPurplePixelPose = new Pose2d(30, 9, Math.toRadians(45)); + dropYellowPixelPose = new Pose2d(21, -36, Math.toRadians(90)); + break; + case MIDDLE: + dropPurplePixelPose = new Pose2d(30, -3, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(29, -36, Math.toRadians(90)); + break; + case RIGHT: + dropPurplePixelPose = new Pose2d(26, -8, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(37, -36, Math.toRadians(90)); + break; + } + midwayPose1 = new Pose2d(14, -13, Math.toRadians(45)); + waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + parkPose = new Pose2d(8, -30, Math.toRadians(90)); + break; + + case BLUE_RIGHT: + drive = new MecanumDrive(hardwareMap, initPose); + switch(identifiedSpikeMarkLocation){ + case LEFT: + dropPurplePixelPose = new Pose2d(27, 9, Math.toRadians(45)); + dropYellowPixelPose = new Pose2d(27, 86, Math.toRadians(-90)); + break; + case MIDDLE: + dropPurplePixelPose = new Pose2d(30, -3, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(34, 86, Math.toRadians(-90)); + break; + case RIGHT: + dropPurplePixelPose = new Pose2d(26, -8, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(43, 86, Math.toRadians(-90)); + break; + } + midwayPose1 = new Pose2d(8, -8, Math.toRadians(0)); + midwayPose1a = new Pose2d(18, -18, Math.toRadians(-90)); + intakeStack = new Pose2d(52, -19,Math.toRadians(-90)); + midwayPose2 = new Pose2d(52, 62, Math.toRadians(-90)); + waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + parkPose = new Pose2d(50, 84, Math.toRadians(-90)); + break; + + case RED_LEFT: + drive = new MecanumDrive(hardwareMap, initPose); + switch(identifiedSpikeMarkLocation){ + case LEFT: + dropPurplePixelPose = new Pose2d(26, 8, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(37, -86, Math.toRadians(90)); + break; + case MIDDLE: + dropPurplePixelPose = new Pose2d(30, -3, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(29, -86, Math.toRadians(90)); + break; + case RIGHT: + dropPurplePixelPose = new Pose2d(27, -9, Math.toRadians(-45)); + dropYellowPixelPose = new Pose2d(21, -86, Math.toRadians(90)); + break; + } + midwayPose1 = new Pose2d(8, 8, Math.toRadians(0)); + midwayPose1a = new Pose2d(18, 18, Math.toRadians(90)); + intakeStack = new Pose2d(52, 19,Math.toRadians(90)); + midwayPose2 = new Pose2d(52, -62, Math.toRadians(90)); + waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + parkPose = new Pose2d(50, -84, Math.toRadians(90)); + break; + } + + //Move robot to dropPurplePixel based on identified Spike Mark Location + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(moveBeyondTrussPose.position, moveBeyondTrussPose.heading) + .strafeToLinearHeading(dropPurplePixelPose.position, dropPurplePixelPose.heading) + .build()); + + //TODO : Code to drop Purple Pixel on Spike Mark + safeWaitSeconds(1); + + //Move robot to midwayPose1 + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(midwayPose1.position, midwayPose1.heading) + .build()); + + //For Blue Right and Red Left, intake pixel from stack + if (startPosition == START_POSITION.BLUE_RIGHT || + startPosition == START_POSITION.RED_LEFT) { + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(midwayPose1a.position, midwayPose1a.heading) + .strafeToLinearHeading(intakeStack.position, intakeStack.heading) + .build()); + + //TODO : Code to intake pixel from stack + safeWaitSeconds(1); + + //Move robot to midwayPose2 and to dropYellowPixelPose + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(midwayPose2.position, midwayPose2.heading) + .build()); + } + + safeWaitSeconds(waitSecondsBeforeDrop); + + //Move robot to midwayPose2 and to dropYellowPixelPose + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .setReversed(true) + .splineToLinearHeading(dropYellowPixelPose,0) + .build()); + + + //TODO : Code to drop Pixel on Backdrop + safeWaitSeconds(1); + + //Move robot to park in Backstage + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(parkPose.position, parkPose.heading) + //.splineToLinearHeading(parkPose,0) + .build()); + } + + + //Method to select starting position using X, Y, A, B buttons on gamepad + public void selectStartingPosition() { + telemetry.setAutoClear(true); + telemetry.clearAll(); + //******select start pose***** + while(!isStopRequested()){ + telemetry.addData("Initializing FTC Wires (ftcwires.org) Autonomous adopted for Team:", + TEAM_NAME, " ", TEAM_NUMBER); + telemetry.addData("---------------------------------------",""); + telemetry.addData("Select Starting Position using XYAB on Logitech (or ▢ΔOX on Playstayion) on gamepad 1:",""); + telemetry.addData(" Blue Left ", "(X / ▢)"); + telemetry.addData(" Blue Right ", "(Y / Δ)"); + telemetry.addData(" Red Left ", "(B / O)"); + telemetry.addData(" Red Right ", "(A / X)"); + if(gamepad1.x){ + startPosition = START_POSITION.BLUE_LEFT; + break; + } + if(gamepad1.y){ + startPosition = START_POSITION.BLUE_RIGHT; + break; + } + if(gamepad1.b){ + startPosition = START_POSITION.RED_LEFT; + break; + } + if(gamepad1.a){ + startPosition = START_POSITION.RED_RIGHT; + break; + } + telemetry.update(); + } + telemetry.clearAll(); + } + + //method to wait safely with stop button working if needed. Use this instead of sleep + public void safeWaitSeconds(double time) { + ElapsedTime timer = new ElapsedTime(SECONDS); + timer.reset(); + while (!isStopRequested() && timer.time() < time) { + } + } + + /** + * Initialize the TensorFlow Object Detection processor. + */ + private void initTfod() { + + // Create the TensorFlow processor the easy way. + tfod = TfodProcessor.easyCreateWithDefaults(); + + // Create the vision portal the easy way. + if (USE_WEBCAM) { + visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, "Webcam 1"), tfod); + } else { + visionPortal = VisionPortal.easyCreateWithDefaults( + BuiltinCameraDirection.BACK, tfod); + } + + // Set confidence threshold for TFOD recognitions, at any time. + tfod.setMinResultConfidence(0.095f); + + } // end method initTfod() + + /** + * Add telemetry about TensorFlow Object Detection (TFOD) recognitions. + */ + private void runTfodTensorFlow() { + + List currentRecognitions = tfod.getRecognitions(); + telemetry.addData("# Objects Detected", currentRecognitions.size()); + + //Camera placed between Left and Right Spike Mark on RED_LEFT and BLUE_LEFT If pixel not visible, assume Right spike Mark + if (startPosition == START_POSITION.RED_LEFT || startPosition == START_POSITION.BLUE_LEFT) { + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.RIGHT; + } else { //RED_RIGHT or BLUE_RIGHT + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; + } + + // Step through the list of recognitions and display info for each one. + for (Recognition recognition : currentRecognitions) { + double x = (recognition.getLeft() + recognition.getRight()) / 2 ; + double y = (recognition.getTop() + recognition.getBottom()) / 2 ; + + telemetry.addData(""," "); + telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); + telemetry.addData("- Position", "%.0f / %.0f", x, y); + telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); + + if (startPosition == START_POSITION.RED_LEFT || startPosition == START_POSITION.BLUE_LEFT) { + if (recognition.getLabel() == "Pixel") { + if (x < 200) { + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; + } else { + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.MIDDLE; + } + } + } else { //RED_RIGHT or BLUE_RIGHT + if (recognition.getLabel() == "Pixel") { + if (x < 200) { + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.MIDDLE; + } else { + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.RIGHT; + } + } + } + + } // end for() loop + + } // end method runTfodTensorFlow() + +} // end class From ee9924db7f8f424015d4e5b198cb2d98edf025ef Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 8 Nov 2023 15:42:28 -0700 Subject: [PATCH 063/154] Attepting to fix home button also dump prep work great --- .../org/firstinspires/ftc/teamcode/Beginnings.java | 3 +++ .../org/firstinspires/ftc/teamcode/MecanumDrive.java | 10 +++++----- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 9fc4852f09..8896587b61 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -193,7 +193,10 @@ private void driveAroundPos() { wrist.setPosition(0.59); } if (gamepad2.dpad_up) { + hopper.setPosition(0); wrist.setPosition(0.51); + shoulder.setPosition(0.46); + sleep(500); shoulder.setPosition(0.44); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 55110c6484..aa34d4ab89 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -51,7 +51,7 @@ public static class Params { // drive model parameters //TODO Step 5 Set value of inPerTick after running ForwardPushTest //TODO Step 14 Make value of inPerTick accurate after running LocalizationTest - public double inPerTick = 0.022622811513096; + public double inPerTick = 0.0058904378558806; //TODO Step 6 (Only for DriveEncoder Localizer) Set value of lateralInPerTick after running LateralPushTest //TODO Step 8 (Only for DeadWheel Localizer) Set value of lateralInPerTick after running LateralRampLogger @@ -65,8 +65,8 @@ public static class Params { // feedforward parameters (in tick units) //TODO Step 7 (Only for DeadWheel Localizer) Set value for kS and KV after running ForwardRampLogger //TODO Step 9 (Only for DriveEncoder Localizer) Set value for kS and kV after running AngularRampLogger - public double kS = 1.0182010371577; - public double kV = 0.004333430259; + public double kS = 3.3630176708546644; + public double kV = 0.00028294910916052253; //TODO Step 12 Set value of kA after running ManualFeedforwardTuner. In this emperical process update value in increments of 0.0001 public double kA = 0.00025; @@ -233,12 +233,12 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { //TODO Step 3: Specify how the robot should track its position //Comment this line if NOT using Drive Encoder localization - localizer = new DriveLocalizer(); + //localizer = new DriveLocalizer(); //Uncomment next line if using Two Dead Wheel Localizer and also check TwoDeadWheelLocalizer.java for Step 3.1 //localizer = new TwoDeadWheelLocalizer(hardwareMap, imu, PARAMS.inPerTick) //Uncomment next line if using Three Dead Wheel Localizer and also check ThreeDeadWheelLocalizer.java for Step 3.1 - //localizer = new ThreeDeadWheelLocalizer(hardwareMap, PARAMS.inPerTick) + localizer = new ThreeDeadWheelLocalizer(hardwareMap, PARAMS.inPerTick); //TODO End Step 3 FlightRecorder.write("MECANUM_PARAMS", PARAMS); From 1e8b6bb3d29c359db92de39316718c1efad14709 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 8 Nov 2023 17:07:33 -0700 Subject: [PATCH 064/154] added drive poses --- .../ftc/teamcode/Beginnings.java | 34 +++++++++++-------- 1 file changed, 19 insertions(+), 15 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 8896587b61..75b2b69e39 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -175,29 +175,17 @@ private void aroundthetop() { } private void dumpPrep() { if (gamepad2.back) { - wrist.setPosition(0.37); - shoulder.setPosition(0.91); - sleep(2345); hopper.setPosition(0.57); wrist.setPosition(0.93); + shoulder.setPosition(0.91); } } private void driveAroundPos() { if (gamepad1.a) { - shoulder.setPosition(0.455); - sleep(653); - shoulder.setPosition(0.47); - wrist.setPosition(0.55); - sleep(531); - shoulder.setPosition(0.49); - wrist.setPosition(0.59); + drivePos(); } if (gamepad2.dpad_up) { - hopper.setPosition(0); - wrist.setPosition(0.51); - shoulder.setPosition(0.46); - sleep(500); - shoulder.setPosition(0.44); + intakePos(); } } private void stopMotion() { @@ -221,6 +209,22 @@ private void stopMotion() { hopper.setPosition(0.16); } } + private void drivePos() { + shoulder.setPosition(0.49); + wrist.setPosition(0.59); + hopper.setPosition(0); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + } + private void intakePos() { + hopper.setPosition(0); + shoulder.setPosition(0.49); + wrist.setPosition(0.59); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + sleep(500); + shoulder.setPosition(0.44); + } /* private void driveCode() { From 6a6c46600f126faf3775170e31e5d42df94da8ad Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 8 Nov 2023 19:00:07 -0700 Subject: [PATCH 065/154] goodnight --- .../ftc/teamcode/Beginnings.java | 77 +++++++------------ 1 file changed, 28 insertions(+), 49 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 75b2b69e39..9b8765b85c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -98,38 +98,6 @@ private void airplane() { launcher.setPosition(0.8); } } - private void tuneshoulder() { - if (gamepad2.right_bumper) { - shoulder.setPosition(shoulder.getPosition() + 0.01); - } else if (gamepad2.right_trigger > 0.5){ - shoulder.setPosition(shoulder.getPosition() - 0.01); - } else { - //shoulder.setPosition(0.1); - } - telemetry.addData("shoulder", shoulder.getPosition()); - } - /* - private void tuneWrist() { - if (gamepad2.left_bumper) { - wrist.setPosition(wrist.getPosition() + 0.01); - } else if (gamepad2.left_trigger > 0.5){ - wrist.setPosition(wrist.getPosition() - 0.01); - } else { - //shoulder.setPosition(0.1); - } - telemetry.addData("wrist", wrist.getPosition()); - } - - private void jukeBeta() { - if (gamepad2.dpad_up) { - shoulder.setPosition(0.48); - wrist.setPosition(0.31); - } else if (gamepad2.dpad_down) { - shoulder.setPosition(0.445); - wrist.setPosition(0.26); - } - } - */ private void liftFunction() { if (gamepad2.y) { leftLift.setPosition(0.9 + LiftOffset); @@ -175,6 +143,8 @@ private void aroundthetop() { } private void dumpPrep() { if (gamepad2.back) { + shoulder.setPosition(0.75); + sleep(400); hopper.setPosition(0.57); wrist.setPosition(0.93); shoulder.setPosition(0.91); @@ -182,13 +152,13 @@ private void dumpPrep() { } private void driveAroundPos() { if (gamepad1.a) { - drivePos(); + incrementalIntake(); } if (gamepad2.dpad_up) { - intakePos(); + incrementalIntake(); } } - private void stopMotion() { + /* private void stopMotion() { if(gamepad1.dpad_left) { wrist.setPosition(0.51); shoulder.setPosition(0.44); @@ -209,23 +179,37 @@ private void stopMotion() { hopper.setPosition(0.16); } } + */ private void drivePos() { shoulder.setPosition(0.49); - wrist.setPosition(0.59); - hopper.setPosition(0); + wrist.setPosition(0.55); + hopper.setPosition(0.01); leftLift.setPosition(0.42); rightLift.setPosition(0.42); } private void intakePos() { - hopper.setPosition(0); - shoulder.setPosition(0.49); - wrist.setPosition(0.59); + hopper.setPosition(0.01); + shoulder.setPosition(0.445); + wrist.setPosition(0.5); leftLift.setPosition(0.42); rightLift.setPosition(0.42); - sleep(500); - shoulder.setPosition(0.44); + } + private void incrementalIntake() { + shoulder.setPosition(0.455); + sleep(653); + shoulder.setPosition(0.47); + wrist.setPosition(0.55); + sleep(531); + shoulder.setPosition(0.49); + wrist.setPosition(0.55); + } + private void startPos() { + launcher.setPosition(0.8); + intakePos(); } + + /* private void driveCode() { @@ -318,12 +302,7 @@ public void runOpMode() throws InterruptedException { frontIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); // servos - launcher.setPosition(0.8); - wrist.setPosition(0.51); - shoulder.setPosition(0.44); - hopper.setPosition(0); - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); + startPos(); /* sleep(500); leftLift.setPosition(0.8); @@ -384,7 +363,7 @@ public void runOpMode() throws InterruptedException { dumpPrep(); //liftFunction(); //worm(); - stopMotion(); + //stopMotion(); aroundthetop(); telemetry.update(); sleep(100); From 545ae06639376622377f9dc803fe9bf8b848b184 Mon Sep 17 00:00:00 2001 From: digitalbreadllc Date: Thu, 9 Nov 2023 17:45:04 -0700 Subject: [PATCH 066/154] add hang, dump prep, home --- .../ftc/teamcode/Beginnings.java | 101 +++++++++++++++++- 1 file changed, 96 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 9b8765b85c..c1f42537d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -4,9 +4,9 @@ import static android.os.SystemClock.sleep; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.PoseVelocity2d; -import com.acmerobotics.roadrunner.Vector2d; +//import com.acmerobotics.roadrunner.Pose2d; +//import com.acmerobotics.roadrunner.PoseVelocity2d; +//import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -43,6 +43,9 @@ public class Beginnings extends LinearOpMode { private DcMotor leftFront; //front left 2 private DcMotor rightBack; //rear right 1 private DcMotor leftBack; //rear left 3 + private DcMotor leftHang; + private DcMotor rightHang; + DcMotor frontIntake; DcMotor rearIntake; @@ -122,7 +125,7 @@ private void intakeFunction() { rearIntake.setPower(1); telemetry.addData("Intake", "in"); } - else if (gamepad1.right_bumper) { + else if (gamepad1.left_trigger > 0.5) { frontIntake.setPower(-1); rearIntake.setPower(-1); telemetry.addData("Intake", "out"); @@ -150,13 +153,52 @@ private void dumpPrep() { shoulder.setPosition(0.91); } } + private void dumpPrepTwo() { + if (gamepad2.back) { + shoulder.setPosition(0.60); + wrist.setPosition(0.61); + sleep(1100); + wrist.setPosition(0.35); + shoulder.setPosition(0.79); + sleep(1400); + hopper.setPosition(0.57); + wrist.setPosition(0.93); + //shoulder.setPosition(0.91); + sleep(1100); + shoulder.setPosition(0.91); + } + } + + private void dumpLeft() { + if (gamepad2.dpad_left) { + hopper.setPosition(0.23); + } + } + private void homePrep() { + if (gamepad1.back) { + shoulder.setPosition(0.79); + hopper.setPosition(0.01); + wrist.setPosition(0.521); + sleep(600); + shoulder.setPosition(0.60); + sleep(600); + shoulder.setPosition(0.50); + wrist.setPosition(0.521); + sleep(600); + intakePos(); + } + } private void driveAroundPos() { if (gamepad1.a) { incrementalIntake(); } + if (gamepad1.x) { + theJuke(); + } if (gamepad2.dpad_up) { incrementalIntake(); } + } /* private void stopMotion() { if(gamepad1.dpad_left) { @@ -194,6 +236,15 @@ private void intakePos() { leftLift.setPosition(0.42); rightLift.setPosition(0.42); } + private void theJuke() { + hopper.setPosition(0.01); + shoulder.setPosition(0.48); + wrist.setPosition(0.521); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + sleep(500); + intakePos(); + } private void incrementalIntake() { shoulder.setPosition(0.455); sleep(653); @@ -261,6 +312,9 @@ public void runOpMode() throws InterruptedException { rightBack = hardwareMap.get(DcMotor.class, "rightBack"); leftBack = hardwareMap.get(DcMotor.class, "leftBack"); + leftHang = hardwareMap.get(DcMotor.class, "leftHang"); + rightHang = hardwareMap.get(DcMotor.class, "rightHang"); + rightFront.setDirection(DcMotor.Direction.REVERSE); leftFront.setDirection(DcMotor.Direction.FORWARD); rightBack.setDirection(DcMotor.Direction.REVERSE); @@ -301,6 +355,12 @@ public void runOpMode() throws InterruptedException { rearIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); frontIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + // sensors + leftUpper = hardwareMap.get(RevTouchSensor.class, "leftUpper"); + rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); + leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); + rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); + // servos startPos(); /* @@ -351,6 +411,35 @@ public void runOpMode() throws InterruptedException { rightBack.setPower(rightBackPower); leftBack.setPower(leftBackPower); + if (gamepad1.left_trigger > 0.1){ + leftHang.setDirection(DcMotor.Direction.FORWARD); + rightHang.setDirection(DcMotor.Direction.FORWARD); + + leftHang.setPower(.9); + rightHang.setPower(.9); + } + if (gamepad1.left_bumper){ + leftHang.setDirection(DcMotor.Direction.REVERSE); + rightHang.setDirection(DcMotor.Direction.REVERSE); + + leftHang.setPower(.9); + rightHang.setPower(.9); + } + + if (leftUpper.isPressed() || rightUpper.isPressed() || gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); + } + + if (leftLower.isPressed() || rightLower.isPressed() || gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); + } + telemetry.addData("Status", "Run " + runtime.toString()); telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); @@ -360,7 +449,9 @@ public void runOpMode() throws InterruptedException { //IsDrive.is_drive_code(gamepad1, telemetry); intakeFunction(); driveAroundPos(); - dumpPrep(); + dumpPrepTwo(); + homePrep(); + dumpLeft(); //liftFunction(); //worm(); //stopMotion(); From 538afdf76886799ef6b94e79bb64083e960f71ad Mon Sep 17 00:00:00 2001 From: digitalbreadllc Date: Thu, 9 Nov 2023 17:58:01 -0700 Subject: [PATCH 067/154] update hang --- .../ftc/teamcode/Beginnings.java | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index c1f42537d4..318b56aa27 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -411,12 +411,17 @@ public void runOpMode() throws InterruptedException { rightBack.setPower(rightBackPower); leftBack.setPower(leftBackPower); - if (gamepad1.left_trigger > 0.1){ + if (gamepad1.right_bumper){ leftHang.setDirection(DcMotor.Direction.FORWARD); rightHang.setDirection(DcMotor.Direction.FORWARD); leftHang.setPower(.9); rightHang.setPower(.9); + } else if (leftUpper.isPressed() || rightUpper.isPressed() || gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); } if (gamepad1.left_bumper){ leftHang.setDirection(DcMotor.Direction.REVERSE); @@ -424,21 +429,15 @@ public void runOpMode() throws InterruptedException { leftHang.setPower(.9); rightHang.setPower(.9); - } - - if (leftUpper.isPressed() || rightUpper.isPressed() || gamepad1.y) { + } else if (leftLower.isPressed() || rightLower.isPressed() || gamepad1.y) { leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); leftHang.setPower(0); rightHang.setPower(0); } - if (leftLower.isPressed() || rightLower.isPressed() || gamepad1.y) { - leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - leftHang.setPower(0); - rightHang.setPower(0); - } + + telemetry.addData("Status", "Run " + runtime.toString()); telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); From e70b13dcc84b54f92970219492ff9cc1ead71301 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Thu, 9 Nov 2023 19:25:33 -0700 Subject: [PATCH 068/154] in progress of 2nd preset --- .../org/firstinspires/ftc/teamcode/Beginnings.java | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 318b56aa27..d94c642309 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -166,17 +166,28 @@ private void dumpPrepTwo() { //shoulder.setPosition(0.91); sleep(1100); shoulder.setPosition(0.91); + sleep(1150); + wrist.setPosition(0.85); + shoulder.setPosition(1); + } + } + private void slidez() { + if (gamepad2.a) { + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); } } private void dumpLeft() { if (gamepad2.dpad_left) { - hopper.setPosition(0.23); + hopper.setPosition(0.2); } } private void homePrep() { if (gamepad1.back) { shoulder.setPosition(0.79); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); hopper.setPosition(0.01); wrist.setPosition(0.521); sleep(600); @@ -455,6 +466,7 @@ public void runOpMode() throws InterruptedException { //worm(); //stopMotion(); aroundthetop(); + slidez(); telemetry.update(); sleep(100); } From 49d2dd8b93cdcc8c5b45e32e43ba874e6af05c37 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 10 Nov 2023 17:34:49 -0700 Subject: [PATCH 069/154] in progress of 2nd preset --- .../ftc/teamcode/Beginnings.java | 30 +++++++++++++------ 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index d94c642309..d9d0fb43c8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -149,12 +149,17 @@ private void dumpPrep() { shoulder.setPosition(0.75); sleep(400); hopper.setPosition(0.57); - wrist.setPosition(0.93); + wrist.setPosition(0.93); shoulder.setPosition(0.91); } } private void dumpPrepTwo() { if (gamepad2.back) { + + } + } + private void SlideFunction() { + if (gamepad2.a) { shoulder.setPosition(0.60); wrist.setPosition(0.61); sleep(1100); @@ -171,12 +176,6 @@ private void dumpPrepTwo() { shoulder.setPosition(1); } } - private void slidez() { - if (gamepad2.a) { - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); - } - } private void dumpLeft() { if (gamepad2.dpad_left) { @@ -207,7 +206,20 @@ private void driveAroundPos() { theJuke(); } if (gamepad2.dpad_up) { - incrementalIntake(); + if (gamepad1.back) { + shoulder.setPosition(0.79); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + hopper.setPosition(0.01); + wrist.setPosition(0.521); + sleep(600); + shoulder.setPosition(0.60); + sleep(600); + shoulder.setPosition(0.50); + wrist.setPosition(0.521); + sleep(600); + intakePos(); + } } } @@ -466,7 +478,7 @@ public void runOpMode() throws InterruptedException { //worm(); //stopMotion(); aroundthetop(); - slidez(); + SlideFunction(); telemetry.update(); sleep(100); } From d18d6e9c13d06240839dc311eeb42ce158dbd571 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Sat, 11 Nov 2023 16:41:08 -0700 Subject: [PATCH 070/154] update V6 needed for new IntakePos and servo --- .../main/java/org/firstinspires/ftc/teamcode/Beginnings.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 827e9e9882..0019136dda 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -258,9 +258,9 @@ private void drivePos() { rightLift.setPosition(0.42); } private void intakePos() { - hopper.setPosition(0.01); + hopper.setPosition(0.02); shoulder.setPosition(0.445); - wrist.setPosition(0.5); + wrist.setPosition(0.2); leftLift.setPosition(0.42); rightLift.setPosition(0.42); } From 337ccf20ac4b98510acedf065351549fff09e8d5 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 11 Nov 2023 18:55:37 -0700 Subject: [PATCH 071/154] good night, sleep well 11/11 --- .../ftc/teamcode/Beginnings.java | 252 +++++++++++------- 1 file changed, 162 insertions(+), 90 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 0019136dda..4ccd659b79 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -18,6 +18,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; import org.firstinspires.ftc.teamcode.tuning.TuningOpModes; @@ -51,7 +52,17 @@ public class Beginnings extends LinearOpMode { // Servo prep + private static enum ArmPosition { + INTAKE, + DRIVE, + UPGO1, + UPGO2, + UPGO3, + UPGO4, + UPGO5; + } + private ArmPosition currentArmPos; double MinLiftHeight = 0.05; double MaxLiftHeight = 0.65; double LiftLeftOffset = -0.08; @@ -60,21 +71,20 @@ public class Beginnings extends LinearOpMode { private AndroidTextToSpeech androidTextToSpeech; - // Functions \/ private void servo_shenanigans() { - setLiftHeight(0.4); - sleep(10000); - setLiftHeight(0.05); + setLiftHeight(0.4); + sleep(10000); + setLiftHeight(0.05); } private void setLiftHeight(double inputLiftHeight) { - if (inputLiftHeight < 0.05){ + if (inputLiftHeight < 0.05) { inputLiftHeight = 0.05; } - if (inputLiftHeight > 0.65){ + if (inputLiftHeight > 0.65) { inputLiftHeight = 0.65; } LiftHeight = inputLiftHeight; @@ -84,39 +94,140 @@ private void setLiftHeight(double inputLiftHeight) { } private void worm() { - if(gamepad2.left_bumper) { + if (gamepad2.left_bumper) { shoulder.setPosition(0.445); wrist.setPosition(0.26); } - if(gamepad2.right_bumper) { + if (gamepad2.right_bumper) { shoulder.setPosition(0.92); } } private void airplane() { - if (gamepad2.dpad_down) { + if (gamepad2.back) { launcher.setPosition(0.1); - } - else { + } else { launcher.setPosition(0.8); } } + private void liftFunction() { if (gamepad2.y) { - leftLift.setPosition(0.9 + LiftOffset); - rightLift.setPosition(0.9); + shoulder.setPosition(0.79); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + hopper.setPosition(0.01); + wrist.setPosition(0.2); + sleep(600); + shoulder.setPosition(0.60); + sleep(600); + shoulder.setPosition(0.50); + sleep(600); + intakePos(); + } else if (gamepad2.x) { + leftLift.setPosition(0.75 + LiftOffset); + rightLift.setPosition(0.75); + } else if (gamepad2.b) { + leftLift.setPosition(0.6 + LiftOffset); + rightLift.setPosition(0.6); } - else if (gamepad2.x) { - leftLift.setPosition(0.8 + LiftOffset); - rightLift.setPosition(0.8); + } + + + private void armmDown() { + if (gamepad2.y) { + switch (currentArmPos) { + case UPGO1: + currentArmPos = ArmPosition.INTAKE; + break; + case UPGO2: + currentArmPos = ArmPosition.UPGO1; + break; + case UPGO3: + currentArmPos = ArmPosition.UPGO2; + break; + case UPGO4: + currentArmPos = ArmPosition.UPGO3; + break; + case UPGO5: + currentArmPos = ArmPosition.UPGO4; + break; + } + doArmm(); + telemetry.addLine(); + telemetry.addData("armpostion", currentArmPos.toString()); + sleep(100); } - else if (gamepad2.b) { - leftLift.setPosition(0.7 + LiftOffset); - rightLift.setPosition(0.7); + } + + + private void doArmm() { + switch (currentArmPos) { + case INTAKE: + intakePos(); + break; + case UPGO1: + shoulder.setPosition(0.60); + hopper.setPosition(0.02); + wrist.setPosition(0.22); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + break; + case UPGO2: + wrist.setPosition(0.22); + shoulder.setPosition(0.79); + hopper.setPosition(0.02); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + break; + case UPGO3: + hopper.setPosition(0.58); + wrist.setPosition(0.6); + shoulder.setPosition(0.79); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + break; + case UPGO4: + shoulder.setPosition(0.91); + hopper.setPosition(0.58); + wrist.setPosition(0.6); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + break; + case UPGO5: + wrist.setPosition(0.6); + shoulder.setPosition(1); + hopper.setPosition(0.58); + leftLift.setPosition(0.42); + rightLift.setPosition(0.42); + break; } - else if (gamepad2.a) { - leftLift.setPosition(0.5 + LiftOffset); - rightLift.setPosition(0.5); + } + + + private void armmUp(){ + if (gamepad2.a) { + switch (currentArmPos) { + case INTAKE: + currentArmPos = ArmPosition.UPGO1; + break; + case UPGO1: + currentArmPos = ArmPosition.UPGO2; + break; + case UPGO2: + currentArmPos = ArmPosition.UPGO3; + break; + case UPGO3: + currentArmPos = ArmPosition.UPGO4; + break; + case UPGO4: + currentArmPos = ArmPosition.UPGO5; + break; + } + doArmm(); + telemetry.addLine(); + telemetry.addData("armpostion", currentArmPos.toString()); + sleep(100); } } private void intakeFunction() { @@ -139,14 +250,9 @@ else if (gamepad1.left_trigger > 0.5) { } private void aroundthetop() { if (gamepad2.start) { - shoulder.setPosition(0.44); - sleep(400); - wrist.setPosition(0.53); - hopper.setPosition(0.33); - sleep(500); - wrist.setPosition(0.93); - hopper.setPosition(0.57); - + shoulder.setPosition(0.56); + wrist.setPosition(0.5); + hopper.setPosition(0.4); } } private void dumpPrep() { @@ -154,7 +260,7 @@ private void dumpPrep() { shoulder.setPosition(0.75); sleep(400); hopper.setPosition(0.57); - wrist.setPosition(0.93); + wrist.setPosition(0.93); shoulder.setPosition(0.91); } } @@ -164,26 +270,11 @@ private void dumpPrepTwo() { } } private void SlideFunction() { - if (gamepad2.a) { - shoulder.setPosition(0.60); - wrist.setPosition(0.61); - sleep(1100); - wrist.setPosition(0.35); - shoulder.setPosition(0.79); - sleep(1400); - hopper.setPosition(0.57); - wrist.setPosition(0.93); - //shoulder.setPosition(0.91); - sleep(1100); - shoulder.setPosition(0.91); - sleep(1150); - wrist.setPosition(0.85); - shoulder.setPosition(1); - } + } private void dumpLeft() { - if (gamepad2.dpad_left) { + if (gamepad2.right_bumper) { hopper.setPosition(0.2); } } @@ -204,27 +295,10 @@ private void homePrep() { } } private void driveAroundPos() { - if (gamepad1.a) { - incrementalIntake(); - } + + if (gamepad1.x) { - theJuke(); - } - if (gamepad2.dpad_up) { - if (gamepad1.back) { - shoulder.setPosition(0.79); - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); - hopper.setPosition(0.01); - wrist.setPosition(0.521); - sleep(600); - shoulder.setPosition(0.60); - sleep(600); - shoulder.setPosition(0.50); - wrist.setPosition(0.521); - sleep(600); - intakePos(); - } + intakePos(); } } @@ -256,22 +330,23 @@ private void drivePos() { hopper.setPosition(0.01); leftLift.setPosition(0.42); rightLift.setPosition(0.42); + currentArmPos = ArmPosition.DRIVE; } private void intakePos() { hopper.setPosition(0.02); - shoulder.setPosition(0.445); - wrist.setPosition(0.2); + shoulder.setPosition(0.455); + wrist.setPosition(0.22); leftLift.setPosition(0.42); rightLift.setPosition(0.42); + currentArmPos = ArmPosition.INTAKE; } private void theJuke() { - hopper.setPosition(0.01); - shoulder.setPosition(0.48); - wrist.setPosition(0.521); - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); - sleep(500); - intakePos(); + if (gamepad1.a && (currentArmPos == ArmPosition.INTAKE)) { + shoulder.setPosition(0.46); + sleep(100); + shoulder.setPosition(0.5); + wrist.setPosition(0.3); + } } private void incrementalIntake() { shoulder.setPosition(0.455); @@ -282,9 +357,8 @@ private void incrementalIntake() { shoulder.setPosition(0.49); wrist.setPosition(0.55); } - private void startPos() { + private void launcherstartPos() { launcher.setPosition(0.8); - intakePos(); } @@ -390,13 +464,8 @@ public void runOpMode() throws InterruptedException { rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); // servos - startPos(); - /* - sleep(500); - leftLift.setPosition(0.8); - rightLift.setPosition(0.8); - wrist.setPosition(0.7); - */ + launcherstartPos(); + intakePos(); //this is a coment to mAKE git update double SLOW_DOWN_FACTOR = 0.5; @@ -470,19 +539,22 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Status", "Run " + runtime.toString()); telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); - //driveCode(); - //airplane(); + //Code(); + airplane(); //ogDrive.og_drive_code(gamepad1, telemetry); //IsDrive.is_drive_code(gamepad1, telemetry); intakeFunction(); driveAroundPos(); - dumpPrepTwo(); - homePrep(); + //dumpPrepTwo(); + //homePrep(); dumpLeft(); + theJuke(); //liftFunction(); //worm(); //stopMotion(); aroundthetop(); + armmUp(); + armmDown(); SlideFunction(); telemetry.update(); sleep(100); From 7bee912fa64f30c4eca2c45588e30b2b9dae209a Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Mon, 13 Nov 2023 16:44:22 -0700 Subject: [PATCH 072/154] FnaF movie har har har har har also frontIntake speed lowered --- .../main/java/org/firstinspires/ftc/teamcode/Beginnings.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 4ccd659b79..4df3a06f7c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -232,12 +232,12 @@ private void armmUp(){ } private void intakeFunction() { if (gamepad1.right_trigger > 0.5) { - frontIntake.setPower(1); + frontIntake.setPower(0.8); rearIntake.setPower(1); telemetry.addData("Intake", "in"); } else if (gamepad1.left_trigger > 0.5) { - frontIntake.setPower(-1); + frontIntake.setPower(-0.8); rearIntake.setPower(-1); telemetry.addData("Intake", "out"); } From 2893b03ed8fb32d396138cb9007e527720f6c3c6 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 14 Nov 2023 17:48:20 -0700 Subject: [PATCH 073/154] Clean code means better soul made lift good - goob --- .../ftc/teamcode/Beginnings.java | 105 ++++-------------- .../ftc/teamcode/McTuner3000.java | 35 ++++++ 2 files changed, 57 insertions(+), 83 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 4df3a06f7c..d24e89a245 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -49,8 +49,6 @@ public class Beginnings extends LinearOpMode { DcMotor frontIntake; DcMotor rearIntake; - - // Servo prep private static enum ArmPosition { INTAKE, @@ -63,11 +61,8 @@ private static enum ArmPosition { } private ArmPosition currentArmPos; - double MinLiftHeight = 0.05; - double MaxLiftHeight = 0.65; - double LiftLeftOffset = -0.08; + double LiftLeftOffset = .04; double LiftHeight; - double LiftOffset = 0; private AndroidTextToSpeech androidTextToSpeech; @@ -81,11 +76,11 @@ private void servo_shenanigans() { } private void setLiftHeight(double inputLiftHeight) { - if (inputLiftHeight < 0.05) { - inputLiftHeight = 0.05; + if (inputLiftHeight < 0.42) { + inputLiftHeight = 0.42; } - if (inputLiftHeight > 0.65) { - inputLiftHeight = 0.65; + if (inputLiftHeight > 1) { + inputLiftHeight = 1; } LiftHeight = inputLiftHeight; leftLift.setPosition(LiftLeftOffset + LiftHeight); @@ -114,8 +109,7 @@ private void airplane() { private void liftFunction() { if (gamepad2.y) { shoulder.setPosition(0.79); - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); + setLiftHeight(0.42); hopper.setPosition(0.01); wrist.setPosition(0.2); sleep(600); @@ -125,15 +119,11 @@ private void liftFunction() { sleep(600); intakePos(); } else if (gamepad2.x) { - leftLift.setPosition(0.75 + LiftOffset); - rightLift.setPosition(0.75); + setLiftHeight(0.71); } else if (gamepad2.b) { - leftLift.setPosition(0.6 + LiftOffset); - rightLift.setPosition(0.6); + setLiftHeight(0.56); } } - - private void armmDown() { if (gamepad2.y) { switch (currentArmPos) { @@ -159,8 +149,6 @@ private void armmDown() { sleep(100); } } - - private void doArmm() { switch (currentArmPos) { case INTAKE: @@ -170,41 +158,34 @@ private void doArmm() { shoulder.setPosition(0.60); hopper.setPosition(0.02); wrist.setPosition(0.22); - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); + setLiftHeight(0.42); break; case UPGO2: wrist.setPosition(0.22); shoulder.setPosition(0.79); hopper.setPosition(0.02); - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); + setLiftHeight(0.42); break; case UPGO3: hopper.setPosition(0.58); wrist.setPosition(0.6); shoulder.setPosition(0.79); - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); + setLiftHeight(0.42); break; case UPGO4: shoulder.setPosition(0.91); hopper.setPosition(0.58); wrist.setPosition(0.6); - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); + setLiftHeight(0.42); break; case UPGO5: wrist.setPosition(0.6); shoulder.setPosition(1); hopper.setPosition(0.58); - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); + setLiftHeight(0.42); break; } } - - private void armmUp(){ if (gamepad2.a) { switch (currentArmPos) { @@ -234,17 +215,20 @@ private void intakeFunction() { if (gamepad1.right_trigger > 0.5) { frontIntake.setPower(0.8); rearIntake.setPower(1); + setLiftHeight(0.45); telemetry.addData("Intake", "in"); } else if (gamepad1.left_trigger > 0.5) { - frontIntake.setPower(-0.8); + frontIntake.setPower(-0.8); rearIntake.setPower(-1); + telemetry.addData("Intake", "out"); } else { telemetry.addData("Intake", "stopped"); frontIntake.setPower(0); rearIntake.setPower(0); + setLiftHeight(0.42); } } @@ -278,66 +262,23 @@ private void dumpLeft() { hopper.setPosition(0.2); } } - private void homePrep() { - if (gamepad1.back) { - shoulder.setPosition(0.79); - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); - hopper.setPosition(0.01); - wrist.setPosition(0.521); - sleep(600); - shoulder.setPosition(0.60); - sleep(600); - shoulder.setPosition(0.50); - wrist.setPosition(0.521); - sleep(600); - intakePos(); - } - } private void driveAroundPos() { - - if (gamepad1.x) { intakePos(); } - } - /* private void stopMotion() { - if(gamepad1.dpad_left) { - wrist.setPosition(0.51); - shoulder.setPosition(0.44); - hopper.setPosition(0); - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); - - sleep(2000); - - wrist.setPosition(0.74); - shoulder.setPosition(0.7); - hopper.setPosition(0); - - sleep(2000); - - wrist.setPosition(0.73); - shoulder.setPosition(0.75); - hopper.setPosition(0.16); - } - } - */ private void drivePos() { shoulder.setPosition(0.49); wrist.setPosition(0.55); hopper.setPosition(0.01); - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); + setLiftHeight(0.42); currentArmPos = ArmPosition.DRIVE; } private void intakePos() { hopper.setPosition(0.02); - shoulder.setPosition(0.455); - wrist.setPosition(0.22); - leftLift.setPosition(0.42); - rightLift.setPosition(0.42); + shoulder.setPosition(0.44); + wrist.setPosition(0.26); + setLiftHeight(0.42); currentArmPos = ArmPosition.INTAKE; } private void theJuke() { @@ -384,8 +325,8 @@ private void driveCode() { //telemetry.addData("RF Encoder", drive.rightFront.getCurrentPosition()); //telemetry.addData("RB Encoder", drive.rightBack.getCurrentPosition()); - telemetry.addLine("Current Pose"); telemetry.addData("x", drive.pose.position.x); + telemetry.addLine("Current Pose"); telemetry.addData("y", drive.pose.position.y); telemetry.addData("heading", Math.toDegrees(drive.pose.heading.log())); // telemetry.update(); @@ -443,8 +384,6 @@ public void runOpMode() throws InterruptedException { shoulder.setDirection(Servo.Direction.REVERSE); wrist.setDirection(Servo.Direction.REVERSE); - - launcher.setDirection(Servo.Direction.REVERSE); frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java index 8667f76b98..e7c4d0bfc2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -2,6 +2,8 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.vision.apriltag.AprilTagCanvasAnnotator; @@ -15,6 +17,9 @@ public class McTuner3000 extends LinearOpMode { Servo leftLift; Servo rightLift; + DcMotor frontIntake; + DcMotor rearIntake; + double speedAmount; //TODO: Step 3, set all of your servo names below @@ -104,6 +109,24 @@ private void whatServoAt() { telemetry.addData("Lift Left = ",leftLift.getPosition()); telemetry.addData("Lift Right = ",rightLift.getPosition()); } + private void slurp() { + if (gamepad1.right_trigger > 0.5) { + frontIntake.setPower(0.8); + rearIntake.setPower(1); + //leftLift.setPosition(.49); + //rightLift.setPosition(.45); + telemetry.addData("Intake", "in"); + } else if (gamepad1.left_trigger > 0.5) { + frontIntake.setPower(-0.8); + rearIntake.setPower(-1); + + telemetry.addData("Intake", "out"); + } else { + telemetry.addData("Intake", "stopped"); + frontIntake.setPower(0); + rearIntake.setPower(0); + } + } @Override public void runOpMode() throws InterruptedException { which = ServoTypes.SHOULDER; @@ -130,12 +153,24 @@ public void runOpMode() throws InterruptedException { rightLift.setPosition(0.42); + // this is a comment + frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); + rearIntake = hardwareMap.get(DcMotor.class, "rearIntake"); + + frontIntake.setDirection(DcMotorSimple.Direction.FORWARD); + rearIntake.setDirection(DcMotorSimple.Direction.REVERSE); + frontIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + telemetry.update(); waitForStart(); while(opModeIsActive()) { setServo(); + slurp(); masterTuner(); setSpeed(); whatServoAt(); From 34a7ca0f7af80a7d1dc66623a06a6404c68f651b Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 14 Nov 2023 18:05:23 -0700 Subject: [PATCH 074/154] just uh kynow juke for mcmankj - goob --- .../main/java/org/firstinspires/ftc/teamcode/Beginnings.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index d24e89a245..df9f2e2431 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -263,7 +263,7 @@ private void dumpLeft() { } } private void driveAroundPos() { - if (gamepad1.x) { + if (gamepad2.dpad_down || gamepad1.x) { intakePos(); } } @@ -282,7 +282,7 @@ private void intakePos() { currentArmPos = ArmPosition.INTAKE; } private void theJuke() { - if (gamepad1.a && (currentArmPos == ArmPosition.INTAKE)) { + if ((gamepad2.dpad_up || gamepad1.a) && (currentArmPos == ArmPosition.INTAKE)) { shoulder.setPosition(0.46); sleep(100); shoulder.setPosition(0.5); From c8cf20db368aeeb226b1f8ff6730b88970666279 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 14 Nov 2023 18:11:25 -0700 Subject: [PATCH 075/154] meerge new wires - goob --- .../ftc/teamcode/MecanumDrive.java | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index aa34d4ab89..046595f3ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -51,25 +51,25 @@ public static class Params { // drive model parameters //TODO Step 5 Set value of inPerTick after running ForwardPushTest //TODO Step 14 Make value of inPerTick accurate after running LocalizationTest - public double inPerTick = 0.0058904378558806; + public double inPerTick = 0; //TODO Step 6 (Only for DriveEncoder Localizer) Set value of lateralInPerTick after running LateralPushTest //TODO Step 8 (Only for DeadWheel Localizer) Set value of lateralInPerTick after running LateralRampLogger //TODO Step 14 Make value of lateralInPerTick accurate after running LocalizationTest - public double lateralInPerTick = 0.022778; + public double lateralInPerTick = 1; //TODO Step 10 (Only for DriveEncoder Localizer) Set value of trackWidthTicks after running AngularRampLogger //TODO Step 11 (Only for DeadWheel Localizer) Set value of trackWidthTicks after running AngularRampLogger - public double trackWidthTicks = 1000.143754986; + public double trackWidthTicks = 0; // feedforward parameters (in tick units) //TODO Step 7 (Only for DeadWheel Localizer) Set value for kS and KV after running ForwardRampLogger //TODO Step 9 (Only for DriveEncoder Localizer) Set value for kS and kV after running AngularRampLogger - public double kS = 3.3630176708546644; - public double kV = 0.00028294910916052253; + public double kS = 0; + public double kV = 0; //TODO Step 12 Set value of kA after running ManualFeedforwardTuner. In this emperical process update value in increments of 0.0001 - public double kA = 0.00025; + public double kA = 0; // path profile parameters (in inches) public double maxWheelVel = 50; @@ -221,7 +221,6 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); imu = hardwareMap.get(IMU.class, "imu"); - // //TODO Step 2 : Update direction of IMU by updating orientation of Driver Hub below IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, // Change to UP / DOWN / LEFT / RIGHT / FORWARD / BACKWARD as in robot @@ -233,12 +232,12 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { //TODO Step 3: Specify how the robot should track its position //Comment this line if NOT using Drive Encoder localization - //localizer = new DriveLocalizer(); + localizer = new DriveLocalizer(); //Uncomment next line if using Two Dead Wheel Localizer and also check TwoDeadWheelLocalizer.java for Step 3.1 //localizer = new TwoDeadWheelLocalizer(hardwareMap, imu, PARAMS.inPerTick) //Uncomment next line if using Three Dead Wheel Localizer and also check ThreeDeadWheelLocalizer.java for Step 3.1 - localizer = new ThreeDeadWheelLocalizer(hardwareMap, PARAMS.inPerTick); + //localizer = new ThreeDeadWheelLocalizer(hardwareMap, PARAMS.inPerTick) //TODO End Step 3 FlightRecorder.write("MECANUM_PARAMS", PARAMS); From 45d1a97109a3cb5c0d4e7230d1643319c86e5500 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 14 Nov 2023 20:34:14 -0700 Subject: [PATCH 076/154] auto that may or may not crash, and roadrunner tuning partway - goob --- .../firstinspires/ftc/teamcode/BlueLeftE.java | 82 +++++++++++++++++++ .../ftc/teamcode/McTuner3000.java | 14 ++-- .../ftc/teamcode/MecanumDrive.java | 10 +-- 3 files changed, 95 insertions(+), 11 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeftE.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeftE.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeftE.java new file mode 100644 index 0000000000..668e900d79 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeftE.java @@ -0,0 +1,82 @@ +/* + * Copyright (c) 2021 OpenFTC Team + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.rev.RevTouchSensor; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; + +import org.openftc.apriltag.AprilTagDetection; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvInternalCamera; + +import java.util.ArrayList; + +@Autonomous(name= "Left Auto") +public class BlueLeftE extends LinearOpMode +{ + // Declare vars + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; + private Servo launcher; + private Servo rightLift; + private Servo leftLift; + private Servo shoulder; + private Servo wrist; + private Servo hopper; + //MecanumDrive drive; + //OgDrive ogDrive; + private ElapsedTime runtime = new ElapsedTime(); + //Motors + //Drivetrain + private DcMotor rightFront; //front right 0 + private DcMotor leftFront; //front left 2 + private DcMotor rightBack; //rear right 1 + private DcMotor leftBack; //rear left 3 + private DcMotor leftHang; + private DcMotor rightHang; + + DcMotor frontIntake; + DcMotor rearIntake; + @Override + public void runOpMode() { + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + wrist = hardwareMap.get(Servo.class, "wrist"); + hopper = hardwareMap.get(Servo.class, "hopper"); + shoulder = hardwareMap.get(Servo.class, "shoulder"); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java index e7c4d0bfc2..f65169f1ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -21,6 +21,8 @@ public class McTuner3000 extends LinearOpMode { DcMotor rearIntake; double speedAmount; + //TODO: Add offset if needed w/ eg double lift + double LiftLeftOffset = -.05; //TODO: Step 3, set all of your servo names below enum ServoTypes{ @@ -44,7 +46,7 @@ else if (which == ServoTypes.WRIST) { wrist.setPosition(wrist.getPosition() - speedAmount); } else if (which == ServoTypes.LIFT) { - leftLift.setPosition(leftLift.getPosition() - speedAmount); + leftLift.setPosition((rightLift.getPosition() + LiftLeftOffset) - speedAmount); rightLift.setPosition(rightLift.getPosition() - speedAmount); } } @@ -59,7 +61,7 @@ else if (which == ServoTypes.WRIST) { wrist.setPosition(wrist.getPosition() + speedAmount); } else if (which == ServoTypes.LIFT) { - leftLift.setPosition(leftLift.getPosition() + speedAmount); + leftLift.setPosition((rightLift.getPosition() + LiftLeftOffset) + speedAmount); rightLift.setPosition(rightLift.getPosition() + speedAmount); } } @@ -85,13 +87,13 @@ private void setSpeed() { speedAmount = 0; } else if (gamepad1.dpad_left) { - speedAmount = 0.01; + speedAmount = 0.005; } else if (gamepad1.dpad_right) { - speedAmount = 0.02; + speedAmount = 0.01; } else if (gamepad1.dpad_up) { - speedAmount = 0.05; + speedAmount = 0.03; } telemetry.addData("Speed = ", speedAmount); } @@ -149,7 +151,7 @@ public void runOpMode() throws InterruptedException { wrist.setPosition(0.51); shoulder.setPosition(0.44); hopper.setPosition(0); - leftLift.setPosition(0.42); + leftLift.setPosition(0.42 + LiftLeftOffset); rightLift.setPosition(0.42); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index eac7a482e3..5dea3b41a6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -51,23 +51,23 @@ public static class Params { // drive model parameters //TODO Step 5 Set value of inPerTick after running ForwardPushTest //TODO Step 14 Make value of inPerTick accurate after running LocalizationTest - public double inPerTick = 0; + public double inPerTick = (123.5 / 20870); // 123.5 in 20870 ticks //TODO Step 6 (Only for DriveEncoder Localizer) Set value of lateralInPerTick after running LateralPushTest //TODO Step 8 (Only for DeadWheel Localizer) Set value of lateralInPerTick after running LateralRampLogger //TODO Step 14 Make value of lateralInPerTick accurate after running LocalizationTest - public double lateralInPerTick = 1; + public double lateralInPerTick = (124 / 10473); //TODO Step 10 (Only for DriveEncoder Localizer) Set value of trackWidthTicks after running AngularRampLogger //TODO Step 11 (Only for DeadWheel Localizer) Set value of trackWidthTicks after running AngularRampLogger // Go to Step 11.1 in Three or Two DeadWheelLocalizer and updated values of par0YTicks, part1YTicks, perpXTicks - public double trackWidthTicks = 0; + public double trackWidthTicks = -3712.5656183161345; // feedforward parameters (in tick units) //TODO Step 7 (Only for DeadWheel Localizer) Set value for kS and KV after running ForwardRampLogger //TODO Step 9 (Only for DriveEncoder Localizer) Set value for kS and kV after running AngularRampLogger - public double kS = 0; - public double kV = 0; + public double kS = 1.042042743032133; + public double kV = -0.0011165000242287573; //TODO Step 12 Set value of kA after running ManualFeedforwardTuner. In this emperical process update value in increments of 0.0001 public double kA = 0; From fcf776da99239210b1a3c9402911e6295f9c75e2 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Tue, 14 Nov 2023 20:35:21 -0700 Subject: [PATCH 077/154] Auto Test --- .../ftc/teamcode/AutoFunctionJetts.java | 83 +++++++++++++++++++ 1 file changed, 83 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoFunctionJetts.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoFunctionJetts.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoFunctionJetts.java new file mode 100644 index 0000000000..f4d3cd7428 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoFunctionJetts.java @@ -0,0 +1,83 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; + +@Autonomous +public class AutoFunctionJetts extends LinearOpMode { + private DcMotor rightFront; //front right 0 + private DcMotor leftFront; //front left 2 + private DcMotor rightBack; //rear right 1 + private DcMotor leftBack; + DcMotor frontIntake; + DcMotor rearIntake; + private Servo shoulder; + private Servo wrist; + private Servo hopper; + +public void FirstTest() { + rightFront.setPower(0.2); + rightBack.setPower(0.2); + leftBack.setPower(0.2); + leftFront.setPower(0.2); + sleep(300); + rightFront.setPower(0); + rightBack.setPower(0); + leftBack.setPower(0); + leftFront.setPower(0); + sleep(300); + rightFront.setPower(0.2); + rightBack.setPower(0.2); + leftBack.setPower(0.2); + leftFront.setPower(0.2); + sleep(100); + rightFront.setPower(0); + rightBack.setPower(0); + leftBack.setPower(0); + sleep(500); + frontIntake.setPower(-0.3); + rearIntake.setPower(-0.3); + + +} + + @Override + public void runOpMode() throws InterruptedException { + //Wheel init + rightFront = hardwareMap.get(DcMotor.class, "rightFront"); + leftFront = hardwareMap.get(DcMotor.class, "leftFront"); + rightBack = hardwareMap.get(DcMotor.class, "rightBack"); + leftBack = hardwareMap.get(DcMotor.class, "leftBack"); + //Intake init + frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); + rearIntake = hardwareMap.get(DcMotor.class, "rearIntake"); + //Holder init + wrist = hardwareMap.get(Servo.class, "wrist"); + hopper = hardwareMap.get(Servo.class, "hopper"); + //Arm init + shoulder = hardwareMap.get(Servo.class, "shoulder"); + //Wheel Set + rightFront.setDirection(DcMotor.Direction.REVERSE); + leftFront.setDirection(DcMotor.Direction.FORWARD); + rightBack.setDirection(DcMotor.Direction.REVERSE); + leftBack.setDirection(DcMotor.Direction.FORWARD); + //Intake Set + frontIntake.setDirection(DcMotorSimple.Direction.FORWARD); + rearIntake.setDirection(DcMotorSimple.Direction.REVERSE); + frontIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + //Holder Set + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + + + + FirstTest(); + + } +} From bfbdbb33e20bf39bedaf5a799298721236af419c Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 15 Nov 2023 16:06:49 -0700 Subject: [PATCH 078/154] auto encoders for pars - goob --- .../ftc/teamcode/MecanumDrive.java | 25 +++++++++++++------ .../ftc/teamcode/ThreeDeadWheelLocalizer.java | 6 ++--- build.gradle | 2 +- gradle/wrapper/gradle-wrapper.properties | 2 +- 4 files changed, 23 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 5dea3b41a6..0c84c696de 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -51,12 +51,12 @@ public static class Params { // drive model parameters //TODO Step 5 Set value of inPerTick after running ForwardPushTest //TODO Step 14 Make value of inPerTick accurate after running LocalizationTest - public double inPerTick = (123.5 / 20870); // 123.5 in 20870 ticks + public double inPerTick = (112 / 18952.5); //TODO Step 6 (Only for DriveEncoder Localizer) Set value of lateralInPerTick after running LateralPushTest //TODO Step 8 (Only for DeadWheel Localizer) Set value of lateralInPerTick after running LateralRampLogger //TODO Step 14 Make value of lateralInPerTick accurate after running LocalizationTest - public double lateralInPerTick = (124 / 10473); + public double lateralInPerTick = (112 / 9577); // last run: 0.0118399694452401 //TODO Step 10 (Only for DriveEncoder Localizer) Set value of trackWidthTicks after running AngularRampLogger //TODO Step 11 (Only for DeadWheel Localizer) Set value of trackWidthTicks after running AngularRampLogger @@ -64,10 +64,10 @@ public static class Params { public double trackWidthTicks = -3712.5656183161345; // feedforward parameters (in tick units) - //TODO Step 7 (Only for DeadWheel Localizer) Set value for kS and KV after running ForwardRampLogger + //TODO Step 7 (Only for DeadWheel Localizer) Set value for kS eeand KV after running ForwardRampLogger //TODO Step 9 (Only for DriveEncoder Localizer) Set value for kS and kV after running AngularRampLogger - public double kS = 1.042042743032133; - public double kV = -0.0011165000242287573; + public double kS = 0.9677232995660399; + public double kV = 0.0005787028260206902; //TODO Step 12 Set value of kA after running ManualFeedforwardTuner. In this emperical process update value in increments of 0.0001 public double kA = 0; @@ -137,6 +137,12 @@ public DriveLocalizer() { leftBack.setDirection(DcMotorEx.Direction.REVERSE); //rightBack.setDirection(DcMotorEx.Direction.REVERSE); //rightFront.setDirection(DcMotorEx.Direction.REVERSE); + + // try to remove encoders + // leftFront.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + // leftBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + // rightFront.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + // rightBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); //TODO End Step 4.2 lastLeftFrontPos = leftFront.getPositionAndVelocity().position; @@ -204,6 +210,11 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { leftBack = hardwareMap.get(DcMotorEx.class, "leftBack"); rightBack = hardwareMap.get(DcMotorEx.class, "rightBack"); rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + // 16265 added + rightBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + rightFront.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + leftFront.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + leftBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); //TODO End Step 1 //TODO Step 4.1 Run MecanumDirectionDebugger Tuning OpMode to set motor direction correctly @@ -233,12 +244,12 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { //TODO Step 3: Specify how the robot should track its position //Comment this line if NOT using Drive Encoder localization - localizer = new DriveLocalizer(); + // localizer = new DriveLocalizer(); //Uncomment next line if using Two Dead Wheel Localizer and also check TwoDeadWheelLocalizer.java for Step 3.1 //localizer = new TwoDeadWheelLocalizer(hardwareMap, imu, PARAMS.inPerTick) //Uncomment next line if using Three Dead Wheel Localizer and also check ThreeDeadWheelLocalizer.java for Step 3.1 - //localizer = new ThreeDeadWheelLocalizer(hardwareMap, PARAMS.inPerTick) + localizer = new ThreeDeadWheelLocalizer(hardwareMap, PARAMS.inPerTick); //TODO End Step 3 FlightRecorder.write("MECANUM_PARAMS", PARAMS); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java index 51056febd1..56ad51c33e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java @@ -32,9 +32,9 @@ public static class Params { public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) { //TODO Step 3.1 : Update hardware configuration names for dead wheel encoders - par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par0"))); - par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par1"))); - perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp"))); + par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "rightFront"))); // was par0 - now 2 = right front + par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "rightBack"))); // was par1 + perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "leftFront"))); // was perp lastPar0Pos = par0.getPositionAndVelocity().position; lastPar1Pos = par1.getPositionAndVelocity().position; diff --git a/build.gradle b/build.gradle index 8969a41590..b26ca8e63a 100644 --- a/build.gradle +++ b/build.gradle @@ -11,7 +11,7 @@ buildscript { } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:7.2.0' + classpath 'com.android.tools.build:gradle:7.4.2' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index aa991fceae..8049c684f0 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-7.5-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists From c8a328ebaf2aeafc7e76c134eaee5517fe8a410d Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 15 Nov 2023 16:23:16 -0700 Subject: [PATCH 079/154] reset to wires --- .../ftc/teamcode/MecanumDrive.java | 39 +++++++------------ .../ftc/teamcode/TheJettCode.java | 5 +-- .../ftc/teamcode/ThreeDeadWheelLocalizer.java | 6 +-- 3 files changed, 18 insertions(+), 32 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 0c84c696de..b3ccb4efad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -51,23 +51,23 @@ public static class Params { // drive model parameters //TODO Step 5 Set value of inPerTick after running ForwardPushTest //TODO Step 14 Make value of inPerTick accurate after running LocalizationTest - public double inPerTick = (112 / 18952.5); + public double inPerTick = 0; //TODO Step 6 (Only for DriveEncoder Localizer) Set value of lateralInPerTick after running LateralPushTest //TODO Step 8 (Only for DeadWheel Localizer) Set value of lateralInPerTick after running LateralRampLogger //TODO Step 14 Make value of lateralInPerTick accurate after running LocalizationTest - public double lateralInPerTick = (112 / 9577); // last run: 0.0118399694452401 + public double lateralInPerTick = 1; //TODO Step 10 (Only for DriveEncoder Localizer) Set value of trackWidthTicks after running AngularRampLogger //TODO Step 11 (Only for DeadWheel Localizer) Set value of trackWidthTicks after running AngularRampLogger // Go to Step 11.1 in Three or Two DeadWheelLocalizer and updated values of par0YTicks, part1YTicks, perpXTicks - public double trackWidthTicks = -3712.5656183161345; + public double trackWidthTicks = 0; // feedforward parameters (in tick units) - //TODO Step 7 (Only for DeadWheel Localizer) Set value for kS eeand KV after running ForwardRampLogger + //TODO Step 7 (Only for DeadWheel Localizer) Set value for kS and KV after running ForwardRampLogger //TODO Step 9 (Only for DriveEncoder Localizer) Set value for kS and kV after running AngularRampLogger - public double kS = 0.9677232995660399; - public double kV = 0.0005787028260206902; + public double kS = 0; + public double kV = 0; //TODO Step 12 Set value of kA after running ManualFeedforwardTuner. In this emperical process update value in increments of 0.0001 public double kA = 0; @@ -133,16 +133,10 @@ public DriveLocalizer() { //TODO Step 4.2 Run MecanumDirectionDebugger Tuning OpMode to set motor direction correctly //Uncomment the lines for which the motorDirection need to be reversed to ensure all motors run forward in test - leftFront.setDirection(DcMotorEx.Direction.REVERSE); - leftBack.setDirection(DcMotorEx.Direction.REVERSE); - //rightBack.setDirection(DcMotorEx.Direction.REVERSE); - //rightFront.setDirection(DcMotorEx.Direction.REVERSE); - - // try to remove encoders - // leftFront.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - // leftBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - // rightFront.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - // rightBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + //leftFront.setDirection(DcMotorEx.Direction.REVERSE); + //leftBack.setDirection(DcMotorEx.Direction.REVERSE); + rightBack.setDirection(DcMotorEx.Direction.REVERSE); + rightFront.setDirection(DcMotorEx.Direction.REVERSE); //TODO End Step 4.2 lastLeftFrontPos = leftFront.getPositionAndVelocity().position; @@ -210,19 +204,14 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { leftBack = hardwareMap.get(DcMotorEx.class, "leftBack"); rightBack = hardwareMap.get(DcMotorEx.class, "rightBack"); rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); - // 16265 added - rightBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - rightFront.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - leftFront.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - leftBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); //TODO End Step 1 //TODO Step 4.1 Run MecanumDirectionDebugger Tuning OpMode to set motor direction correctly //Uncomment the lines for which the motorDirection need to be reversed to ensure all motors run forward in test - leftFront.setDirection(DcMotorEx.Direction.REVERSE); - leftBack.setDirection(DcMotorEx.Direction.REVERSE); - //rightFront.setDirection(DcMotorEx.Direction.REVERSE); - //rightBack.setDirection(DcMotorEx.Direction.REVERSE); + //leftFront.setDirection(DcMotorEx.Direction.REVERSE); + //leftBack.setDirection(DcMotorEx.Direction.REVERSE); + rightFront.setDirection(DcMotorEx.Direction.REVERSE); + rightBack.setDirection(DcMotorEx.Direction.REVERSE); //TODO Make the same update in DriveLocalizer() function. Search for Step 4.2 //TODO End Step 4.1 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java index 5964c70a17..7ac4daa0cc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java @@ -12,10 +12,7 @@ public class TheJettCode extends LinearOpMode { double LiftHeight; boolean LiftMax; - private void ServoNo() { - } - } private void ServoYes() { if (gamepad1.y) { @@ -37,7 +34,7 @@ public void runOpMode() throws InterruptedException { waitForStart(); while (opModeIsActive()) { ServoYes(); - ServoNo(); + telemetry.update(); sleep(100); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java index 56ad51c33e..aa3451d571 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java @@ -32,9 +32,9 @@ public static class Params { public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) { //TODO Step 3.1 : Update hardware configuration names for dead wheel encoders - par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "rightFront"))); // was par0 - now 2 = right front - par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "rightBack"))); // was par1 - perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "leftFront"))); // was perp + par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "rightFront"))); // par0 + par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "rightBack"))); // par1 + perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "leftFront"))); // perp lastPar0Pos = par0.getPositionAndVelocity().position; lastPar1Pos = par1.getPositionAndVelocity().position; From c5f60596fa9abb479219b23b4d1f98994432d0eb Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Wed, 15 Nov 2023 18:03:34 -0700 Subject: [PATCH 080/154] auto Test from jett updated --- .../ftc/teamcode/AutoFunctionJetts.java | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoFunctionJetts.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoFunctionJetts.java index f4d3cd7428..54b657ed05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoFunctionJetts.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoFunctionJetts.java @@ -24,20 +24,16 @@ public void FirstTest() { leftBack.setPower(0.2); leftFront.setPower(0.2); sleep(300); - rightFront.setPower(0); - rightBack.setPower(0); - leftBack.setPower(0); - leftFront.setPower(0); - sleep(300); + rightFront.setPower(-0.25); + rightBack.setPower(-0.25); + leftBack.setPower(0.25); + leftFront.setPower(0.25); + sleep(500); rightFront.setPower(0.2); rightBack.setPower(0.2); leftBack.setPower(0.2); leftFront.setPower(0.2); - sleep(100); - rightFront.setPower(0); - rightBack.setPower(0); - leftBack.setPower(0); - sleep(500); + sleep(300); frontIntake.setPower(-0.3); rearIntake.setPower(-0.3); From fb7c2447133fc6c7a34c50384b37f91e515d3108 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 15 Nov 2023 20:06:54 -0700 Subject: [PATCH 081/154] auto tuning --- .../ftc/teamcode/Beginnings.java | 18 ++++---- .../ftc/teamcode/McTuner3000.java | 6 +-- .../ftc/teamcode/MecanumDrive.java | 43 +++++++++++-------- .../ftc/teamcode/ThreeDeadWheelLocalizer.java | 8 ++-- 4 files changed, 43 insertions(+), 32 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index df9f2e2431..42af47683e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -215,7 +215,7 @@ private void intakeFunction() { if (gamepad1.right_trigger > 0.5) { frontIntake.setPower(0.8); rearIntake.setPower(1); - setLiftHeight(0.45); + //setLiftHeight(0.45); telemetry.addData("Intake", "in"); } else if (gamepad1.left_trigger > 0.5) { @@ -228,7 +228,7 @@ else if (gamepad1.left_trigger > 0.5) { telemetry.addData("Intake", "stopped"); frontIntake.setPower(0); rearIntake.setPower(0); - setLiftHeight(0.42); + //setLiftHeight(0.42); } } @@ -263,7 +263,7 @@ private void dumpLeft() { } } private void driveAroundPos() { - if (gamepad2.dpad_down || gamepad1.x) { + if (gamepad2.dpad_down) { intakePos(); } } @@ -275,18 +275,20 @@ private void drivePos() { currentArmPos = ArmPosition.DRIVE; } private void intakePos() { + //hopper.setPosition(0.02); + //shoulder.setPosition(0.44); + //wrist.setPosition(0.26); + wrist.setPosition(0.265); + shoulder.setPosition(0.455); hopper.setPosition(0.02); - shoulder.setPosition(0.44); - wrist.setPosition(0.26); setLiftHeight(0.42); currentArmPos = ArmPosition.INTAKE; } private void theJuke() { - if ((gamepad2.dpad_up || gamepad1.a) && (currentArmPos == ArmPosition.INTAKE)) { + if ((gamepad2.dpad_up) && (currentArmPos == ArmPosition.INTAKE)) { shoulder.setPosition(0.46); sleep(100); - shoulder.setPosition(0.5); - wrist.setPosition(0.3); + intakePos(); } } private void incrementalIntake() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java index f65169f1ad..815575f7a5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -148,9 +148,9 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Wrist Position", wrist.getPosition()); telemetry.addData("Hopper Position", hopper.getPosition()); - wrist.setPosition(0.51); - shoulder.setPosition(0.44); - hopper.setPosition(0); + wrist.setPosition(0.265); + shoulder.setPosition(0.455); + hopper.setPosition(0.02); leftLift.setPosition(0.42 + LiftLeftOffset); rightLift.setPosition(0.42); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index b3ccb4efad..cf7e5e2a71 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -51,26 +51,26 @@ public static class Params { // drive model parameters //TODO Step 5 Set value of inPerTick after running ForwardPushTest //TODO Step 14 Make value of inPerTick accurate after running LocalizationTest - public double inPerTick = 0; + public double inPerTick = (112 / 38241.5) * (113 / 112); // post-tuning. orig: // ( 112 / 38241.5); // 0.0029287554096989 //TODO Step 6 (Only for DriveEncoder Localizer) Set value of lateralInPerTick after running LateralPushTest //TODO Step 8 (Only for DeadWheel Localizer) Set value of lateralInPerTick after running LateralRampLogger //TODO Step 14 Make value of lateralInPerTick accurate after running LocalizationTest - public double lateralInPerTick = 1; + public double lateralInPerTick = 0.00216486226614309; // needs work? //TODO Step 10 (Only for DriveEncoder Localizer) Set value of trackWidthTicks after running AngularRampLogger //TODO Step 11 (Only for DeadWheel Localizer) Set value of trackWidthTicks after running AngularRampLogger // Go to Step 11.1 in Three or Two DeadWheelLocalizer and updated values of par0YTicks, part1YTicks, perpXTicks - public double trackWidthTicks = 0; + public double trackWidthTicks = 4833.29335965409; // feedforward parameters (in tick units) //TODO Step 7 (Only for DeadWheel Localizer) Set value for kS and KV after running ForwardRampLogger //TODO Step 9 (Only for DriveEncoder Localizer) Set value for kS and kV after running AngularRampLogger - public double kS = 0; - public double kV = 0; + public double kS = 0.8923115403108404; + public double kV = 0.0005909106389939235; //TODO Step 12 Set value of kA after running ManualFeedforwardTuner. In this emperical process update value in increments of 0.0001 - public double kA = 0; + public double kA = 0.0001; // path profile parameters (in inches) public double maxWheelVel = 50; @@ -83,14 +83,14 @@ public static class Params { // path controller gains //TODO Step 13 Set value of Gains after running ManualFeedbackTuner - public double axialGain = 0.0; - public double lateralGain = 0.0; - public double headingGain = 0.0; // shared with turn + public double axialGain = 6.0; + public double lateralGain = 3.0; + public double headingGain = 16.5; // shared with turn public double axialVelGain = 0.0; public double lateralVelGain = 0.0; public double headingVelGain = 0.0; // shared with turn - //TODO End Step 12 + //TODO End Step 13 } public static Params PARAMS = new Params(); @@ -133,10 +133,10 @@ public DriveLocalizer() { //TODO Step 4.2 Run MecanumDirectionDebugger Tuning OpMode to set motor direction correctly //Uncomment the lines for which the motorDirection need to be reversed to ensure all motors run forward in test - //leftFront.setDirection(DcMotorEx.Direction.REVERSE); - //leftBack.setDirection(DcMotorEx.Direction.REVERSE); - rightBack.setDirection(DcMotorEx.Direction.REVERSE); - rightFront.setDirection(DcMotorEx.Direction.REVERSE); + leftFront.setDirection(DcMotorEx.Direction.REVERSE); + leftBack.setDirection(DcMotorEx.Direction.REVERSE); + //rightBack.setDirection(DcMotorEx.Direction.REVERSE); + //rightFront.setDirection(DcMotorEx.Direction.REVERSE); //TODO End Step 4.2 lastLeftFrontPos = leftFront.getPositionAndVelocity().position; @@ -204,14 +204,21 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { leftBack = hardwareMap.get(DcMotorEx.class, "leftBack"); rightBack = hardwareMap.get(DcMotorEx.class, "rightBack"); rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + + // 16265 added + leftFront.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); + leftBack.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); + rightBack.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); + rightFront.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); + //TODO End Step 1 //TODO Step 4.1 Run MecanumDirectionDebugger Tuning OpMode to set motor direction correctly //Uncomment the lines for which the motorDirection need to be reversed to ensure all motors run forward in test - //leftFront.setDirection(DcMotorEx.Direction.REVERSE); - //leftBack.setDirection(DcMotorEx.Direction.REVERSE); - rightFront.setDirection(DcMotorEx.Direction.REVERSE); - rightBack.setDirection(DcMotorEx.Direction.REVERSE); + leftFront.setDirection(DcMotorEx.Direction.REVERSE); + leftBack.setDirection(DcMotorEx.Direction.REVERSE); + //rightFront.setDirection(DcMotorEx.Direction.REVERSE); + //rightBack.setDirection(DcMotorEx.Direction.REVERSE); //TODO Make the same update in DriveLocalizer() function. Search for Step 4.2 //TODO End Step 4.1 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java index aa3451d571..c41932a232 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java @@ -11,15 +11,16 @@ import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; import com.acmerobotics.roadrunner.ftc.RawEncoder; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; @Config public final class ThreeDeadWheelLocalizer implements Localizer { public static class Params { //TODO Step 11.1 : Update values of par0YTicks, part1YTicks, perpXTicks from AngularRampLogger - public double par0YTicks = 0.0; // y position of the first parallel encoder (in tick units) - public double par1YTicks = 1.0; // y position of the second parallel encoder (in tick units) - public double perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units) + public double par0YTicks = 2348.018917004752; // y position of the first parallel encoder (in tick units) + public double par1YTicks = -2583.0561428964884; // y position of the second parallel encoder (in tick units) + public double perpXTicks = -2523.6045747524818; // x position of the perpendicular encoder (in tick units) } public static Params PARAMS = new Params(); @@ -35,6 +36,7 @@ public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) { par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "rightFront"))); // par0 par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "rightBack"))); // par1 perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "leftFront"))); // perp + par0.setDirection(DcMotorSimple.Direction.REVERSE); lastPar0Pos = par0.getPositionAndVelocity().position; lastPar1Pos = par1.getPositionAndVelocity().position; From 0e18c28b970f3fa6b9535421d6885be8eb230c4f Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 15 Nov 2023 20:22:41 -0700 Subject: [PATCH 082/154] gn --- .../main/java/org/firstinspires/ftc/teamcode/Beginnings.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 42af47683e..5b14bf1a9c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -286,8 +286,8 @@ private void intakePos() { } private void theJuke() { if ((gamepad2.dpad_up) && (currentArmPos == ArmPosition.INTAKE)) { - shoulder.setPosition(0.46); - sleep(100); + shoulder.setPosition(0.48); + sleep(200); intakePos(); } } From fa22519a86bbd2cf60988a97d0597ebb104bff68 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Thu, 16 Nov 2023 18:18:16 -0700 Subject: [PATCH 083/154] Egg auto frfrrfrfrfrfrfr -Goober "say ong" - Dimitri --- .../firstinspires/ftc/teamcode/EggAuto.java | 472 ++++++++++++++++++ 1 file changed, 472 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java new file mode 100644 index 0000000000..9ae84edde6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java @@ -0,0 +1,472 @@ +/* Copyright (c) 2019 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 static com.qualcomm.robotcore.util.ElapsedTime.Resolution.SECONDS; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.hardware.rev.RevTouchSensor; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.tfod.TfodProcessor; + +import java.util.List; + +/** + * FTC WIRES Autonomous Example for only vision detection using tensorflow and park + */ +@Autonomous(name = "FTC Wires Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "FTC Wires TeleOp") +public class EggAuto extends LinearOpMode { + + public static String TEAM_NAME = "Enigma"; //TODO: Enter team Name + public static int TEAM_NUMBER = 16265; //TODO: Enter team Number + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + //Vision parameters + private TfodProcessor tfod; + private VisionPortal visionPortal; + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; + private Servo launcher; + private Servo rightLift; + private Servo leftLift; + private Servo shoulder; + private Servo wrist; + private Servo hopper; + DcMotor frontIntake; + DcMotor rearIntake; + + double LiftLeftOffset = .04; + double LiftHeight; + + //MecanumDrive drive; + //OgDrive ogDrive; + //Define and declare Robot Starting Locations + public enum START_POSITION{ + BLUE_LEFT, + BLUE_RIGHT, + RED_LEFT, + RED_RIGHT + } + public static START_POSITION startPosition; + + public enum IDENTIFIED_SPIKE_MARK_LOCATION { + LEFT, + MIDDLE, + RIGHT + } + public static IDENTIFIED_SPIKE_MARK_LOCATION identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; + + @Override + public void runOpMode() throws InterruptedException { + launcher = hardwareMap.get(Servo.class, "launcher"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + wrist = hardwareMap.get(Servo.class, "wrist"); + hopper = hardwareMap.get(Servo.class, "hopper"); + shoulder = hardwareMap.get(Servo.class, "shoulder"); + + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + launcher.setDirection(Servo.Direction.REVERSE); + + frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); + rearIntake = hardwareMap.get(DcMotor.class, "rearIntake"); + + frontIntake.setDirection(DcMotorSimple.Direction.FORWARD); + rearIntake.setDirection(DcMotorSimple.Direction.REVERSE); + frontIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + //Key Pay inputs to selecting Starting Position of robot + selectStartingPosition(); + telemetry.addData("Selected Starting Position", startPosition); + + //Activate Camera Vision that uses TensorFlow for pixel detection + initTfod(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.update(); + //waitForStart(); + + while (!isStopRequested() && !opModeIsActive()) { + telemetry.addData("Selected Starting Position", startPosition); + + //Run Vuforia Tensor Flow and keep watching for the identifier in the Signal Cone. + runTfodTensorFlow(); + telemetry.addData("Vision identified Parking Location", identifiedSpikeMarkLocation); + telemetry.update(); + } + + //Game Play Button is pressed + if (opModeIsActive() && !isStopRequested()) { + //Build parking trajectory based on last detected target by vision + runAutonoumousMode(); + } + } // end runOpMode() + + private void setLiftHeight(double inputLiftHeight) { + if (inputLiftHeight < 0.42) { + inputLiftHeight = 0.42; + } + if (inputLiftHeight > 1) { + inputLiftHeight = 1; + } + LiftHeight = inputLiftHeight; + leftLift.setPosition(LiftLeftOffset + LiftHeight); + rightLift.setPosition(LiftHeight); + + } + + + + public void runAutonoumousMode() { + //Initialize Pose2d as desired + Pose2d initPose = new Pose2d(0, 0, 0); // Starting Pose + Pose2d moveBeyondTrussPose = new Pose2d(0,0,0); + Pose2d dropPurplePixelPose = new Pose2d(0, 0, 0); + Pose2d midwayPose1 = new Pose2d(0,0,0); + Pose2d midwayPose1a = new Pose2d(0,0,0); + Pose2d intakeStack = new Pose2d(0,0,0); + Pose2d midwayPose2 = new Pose2d(0,0,0); + Pose2d dropYellowPixelPose = new Pose2d(0, 0, 0); + Pose2d parkPose = new Pose2d(0,0, 0); + double waitSecondsBeforeDrop = 0; + MecanumDrive drive = new MecanumDrive(hardwareMap, initPose); + + initPose = new Pose2d(0, 0, Math.toRadians(0)); //Starting pose + moveBeyondTrussPose = new Pose2d(15,0,0); + + switch (startPosition) { + case BLUE_LEFT: + drive = new MecanumDrive(hardwareMap, initPose); + switch(identifiedSpikeMarkLocation){ + case LEFT: + dropPurplePixelPose = new Pose2d(26, 8, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(23, 36, Math.toRadians(-90)); + break; + case MIDDLE: + dropPurplePixelPose = new Pose2d(30, 3, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(30, 36, Math.toRadians(-90)); + break; + case RIGHT: + dropPurplePixelPose = new Pose2d(30, -9, Math.toRadians(-45)); + dropYellowPixelPose = new Pose2d(37, 36, Math.toRadians(-90)); + break; + } + midwayPose1 = new Pose2d(14, 13, Math.toRadians(-45)); + waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + parkPose = new Pose2d(8, 30, Math.toRadians(-90)); + break; + + case RED_RIGHT: + drive = new MecanumDrive(hardwareMap, initPose); + switch(identifiedSpikeMarkLocation){ + case LEFT: + dropPurplePixelPose = new Pose2d(30, 9, Math.toRadians(45)); + dropYellowPixelPose = new Pose2d(21, -36, Math.toRadians(90)); + break; + case MIDDLE: + dropPurplePixelPose = new Pose2d(30, -3, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(29, -36, Math.toRadians(90)); + break; + case RIGHT: + dropPurplePixelPose = new Pose2d(26, -8, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(37, -36, Math.toRadians(90)); + break; + } + midwayPose1 = new Pose2d(14, -13, Math.toRadians(45)); + waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + parkPose = new Pose2d(8, -30, Math.toRadians(90)); + break; + + case BLUE_RIGHT: + drive = new MecanumDrive(hardwareMap, initPose); + switch(identifiedSpikeMarkLocation){ + case LEFT: + dropPurplePixelPose = new Pose2d(27, 9, Math.toRadians(45)); + dropYellowPixelPose = new Pose2d(27, 86, Math.toRadians(-90)); + break; + case MIDDLE: + dropPurplePixelPose = new Pose2d(30, -3, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(34, 86, Math.toRadians(-90)); + break; + case RIGHT: + dropPurplePixelPose = new Pose2d(26, -8, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(43, 86, Math.toRadians(-90)); + break; + } + midwayPose1 = new Pose2d(8, -8, Math.toRadians(0)); + midwayPose1a = new Pose2d(18, -18, Math.toRadians(-90)); + intakeStack = new Pose2d(52, -19,Math.toRadians(-90)); + midwayPose2 = new Pose2d(52, 62, Math.toRadians(-90)); + waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + parkPose = new Pose2d(50, 84, Math.toRadians(-90)); + break; + + case RED_LEFT: + drive = new MecanumDrive(hardwareMap, initPose); + switch(identifiedSpikeMarkLocation){ + case LEFT: + dropPurplePixelPose = new Pose2d(26, 8, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(37, -86, Math.toRadians(90)); + break; + case MIDDLE: + dropPurplePixelPose = new Pose2d(30, -3, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(29, -86, Math.toRadians(90)); + break; + case RIGHT: + dropPurplePixelPose = new Pose2d(27, -9, Math.toRadians(-45)); + dropYellowPixelPose = new Pose2d(21, -86, Math.toRadians(90)); + break; + } + midwayPose1 = new Pose2d(8, 8, Math.toRadians(0)); + midwayPose1a = new Pose2d(18, 18, Math.toRadians(90)); + intakeStack = new Pose2d(52, 19,Math.toRadians(90)); + midwayPose2 = new Pose2d(52, -62, Math.toRadians(90)); + waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + parkPose = new Pose2d(50, -84, Math.toRadians(90)); + break; + } + + //Move robot to dropPurplePixel based on identified Spike Mark Location + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(moveBeyondTrussPose.position, moveBeyondTrussPose.heading) + .strafeToLinearHeading(dropPurplePixelPose.position, dropPurplePixelPose.heading) + .build()); + + //TODO : Code to drop Purple Pixel on Spike Mark + safeWaitSeconds(1); + + //Move robot to midwayPose1 + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(midwayPose1.position, midwayPose1.heading) + .build()); + + //For Blue Right and Red Left, intake pixel from stack + if (startPosition == START_POSITION.BLUE_RIGHT || + startPosition == START_POSITION.RED_LEFT) { + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(midwayPose1a.position, midwayPose1a.heading) + .strafeToLinearHeading(intakeStack.position, intakeStack.heading) + .build()); + + //TODO : Code to intake pixel from stack + safeWaitSeconds(1); + + //Move robot to midwayPose2 and to dropYellowPixelPose + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(midwayPose2.position, midwayPose2.heading) + .build()); + } + + safeWaitSeconds(waitSecondsBeforeDrop); + + //Move robot to midwayPose2 and to dropYellowPixelPose + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .setReversed(true) + .splineToLinearHeading(dropYellowPixelPose,0) + .build()); + + + //TODO : Code to drop Pixel on Backdrop + safeWaitSeconds(1); + shoulder.setPosition(0.60); + hopper.setPosition(0.02); + wrist.setPosition(0.22); + setLiftHeight(0.42); + sleep(500); + wrist.setPosition(0.22); + shoulder.setPosition(0.79); + hopper.setPosition(0.02); + setLiftHeight(0.42); + sleep(500); + hopper.setPosition(0.58); + wrist.setPosition(0.6); + shoulder.setPosition(0.79); + setLiftHeight(0.42); + sleep(500); + shoulder.setPosition(0.91); + hopper.setPosition(0.58); + wrist.setPosition(0.6); + setLiftHeight(0.42); + sleep(500); + wrist.setPosition(0.6); + shoulder.setPosition(1); + hopper.setPosition(0.58); + setLiftHeight(0.42); + sleep(500); + //dump + hopper.setPosition(0.2); + + //Move robot to park in Backstage + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(parkPose.position, parkPose.heading) + //.splineToLinearHeading(parkPose,0) + .build()); + } + + + //Method to select starting position using X, Y, A, B buttons on gamepad + public void selectStartingPosition() { + telemetry.setAutoClear(true); + telemetry.clearAll(); + //******select start pose***** + while(!isStopRequested()){ + telemetry.addData("Initializing FTC Wires (ftcwires.org) Autonomous adopted for Team:", + TEAM_NAME, " ", TEAM_NUMBER); + telemetry.addData("---------------------------------------",""); + telemetry.addData("Select Starting Position using XYAB on Logitech (or ▢ΔOX on Playstayion) on gamepad 1:",""); + telemetry.addData(" Blue Left ", "(X / ▢)"); + telemetry.addData(" Blue Right ", "(Y / Δ)"); + telemetry.addData(" Red Left ", "(B / O)"); + telemetry.addData(" Red Right ", "(A / X)"); + if(gamepad1.x){ + startPosition = START_POSITION.BLUE_LEFT; + break; + } + if(gamepad1.y){ + startPosition = START_POSITION.BLUE_RIGHT; + break; + } + if(gamepad1.b){ + startPosition = START_POSITION.RED_LEFT; + break; + } + if(gamepad1.a){ + startPosition = START_POSITION.RED_RIGHT; + break; + } + telemetry.update(); + } + telemetry.clearAll(); + } + + //method to wait safely with stop button working if needed. Use this instead of sleep + public void safeWaitSeconds(double time) { + ElapsedTime timer = new ElapsedTime(SECONDS); + timer.reset(); + while (!isStopRequested() && timer.time() < time) { + } + } + + /** + * Initialize the TensorFlow Object Detection processor. + */ + private void initTfod() { + + // Create the TensorFlow processor the easy way. + tfod = TfodProcessor.easyCreateWithDefaults(); + + // Create the vision portal the easy way. + if (USE_WEBCAM) { + visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, "Webcam 1"), tfod); + } else { + visionPortal = VisionPortal.easyCreateWithDefaults( + BuiltinCameraDirection.BACK, tfod); + } + + // Set confidence threshold for TFOD recognitions, at any time. + tfod.setMinResultConfidence(0.095f); + + } // end method initTfod() + + /** + * Add telemetry about TensorFlow Object Detection (TFOD) recognitions. + */ + private void runTfodTensorFlow() { + + List currentRecognitions = tfod.getRecognitions(); + telemetry.addData("# Objects Detected", currentRecognitions.size()); + + //Camera placed between Left and Right Spike Mark on RED_LEFT and BLUE_LEFT If pixel not visible, assume Right spike Mark + if (startPosition == START_POSITION.RED_LEFT || startPosition == START_POSITION.BLUE_LEFT) { + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.RIGHT; + } else { //RED_RIGHT or BLUE_RIGHT + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; + } + + // Step through the list of recognitions and display info for each one. + for (Recognition recognition : currentRecognitions) { + double x = (recognition.getLeft() + recognition.getRight()) / 2 ; + double y = (recognition.getTop() + recognition.getBottom()) / 2 ; + + telemetry.addData(""," "); + telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); + telemetry.addData("- Position", "%.0f / %.0f", x, y); + telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); + + if (startPosition == START_POSITION.RED_LEFT || startPosition == START_POSITION.BLUE_LEFT) { + if (recognition.getLabel() == "Pixel") { + if (x < 200) { + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; + } else { + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.MIDDLE; + } + } + } else { //RED_RIGHT or BLUE_RIGHT + if (recognition.getLabel() == "Pixel") { + if (x < 200) { + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.MIDDLE; + } else { + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.RIGHT; + } + } + } + + } // end for() loop + + } // end method runTfodTensorFlow() + +} // end class From f612b86ca9c55dabbc062a5126cfc2a5cf21025f Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Thu, 16 Nov 2023 18:34:45 -0700 Subject: [PATCH 084/154] limbo? by mindcap? the hardest memory demon? Key world? "Ispywithmylittleeye" - Taqavian -The g --- .../org/firstinspires/ftc/teamcode/EggAuto.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java index 9ae84edde6..37cae3ca96 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java @@ -67,7 +67,6 @@ public class EggAuto extends LinearOpMode { private RevTouchSensor leftUpper; private RevTouchSensor rightLower; private RevTouchSensor leftLower; - private Servo launcher; private Servo rightLift; private Servo leftLift; private Servo shoulder; @@ -99,16 +98,12 @@ public enum IDENTIFIED_SPIKE_MARK_LOCATION { @Override public void runOpMode() throws InterruptedException { - launcher = hardwareMap.get(Servo.class, "launcher"); - rightLift = hardwareMap.get(Servo.class, "rightLift"); - leftLift = hardwareMap.get(Servo.class, "leftLift"); wrist = hardwareMap.get(Servo.class, "wrist"); hopper = hardwareMap.get(Servo.class, "hopper"); shoulder = hardwareMap.get(Servo.class, "shoulder"); shoulder.setDirection(Servo.Direction.REVERSE); wrist.setDirection(Servo.Direction.REVERSE); - launcher.setDirection(Servo.Direction.REVERSE); frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); rearIntake = hardwareMap.get(DcMotor.class, "rearIntake"); @@ -282,7 +277,13 @@ public void runAutonoumousMode() { //TODO : Code to drop Purple Pixel on Spike Mark safeWaitSeconds(1); - + frontIntake.setPower(-1); + rearIntake.setPower(-1); + sleep(1500); + frontIntake.setPower(0); + rearIntake.setPower(0); + //drive pos \/ + shoulder.setPosition(0.48); //Move robot to midwayPose1 Actions.runBlocking( drive.actionBuilder(drive.pose) @@ -429,7 +430,6 @@ private void runTfodTensorFlow() { List currentRecognitions = tfod.getRecognitions(); telemetry.addData("# Objects Detected", currentRecognitions.size()); - //Camera placed between Left and Right Spike Mark on RED_LEFT and BLUE_LEFT If pixel not visible, assume Right spike Mark if (startPosition == START_POSITION.RED_LEFT || startPosition == START_POSITION.BLUE_LEFT) { identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.RIGHT; From 5af4a5317708994727fc2e0386d6bf008e3861bb Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Thu, 16 Nov 2023 18:44:21 -0700 Subject: [PATCH 085/154] opencv initial --- TeamCode/build.gradle | 1 + .../firstinspires/ftc/teamcode/EggAuto.java | 2 +- .../ftc/teamcode/EnigmaAuto.java | 187 +++++++++++------- build.common.gradle | 4 +- 4 files changed, 115 insertions(+), 79 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index c46718aec0..60cf35079c 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -35,4 +35,5 @@ dependencies { implementation "com.acmerobotics.roadrunner:ftc:0.1.8" implementation "com.acmerobotics.dashboard:dashboard:0.4.14" + implementation "org.openftc:easyopencv:1.5.1" } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java index 9ae84edde6..b327dffae7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java @@ -52,7 +52,7 @@ /** * FTC WIRES Autonomous Example for only vision detection using tensorflow and park */ -@Autonomous(name = "FTC Wires Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "FTC Wires TeleOp") +@Autonomous(name = "EggAuto", group = "00-Autonomous", preselectTeleOp = "Beginnings") public class EggAuto extends LinearOpMode { public static String TEAM_NAME = "Enigma"; //TODO: Enter team Name diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 4f543bfdb8..da7f1f1b9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -37,18 +37,33 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.teamcode.MecanumDrive; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvInternalCamera; +import org.openftc.easyopencv.OpenCvPipeline; +/* import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.tfod.Recognition; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.tfod.TfodProcessor; - +*/ +import java.util.ArrayList; +import java.util.Locale; import java.util.List; /** - * FTC WIRES Autonomous Example for only vision detection using tensorflow and park + * ENIGMA Autonomous Example for only vision detection using openCv and park */ -@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "ENIGMA TeleOp") +@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "Beginnings") public class EnigmaAuto extends LinearOpMode { public static String TEAM_NAME = "ENIGMA"; //TODO: Enter team Name @@ -57,8 +72,8 @@ public class EnigmaAuto extends LinearOpMode { private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera //Vision parameters - private TfodProcessor tfod; - private VisionPortal visionPortal; + //private TfodProcessor tfod; + //private VisionPortal visionPortal; //Define and declare Robot Starting Locations public enum START_POSITION{ @@ -69,6 +84,11 @@ public enum START_POSITION{ } public static START_POSITION startPosition; + /* + OpenCV / Color Detection + */ + OpenCvCamera webcam1 = null; + public enum IDENTIFIED_SPIKE_MARK_LOCATION { LEFT, MIDDLE, @@ -79,12 +99,35 @@ public enum IDENTIFIED_SPIKE_MARK_LOCATION { @Override public void runOpMode() throws InterruptedException { + // Vision OpenCV / Color Detection + WebcamName webcamName = hardwareMap.get(WebcamName.class, "Webcam 1"); + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + webcam1 = OpenCvCameraFactory.getInstance().createInternalCamera(OpenCvInternalCamera.CameraDirection.BACK, cameraMonitorViewId); + //webcam1 = OpenCvCameraFactory.getInstance().createWebcam(webcamName, cameraMonitorViewId); + + webcam1.setPipeline(new teamElementPipeline()); + + webcam1.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + webcam1.startStreaming(1280, 720, OpenCvCameraRotation.SENSOR_NATIVE); + } + + @Override + public void onError(int errorCode) { + + } + }); + + + telemetry.setMsTransmissionInterval(50); + //Key Pay inputs to selecting Starting Position of robot selectStartingPosition(); telemetry.addData("Selected Starting Position", startPosition); //Activate Camera Vision that uses TensorFlow for pixel detection - initTfod(); + //initTfod(); // Wait for the DS start button to be touched. telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); @@ -96,7 +139,7 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Selected Starting Position", startPosition); //Run Vuforia Tensor Flow and keep watching for the identifier in the Signal Cone. - runTfodTensorFlow(); + // runTfodTensorFlow(); telemetry.addData("Vision identified Parking Location", identifiedSpikeMarkLocation); telemetry.update(); } @@ -280,7 +323,7 @@ public void selectStartingPosition() { telemetry.clearAll(); //******select start pose***** while(!isStopRequested()){ - telemetry.addData("Initializing FTC Wires (ftcwires.org) Autonomous adopted for Team:", + telemetry.addData("Initializing ENIGMA Autonomous Team# a", TEAM_NAME, " ", TEAM_NUMBER); telemetry.addData("---------------------------------------",""); telemetry.addData("Select Starting Position using XYAB on Logitech (or ▢ΔOX on Playstayion) on gamepad 1:",""); @@ -317,73 +360,65 @@ public void safeWaitSeconds(double time) { } } - /** - * Initialize the TensorFlow Object Detection processor. - */ - private void initTfod() { - - // Create the TensorFlow processor the easy way. - tfod = TfodProcessor.easyCreateWithDefaults(); - - // Create the vision portal the easy way. - if (USE_WEBCAM) { - visionPortal = VisionPortal.easyCreateWithDefaults( - hardwareMap.get(WebcamName.class, "Webcam 1"), tfod); - } else { - visionPortal = VisionPortal.easyCreateWithDefaults( - BuiltinCameraDirection.BACK, tfod); - } - - // Set confidence threshold for TFOD recognitions, at any time. - tfod.setMinResultConfidence(0.095f); - - } // end method initTfod() - - /** - * Add telemetry about TensorFlow Object Detection (TFOD) recognitions. - */ - private void runTfodTensorFlow() { - - List currentRecognitions = tfod.getRecognitions(); - telemetry.addData("# Objects Detected", currentRecognitions.size()); - - //Camera placed between Left and Right Spike Mark on RED_LEFT and BLUE_LEFT If pixel not visible, assume Right spike Mark - if (startPosition == START_POSITION.RED_LEFT || startPosition == START_POSITION.BLUE_LEFT) { - identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.RIGHT; - } else { //RED_RIGHT or BLUE_RIGHT - identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; - } - - // Step through the list of recognitions and display info for each one. - for (Recognition recognition : currentRecognitions) { - double x = (recognition.getLeft() + recognition.getRight()) / 2 ; - double y = (recognition.getTop() + recognition.getBottom()) / 2 ; - - telemetry.addData(""," "); - telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100); - telemetry.addData("- Position", "%.0f / %.0f", x, y); - telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight()); - - if (startPosition == START_POSITION.RED_LEFT || startPosition == START_POSITION.BLUE_LEFT) { - if (recognition.getLabel() == "Pixel") { - if (x < 200) { - identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; - } else { - identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.MIDDLE; - } - } - } else { //RED_RIGHT or BLUE_RIGHT - if (recognition.getLabel() == "Pixel") { - if (x < 200) { - identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.MIDDLE; - } else { - identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.RIGHT; - } - } +class teamElementPipeline extends OpenCvPipeline{ + Mat YCbCr = new Mat(); + Mat leftCrop; + Mat centerCrop; + Mat rightCrop; + double leftavgfin; + double centeravgfin; + double rightavgfin; + Mat outPut = new Mat(); + Scalar rectColor = new Scalar(255.0, 0.0, 0.0); + + public Mat processFrame(Mat input){ + Imgproc.cvtColor(input,YCbCr,Imgproc.COLOR_RGB2YCrCb); + telemetry.addLine("pipeline running"); + /* + Rect leftRect = new Rect(1,1,212, 479); + Rect centerRect = new Rect(213,1,212, 479); + Rect rightRect = new Rect(426,1,213, 479); + */ + Rect leftRect = new Rect(1,1,1280/3, 719); + Rect centerRect = new Rect((1280/3+1),1,1280/3, 719); + Rect rightRect = new Rect((1280/3*2)+1,1,1280/3, 719); + input.copyTo(outPut); + + + Imgproc.rectangle(outPut, leftRect, rectColor, 2); + Imgproc.rectangle(outPut, centerRect, rectColor, 2); + Imgproc.rectangle(outPut, rightRect, rectColor, 2); + + leftCrop = YCbCr.submat(leftRect); + centerCrop = YCbCr.submat(centerRect); + rightCrop = YCbCr.submat(rightRect); + + Core.extractChannel(leftCrop, leftCrop, 2); + Core.extractChannel(centerCrop, centerCrop, 2); + Core.extractChannel(rightCrop, rightCrop, 2); + + Scalar leftavg = Core.mean(leftCrop); + Scalar centeravg = Core.mean(centerCrop); + Scalar rightavg = Core.mean(rightCrop); + + leftavgfin = leftavg.val[0]; + centeravgfin = centeravg.val[0]; + rightavgfin = rightavg.val[0]; + + if (leftavgfin > centeravgfin && leftavgfin > rightavgfin) { + telemetry.addLine("Left"); + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; + } else if (centeravgfin > leftavgfin && centeravgfin > rightavgfin) { + telemetry.addLine("Center"); + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.MIDDLE; + } else if (rightavgfin > leftavgfin && rightavgfin > centeravgfin) { + telemetry.addLine("Right"); + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.RIGHT; + } else { + telemetry.addLine("Failed to detect position"); + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.RIGHT; } - - } // end for() loop - - } // end method runTfodTensorFlow() - + return (outPut); + } +} } // end class diff --git a/build.common.gradle b/build.common.gradle index 12e6acb06a..c85d18a472 100644 --- a/build.common.gradle +++ b/build.common.gradle @@ -93,14 +93,14 @@ android { signingConfig signingConfigs.release ndk { - abiFilters "armeabi-v7a", "arm64-v8a" + abiFilters "armeabi-v7a" } } debug { debuggable true jniDebuggable true ndk { - abiFilters "armeabi-v7a", "arm64-v8a" + abiFilters "armeabi-v7a" } } } From c2d6849ef064c90f626ada0b0be704f6dcfdb33e Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Thu, 16 Nov 2023 19:18:00 -0700 Subject: [PATCH 086/154] lift exist now -The g --- .../src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java index 37cae3ca96..e83183fc2d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java @@ -101,6 +101,8 @@ public void runOpMode() throws InterruptedException { wrist = hardwareMap.get(Servo.class, "wrist"); hopper = hardwareMap.get(Servo.class, "hopper"); shoulder = hardwareMap.get(Servo.class, "shoulder"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); shoulder.setDirection(Servo.Direction.REVERSE); wrist.setDirection(Servo.Direction.REVERSE); From 667785c229cf887538c141b7fd4900a3d1ae516c Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Thu, 16 Nov 2023 19:30:16 -0700 Subject: [PATCH 087/154] almost good auto. the arm now goes down -The g --- .../firstinspires/ftc/teamcode/EggAuto.java | 25 ++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java index fb38ae4818..073b0009be 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java @@ -350,7 +350,30 @@ public void runAutonoumousMode() { sleep(500); //dump hopper.setPosition(0.2); - + sleep(1000); + wrist.setPosition(0.6); + shoulder.setPosition(1); + hopper.setPosition(0.58); + setLiftHeight(0.42); + sleep(200); + shoulder.setPosition(0.91); + hopper.setPosition(0.58); + wrist.setPosition(0.6); + setLiftHeight(0.42); + sleep(200); + hopper.setPosition(0.58); + wrist.setPosition(0.6); + shoulder.setPosition(0.79); + sleep(350); + wrist.setPosition(0.22); + shoulder.setPosition(0.79); + hopper.setPosition(0.02); + setLiftHeight(0.42); + sleep(500); + shoulder.setPosition(0.60); + hopper.setPosition(0.02); + wrist.setPosition(0.22); + setLiftHeight(0.42); //Move robot to park in Backstage Actions.runBlocking( drive.actionBuilder(drive.pose) From d5cf81a0c0b61d1448649475bfba01493bd7d4cd Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Thu, 16 Nov 2023 20:27:51 -0700 Subject: [PATCH 088/154] opencv work --- .../ftc/teamcode/EnigmaAuto.java | 24 ++++++++++++++----- 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index da7f1f1b9a..029a309751 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -95,6 +95,11 @@ public enum IDENTIFIED_SPIKE_MARK_LOCATION { RIGHT } public static IDENTIFIED_SPIKE_MARK_LOCATION identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; + public static double leftavgfinoutput = 0; + public static double centeravgfinoutput = 0; + public static double rightavgfinoutput = 0; + + @Override public void runOpMode() throws InterruptedException { @@ -141,6 +146,9 @@ public void onError(int errorCode) { //Run Vuforia Tensor Flow and keep watching for the identifier in the Signal Cone. // runTfodTensorFlow(); telemetry.addData("Vision identified Parking Location", identifiedSpikeMarkLocation); + telemetry.addData("leftavfin", leftavgfinoutput); + telemetry.addData("centeravfin", centeravgfinoutput); + telemetry.addData("rightavfin", rightavgfinoutput); telemetry.update(); } @@ -379,9 +387,9 @@ public Mat processFrame(Mat input){ Rect centerRect = new Rect(213,1,212, 479); Rect rightRect = new Rect(426,1,213, 479); */ - Rect leftRect = new Rect(1,1,1280/3, 719); - Rect centerRect = new Rect((1280/3+1),1,1280/3, 719); - Rect rightRect = new Rect((1280/3*2)+1,1,1280/3, 719); + Rect leftRect = new Rect(1,300,200, 400); + Rect centerRect = new Rect(400,300,500, 200); + Rect rightRect = new Rect(1000,300,260, 400); input.copyTo(outPut); @@ -393,9 +401,9 @@ public Mat processFrame(Mat input){ centerCrop = YCbCr.submat(centerRect); rightCrop = YCbCr.submat(rightRect); - Core.extractChannel(leftCrop, leftCrop, 2); - Core.extractChannel(centerCrop, centerCrop, 2); - Core.extractChannel(rightCrop, rightCrop, 2); + Core.extractChannel(leftCrop, leftCrop, 0); + Core.extractChannel(centerCrop, centerCrop, 0); + Core.extractChannel(rightCrop, rightCrop, 0); // blue is 0, green is 1, red is 2 Scalar leftavg = Core.mean(leftCrop); Scalar centeravg = Core.mean(centerCrop); @@ -405,6 +413,10 @@ public Mat processFrame(Mat input){ centeravgfin = centeravg.val[0]; rightavgfin = rightavg.val[0]; + leftavgfinoutput = leftavgfin; + centeravgfinoutput = centeravgfin; + rightavgfinoutput = rightavgfin; + if (leftavgfin > centeravgfin && leftavgfin > rightavgfin) { telemetry.addLine("Left"); identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; From 9cb17097617fc1aa1eb1f7fb8a3fa98f5cb6e534 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 17 Nov 2023 12:57:41 -0700 Subject: [PATCH 089/154] auto work and remove unused opmodes --- .../ftc/teamcode/AutoFunctionJetts.java | 2 +- .../firstinspires/ftc/teamcode/BlueLeftE.java | 2 +- .../ConceptTensorFlowObjectDetection.java | 2 +- .../firstinspires/ftc/teamcode/EggAuto.java | 2 +- .../ftc/teamcode/EnigmaAuto.java | 129 ++++++++++++++---- .../ftc/teamcode/FTCWiresAutonomous.java | 2 +- .../ftc/teamcode/FTCWiresTeleOpMode.java | 2 +- .../firstinspires/ftc/teamcode/OdoMec.java | 2 +- .../ftc/teamcode/TheJettCode.java | 2 +- .../ftc/teamcode/apriltagDimitri.java | 2 +- 10 files changed, 109 insertions(+), 38 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoFunctionJetts.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoFunctionJetts.java index 54b657ed05..4dd814b012 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoFunctionJetts.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoFunctionJetts.java @@ -6,7 +6,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; -@Autonomous +//@Autonomous public class AutoFunctionJetts extends LinearOpMode { private DcMotor rightFront; //front right 0 private DcMotor leftFront; //front left 2 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeftE.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeftE.java index 668e900d79..3c9e56f78d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeftE.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeftE.java @@ -43,7 +43,7 @@ import java.util.ArrayList; -@Autonomous(name= "Left Auto") +//@Autonomous(name= "Left Auto") public class BlueLeftE extends LinearOpMode { // Declare vars diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTensorFlowObjectDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTensorFlowObjectDetection.java index f841e5fb97..0cb6466e4e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTensorFlowObjectDetection.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTensorFlowObjectDetection.java @@ -48,7 +48,7 @@ * 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. */ -@TeleOp(name = "Concept: TensorFlow Object Detection", group = "Concept") +//@TeleOp(name = "Concept: TensorFlow Object Detection", group = "Concept") public class ConceptTensorFlowObjectDetection extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java index 073b0009be..062ecdaef8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EggAuto.java @@ -52,7 +52,7 @@ /** * FTC WIRES Autonomous Example for only vision detection using tensorflow and park */ -@Autonomous(name = "EggAuto", group = "00-Autonomous", preselectTeleOp = "Beginnings") +//@Autonomous(name = "EggAuto", group = "00-Autonomous", preselectTeleOp = "Beginnings") public class EggAuto extends LinearOpMode { public static String TEAM_NAME = "Enigma"; //TODO: Enter team Name diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 029a309751..0fa755c967 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -35,10 +35,16 @@ import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.hardware.rev.RevTouchSensor; + import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.teamcode.MecanumDrive; + import org.opencv.core.Core; import org.opencv.core.Mat; import org.opencv.core.Rect; @@ -49,13 +55,7 @@ import org.openftc.easyopencv.OpenCvCameraRotation; import org.openftc.easyopencv.OpenCvInternalCamera; import org.openftc.easyopencv.OpenCvPipeline; -/* -import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.robotcore.external.tfod.Recognition; -import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.tfod.TfodProcessor; -*/ + import java.util.ArrayList; import java.util.Locale; import java.util.List; @@ -71,9 +71,20 @@ public class EnigmaAuto extends LinearOpMode { private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera - //Vision parameters - //private TfodProcessor tfod; - //private VisionPortal visionPortal; + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; + private Servo rightLift; + private Servo leftLift; + private Servo shoulder; + private Servo wrist; + private Servo hopper; + DcMotor frontIntake; + DcMotor rearIntake; + + double LiftLeftOffset = .04; + double LiftHeight; //Define and declare Robot Starting Locations public enum START_POSITION{ @@ -103,7 +114,24 @@ public enum IDENTIFIED_SPIKE_MARK_LOCATION { @Override public void runOpMode() throws InterruptedException { - + wrist = hardwareMap.get(Servo.class, "wrist"); + hopper = hardwareMap.get(Servo.class, "hopper"); + shoulder = hardwareMap.get(Servo.class, "shoulder"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + + frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); + rearIntake = hardwareMap.get(DcMotor.class, "rearIntake"); + + frontIntake.setDirection(DcMotorSimple.Direction.FORWARD); + rearIntake.setDirection(DcMotorSimple.Direction.REVERSE); + frontIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); // Vision OpenCV / Color Detection WebcamName webcamName = hardwareMap.get(WebcamName.class, "Webcam 1"); int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); @@ -158,11 +186,23 @@ public void onError(int errorCode) { runAutonoumousMode(); } } // end runOpMode() - + private void setLiftHeight(double inputLiftHeight) { + if (inputLiftHeight < 0.42) { + inputLiftHeight = 0.42; + } + if (inputLiftHeight > 1) { + inputLiftHeight = 1; + } + LiftHeight = inputLiftHeight; + leftLift.setPosition(LiftLeftOffset + LiftHeight); + rightLift.setPosition(LiftHeight); + } public void runAutonoumousMode() { //Initialize Pose2d as desired Pose2d initPose = new Pose2d(0, 0, 0); // Starting Pose Pose2d moveBeyondTrussPose = new Pose2d(0,0,0); + Pose2d dropPurplePixelPosePush = new Pose2d(0, 0, 0); + //Pose2d dropPurplePixelPosePos = new Pose2d(0, 0, 0); Pose2d dropPurplePixelPose = new Pose2d(0, 0, 0); Pose2d midwayPose1 = new Pose2d(0,0,0); Pose2d midwayPose1a = new Pose2d(0,0,0); @@ -223,20 +263,23 @@ public void runAutonoumousMode() { drive = new MecanumDrive(hardwareMap, initPose); switch(identifiedSpikeMarkLocation){ case LEFT: - dropPurplePixelPose = new Pose2d(27, 9, Math.toRadians(45)); + dropPurplePixelPosePush = new Pose2d(27, 9, Math.toRadians(45)); + dropPurplePixelPose = new Pose2d(23.5, 7, Math.toRadians(35)); dropYellowPixelPose = new Pose2d(27, 86, Math.toRadians(-90)); break; case MIDDLE: - dropPurplePixelPose = new Pose2d(30, -3, Math.toRadians(0)); + dropPurplePixelPosePush = new Pose2d(30.5, 0, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(26.5, -3, Math.toRadians(0)); dropYellowPixelPose = new Pose2d(34, 86, Math.toRadians(-90)); break; case RIGHT: - dropPurplePixelPose = new Pose2d(26, -8, Math.toRadians(0)); + dropPurplePixelPosePush = new Pose2d(27, -8, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(23, -11, Math.toRadians(0)); dropYellowPixelPose = new Pose2d(43, 86, Math.toRadians(-90)); break; } midwayPose1 = new Pose2d(8, -8, Math.toRadians(0)); - midwayPose1a = new Pose2d(18, -18, Math.toRadians(-90)); + midwayPose1a = new Pose2d(18, -21, Math.toRadians(-90)); intakeStack = new Pose2d(52, -19,Math.toRadians(-90)); midwayPose2 = new Pose2d(52, 62, Math.toRadians(-90)); waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board @@ -272,12 +315,20 @@ public void runAutonoumousMode() { Actions.runBlocking( drive.actionBuilder(drive.pose) .strafeToLinearHeading(moveBeyondTrussPose.position, moveBeyondTrussPose.heading) + .strafeToLinearHeading(dropPurplePixelPosePush.position, dropPurplePixelPosePush.heading) .strafeToLinearHeading(dropPurplePixelPose.position, dropPurplePixelPose.heading) + //.strafeToLinearHeading(dropPurplePixelPosePos.position, dropPurplePixelPosePos.heading) .build()); //TODO : Code to drop Purple Pixel on Spike Mark safeWaitSeconds(1); - + frontIntake.setPower(.5); + //rearIntake.setPower(-1); + sleep(37); + frontIntake.setPower(0); + //rearIntake.setPower(0); + //drive pos \/ + shoulder.setPosition(0.48); //Move robot to midwayPose1 Actions.runBlocking( drive.actionBuilder(drive.pose) @@ -382,7 +433,7 @@ class teamElementPipeline extends OpenCvPipeline{ public Mat processFrame(Mat input){ Imgproc.cvtColor(input,YCbCr,Imgproc.COLOR_RGB2YCrCb); telemetry.addLine("pipeline running"); - /* + /* OG Config Rect leftRect = new Rect(1,1,212, 479); Rect centerRect = new Rect(213,1,212, 479); Rect rightRect = new Rect(426,1,213, 479); @@ -390,6 +441,16 @@ public Mat processFrame(Mat input){ Rect leftRect = new Rect(1,300,200, 400); Rect centerRect = new Rect(400,300,500, 200); Rect rightRect = new Rect(1000,300,260, 400); + + // testing tighter frames + /* + Rect leftRect = new Rect(1,350,125, 200); + Rect centerRect = new Rect(500,300,200, 200); + Rect rightRect = new Rect(1000,350,150, 200); + */ + + + input.copyTo(outPut); @@ -401,9 +462,19 @@ public Mat processFrame(Mat input){ centerCrop = YCbCr.submat(centerRect); rightCrop = YCbCr.submat(rightRect); - Core.extractChannel(leftCrop, leftCrop, 0); - Core.extractChannel(centerCrop, centerCrop, 0); - Core.extractChannel(rightCrop, rightCrop, 0); // blue is 0, green is 1, red is 2 + if (startPosition == START_POSITION.BLUE_LEFT || startPosition == START_POSITION.BLUE_RIGHT) { + Core.extractChannel(leftCrop, leftCrop, 0); + Core.extractChannel(centerCrop, centerCrop, 0); + Core.extractChannel(rightCrop, rightCrop, 0); // blue is 0, green is 1, red is 2 + } else if (startPosition == START_POSITION.RED_LEFT || startPosition == START_POSITION.RED_RIGHT) { + Core.extractChannel(leftCrop, leftCrop, 2); + Core.extractChannel(centerCrop, centerCrop, 2); + Core.extractChannel(rightCrop, rightCrop, 2); // blue is 0, green is 1, red is 2 + } else { + Core.extractChannel(leftCrop, leftCrop, 2); + Core.extractChannel(centerCrop, centerCrop, 2); + Core.extractChannel(rightCrop, rightCrop, 2); // blue is 0, green is 1, red is 2 + } Scalar leftavg = Core.mean(leftCrop); Scalar centeravg = Core.mean(centerCrop); @@ -417,17 +488,17 @@ public Mat processFrame(Mat input){ centeravgfinoutput = centeravgfin; rightavgfinoutput = rightavgfin; - if (leftavgfin > centeravgfin && leftavgfin > rightavgfin) { - telemetry.addLine("Left"); + if (leftavgfin < centeravgfin && leftavgfin < rightavgfin) { + //telemetry.addLine("Left"); identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; - } else if (centeravgfin > leftavgfin && centeravgfin > rightavgfin) { - telemetry.addLine("Center"); + } else if (centeravgfin < leftavgfin && centeravgfin < rightavgfin) { + //telemetry.addLine("Center"); identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.MIDDLE; - } else if (rightavgfin > leftavgfin && rightavgfin > centeravgfin) { - telemetry.addLine("Right"); + } else if (rightavgfin < leftavgfin && rightavgfin < centeravgfin) { + //telemetry.addLine("Right"); identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.RIGHT; } else { - telemetry.addLine("Failed to detect position"); + //telemetry.addLine("Failed to detect position"); identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.RIGHT; } return (outPut); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FTCWiresAutonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FTCWiresAutonomous.java index caeaea8579..4877805f47 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FTCWiresAutonomous.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FTCWiresAutonomous.java @@ -48,7 +48,7 @@ /** * FTC WIRES Autonomous Example for only vision detection using tensorflow and park */ -@Autonomous(name = "FTC Wires Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "FTC Wires TeleOp") +//@Autonomous(name = "FTC Wires Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "FTC Wires TeleOp") public class FTCWiresAutonomous extends LinearOpMode { public static String TEAM_NAME = "EDIT TEAM NAME"; //TODO: Enter team Name diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FTCWiresTeleOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FTCWiresTeleOpMode.java index c4f05d5acc..288cdcbed5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FTCWiresTeleOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FTCWiresTeleOpMode.java @@ -13,7 +13,7 @@ * */ -@TeleOp(name = "FTC Wires TeleOp", group = "00-Teleop") +//@TeleOp(name = "FTC Wires TeleOp", group = "00-Teleop") public class FTCWiresTeleOpMode extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java index 2cf3101819..66c975b533 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -8,7 +8,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; -@TeleOp(name="TeleMec") +//@TeleOp(name="TeleMec") public class OdoMec extends LinearOpMode { private ElapsedTime runtime = new ElapsedTime(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java index 7ac4daa0cc..818c509799 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheJettCode.java @@ -6,7 +6,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; -@TeleOp +//@TeleOp public class TheJettCode extends LinearOpMode { private Servo LiftLeft; double LiftHeight; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/apriltagDimitri.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/apriltagDimitri.java index b9fe01ac11..50881266b7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/apriltagDimitri.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/apriltagDimitri.java @@ -3,7 +3,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -@TeleOp +//@TeleOp public class apriltagDimitri extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { From f9cbcf8b03615d90cdaaf11ded1a6219908c7da3 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Fri, 17 Nov 2023 14:36:51 -0700 Subject: [PATCH 090/154] auto final / teleop tweak --- .../ftc/teamcode/Beginnings.java | 27 ++-- .../ftc/teamcode/EnigmaAuto.java | 120 +++++++++++++----- 2 files changed, 105 insertions(+), 42 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 5b14bf1a9c..55cb6e55bb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -158,31 +158,31 @@ private void doArmm() { shoulder.setPosition(0.60); hopper.setPosition(0.02); wrist.setPosition(0.22); - setLiftHeight(0.42); + //setLiftHeight(0.42); break; case UPGO2: wrist.setPosition(0.22); shoulder.setPosition(0.79); hopper.setPosition(0.02); - setLiftHeight(0.42); + //setLiftHeight(0.42); break; case UPGO3: hopper.setPosition(0.58); wrist.setPosition(0.6); shoulder.setPosition(0.79); - setLiftHeight(0.42); + //setLiftHeight(0.42); break; case UPGO4: shoulder.setPosition(0.91); hopper.setPosition(0.58); wrist.setPosition(0.6); - setLiftHeight(0.42); + //setLiftHeight(0.42); break; case UPGO5: wrist.setPosition(0.6); shoulder.setPosition(1); hopper.setPosition(0.58); - setLiftHeight(0.42); + //setLiftHeight(0.42); break; } } @@ -268,11 +268,13 @@ private void driveAroundPos() { } } private void drivePos() { - shoulder.setPosition(0.49); - wrist.setPosition(0.55); - hopper.setPosition(0.01); - setLiftHeight(0.42); - currentArmPos = ArmPosition.DRIVE; + if ((gamepad1.dpad_up) && (currentArmPos == ArmPosition.INTAKE)) { + shoulder.setPosition(0.49); + wrist.setPosition(0.55); + hopper.setPosition(0.01); + //setLiftHeight(0.42); + currentArmPos = ArmPosition.DRIVE; + } } private void intakePos() { //hopper.setPosition(0.02); @@ -281,7 +283,7 @@ private void intakePos() { wrist.setPosition(0.265); shoulder.setPosition(0.455); hopper.setPosition(0.02); - setLiftHeight(0.42); + //setLiftHeight(0.42); currentArmPos = ArmPosition.INTAKE; } private void theJuke() { @@ -488,12 +490,13 @@ public void runOpMode() throws InterruptedException { driveAroundPos(); //dumpPrepTwo(); //homePrep(); + drivePos(); dumpLeft(); theJuke(); //liftFunction(); //worm(); //stopMotion(); - aroundthetop(); + //aroundthetop(); armmUp(); armmDown(); SlideFunction(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 0fa755c967..2500b12bc7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -221,16 +221,19 @@ public void runAutonoumousMode() { drive = new MecanumDrive(hardwareMap, initPose); switch(identifiedSpikeMarkLocation){ case LEFT: - dropPurplePixelPose = new Pose2d(26, 8, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(23, 36, Math.toRadians(-90)); + dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(23, 11, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(27, 36, Math.toRadians(-90)); break; case MIDDLE: - dropPurplePixelPose = new Pose2d(30, 3, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(30, 36, Math.toRadians(-90)); + dropPurplePixelPosePush = new Pose2d(30.5, 0, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(26.5, -3, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(27, 36, Math.toRadians(-90)); break; case RIGHT: - dropPurplePixelPose = new Pose2d(30, -9, Math.toRadians(-45)); - dropYellowPixelPose = new Pose2d(37, 36, Math.toRadians(-90)); + dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); + dropPurplePixelPose = new Pose2d(23.5, -7, Math.toRadians(-35)); + dropYellowPixelPose = new Pose2d(27, 36, Math.toRadians(-90)); break; } midwayPose1 = new Pose2d(14, 13, Math.toRadians(-45)); @@ -242,16 +245,19 @@ public void runAutonoumousMode() { drive = new MecanumDrive(hardwareMap, initPose); switch(identifiedSpikeMarkLocation){ case LEFT: - dropPurplePixelPose = new Pose2d(30, 9, Math.toRadians(45)); - dropYellowPixelPose = new Pose2d(21, -36, Math.toRadians(90)); + dropPurplePixelPosePush = new Pose2d(27, 9, Math.toRadians(45)); + dropPurplePixelPose = new Pose2d(23.5, 7, Math.toRadians(35)); + dropYellowPixelPose = new Pose2d(27, -36, Math.toRadians(90)); break; case MIDDLE: - dropPurplePixelPose = new Pose2d(30, -3, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(29, -36, Math.toRadians(90)); + dropPurplePixelPosePush = new Pose2d(30.5, 0, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(26.5, -3, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(27, -36, Math.toRadians(90)); break; case RIGHT: - dropPurplePixelPose = new Pose2d(26, -8, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(37, -36, Math.toRadians(90)); + dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(23, -11, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(27, -36, Math.toRadians(90)); break; } midwayPose1 = new Pose2d(14, -13, Math.toRadians(45)); @@ -270,12 +276,12 @@ public void runAutonoumousMode() { case MIDDLE: dropPurplePixelPosePush = new Pose2d(30.5, 0, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(26.5, -3, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(34, 86, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(27, 86, Math.toRadians(-90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -8, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(23, -11, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(43, 86, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(27, 86, Math.toRadians(-90)); break; } midwayPose1 = new Pose2d(8, -8, Math.toRadians(0)); @@ -290,20 +296,23 @@ public void runAutonoumousMode() { drive = new MecanumDrive(hardwareMap, initPose); switch(identifiedSpikeMarkLocation){ case LEFT: - dropPurplePixelPose = new Pose2d(26, 8, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(37, -86, Math.toRadians(90)); + dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(23, 11, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(27, -86, Math.toRadians(90)); break; case MIDDLE: - dropPurplePixelPose = new Pose2d(30, -3, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(29, -86, Math.toRadians(90)); + dropPurplePixelPosePush = new Pose2d(30.5, 0, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(26.5, -3, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(27, -86, Math.toRadians(90)); break; case RIGHT: - dropPurplePixelPose = new Pose2d(27, -9, Math.toRadians(-45)); - dropYellowPixelPose = new Pose2d(21, -86, Math.toRadians(90)); + dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); + dropPurplePixelPose = new Pose2d(23.5, -7, Math.toRadians(-35)); + dropYellowPixelPose = new Pose2d(27, -86, Math.toRadians(90)); break; } midwayPose1 = new Pose2d(8, 8, Math.toRadians(0)); - midwayPose1a = new Pose2d(18, 18, Math.toRadians(90)); + midwayPose1a = new Pose2d(18, 21, Math.toRadians(90)); intakeStack = new Pose2d(52, 19,Math.toRadians(90)); midwayPose2 = new Pose2d(52, -62, Math.toRadians(90)); waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board @@ -366,6 +375,57 @@ public void runAutonoumousMode() { //TODO : Code to drop Pixel on Backdrop safeWaitSeconds(1); + shoulder.setPosition(0.60); + hopper.setPosition(0.02); + wrist.setPosition(0.22); + setLiftHeight(0.42); + sleep(500); + wrist.setPosition(0.22); + shoulder.setPosition(0.79); + hopper.setPosition(0.02); + setLiftHeight(0.42); + sleep(500); + hopper.setPosition(0.58); + wrist.setPosition(0.6); + shoulder.setPosition(0.79); + setLiftHeight(0.42); + sleep(500); + shoulder.setPosition(0.91); + hopper.setPosition(0.58); + wrist.setPosition(0.6); + setLiftHeight(0.42); + sleep(500); + wrist.setPosition(0.6); + shoulder.setPosition(1); + hopper.setPosition(0.58); + setLiftHeight(0.42); + sleep(500); + //dump + hopper.setPosition(0.2); + sleep(1000); + wrist.setPosition(0.6); + shoulder.setPosition(1); + hopper.setPosition(0.58); + setLiftHeight(0.42); + sleep(200); + shoulder.setPosition(0.91); + hopper.setPosition(0.58); + wrist.setPosition(0.6); + setLiftHeight(0.42); + sleep(200); + hopper.setPosition(0.58); + wrist.setPosition(0.6); + shoulder.setPosition(0.79); + sleep(350); + wrist.setPosition(0.22); + shoulder.setPosition(0.79); + hopper.setPosition(0.02); + setLiftHeight(0.42); + sleep(500); + shoulder.setPosition(0.60); + hopper.setPosition(0.02); + wrist.setPosition(0.22); + setLiftHeight(0.42); //Move robot to park in Backstage Actions.runBlocking( @@ -437,17 +497,17 @@ public Mat processFrame(Mat input){ Rect leftRect = new Rect(1,1,212, 479); Rect centerRect = new Rect(213,1,212, 479); Rect rightRect = new Rect(426,1,213, 479); - */ + Rect leftRect = new Rect(1,300,200, 400); Rect centerRect = new Rect(400,300,500, 200); Rect rightRect = new Rect(1000,300,260, 400); - +*/ // testing tighter frames - /* - Rect leftRect = new Rect(1,350,125, 200); + + Rect leftRect = new Rect(1,380,125, 200); Rect centerRect = new Rect(500,300,200, 200); - Rect rightRect = new Rect(1000,350,150, 200); - */ + Rect rightRect = new Rect(1100,380,150, 200); + @@ -471,9 +531,9 @@ public Mat processFrame(Mat input){ Core.extractChannel(centerCrop, centerCrop, 2); Core.extractChannel(rightCrop, rightCrop, 2); // blue is 0, green is 1, red is 2 } else { - Core.extractChannel(leftCrop, leftCrop, 2); - Core.extractChannel(centerCrop, centerCrop, 2); - Core.extractChannel(rightCrop, rightCrop, 2); // blue is 0, green is 1, red is 2 + Core.extractChannel(leftCrop, leftCrop, 0); + Core.extractChannel(centerCrop, centerCrop, 0); + Core.extractChannel(rightCrop, rightCrop, 0); // blue is 0, green is 1, red is 2 } Scalar leftavg = Core.mean(leftCrop); From 80a7882a41c26df3116f07cedab134ea04967c6b Mon Sep 17 00:00:00 2001 From: digitalbreadllc Date: Sun, 19 Nov 2023 12:46:59 -0700 Subject: [PATCH 091/154] updates @ Alamogordo --- .../org/firstinspires/ftc/teamcode/Beginnings.java | 13 ++++++++----- build.gradle | 2 +- gradle.properties | 3 +++ gradle/wrapper/gradle-wrapper.properties | 2 +- 4 files changed, 13 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 55cb6e55bb..1580067d56 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -155,9 +155,9 @@ private void doArmm() { intakePos(); break; case UPGO1: - shoulder.setPosition(0.60); + shoulder.setPosition(0.55); hopper.setPosition(0.02); - wrist.setPosition(0.22); + wrist.setPosition(0.28); //setLiftHeight(0.42); break; case UPGO2: @@ -258,9 +258,12 @@ private void SlideFunction() { } private void dumpLeft() { - if (gamepad2.right_bumper) { + if (gamepad2.left_bumper) { hopper.setPosition(0.2); } + if (gamepad2.right_bumper) { + hopper.setPosition(1); + } } private void driveAroundPos() { if (gamepad2.dpad_down) { @@ -268,7 +271,7 @@ private void driveAroundPos() { } } private void drivePos() { - if ((gamepad1.dpad_up) && (currentArmPos == ArmPosition.INTAKE)) { + if ((gamepad1.a) && (currentArmPos == ArmPosition.INTAKE)) { shoulder.setPosition(0.49); wrist.setPosition(0.55); hopper.setPosition(0.01); @@ -287,7 +290,7 @@ private void intakePos() { currentArmPos = ArmPosition.INTAKE; } private void theJuke() { - if ((gamepad2.dpad_up) && (currentArmPos == ArmPosition.INTAKE)) { + if ((gamepad2.dpad_up || gamepad1.x) && (currentArmPos == ArmPosition.INTAKE)) { shoulder.setPosition(0.48); sleep(200); intakePos(); diff --git a/build.gradle b/build.gradle index b26ca8e63a..f3501e0161 100644 --- a/build.gradle +++ b/build.gradle @@ -11,7 +11,7 @@ buildscript { } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:7.4.2' + classpath 'com.android.tools.build:gradle:8.1.4' } } diff --git a/gradle.properties b/gradle.properties index 104e43cae8..8c6e92408d 100644 --- a/gradle.properties +++ b/gradle.properties @@ -8,3 +8,6 @@ android.enableJetifier=true # Allow Gradle to use up to 1 GB of RAM org.gradle.jvmargs=-Xmx1024M +android.defaults.buildfeatures.buildconfig=true +android.nonTransitiveRClass=false +android.nonFinalResIds=false diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 8049c684f0..da1db5f04e 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.5-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.0-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists From c63d0e6409d36687fd5f9b3dd7935e7f2f24b81f Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 28 Nov 2023 18:36:59 -0700 Subject: [PATCH 092/154] McMuffin sounds good right about now 10% chance of not blowing up --- .../firstinspires/ftc/teamcode/McMuffin.java | 188 ++++++++++++++++++ 1 file changed, 188 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java new file mode 100644 index 0000000000..0f12d1b060 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java @@ -0,0 +1,188 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; + +@TeleOp +public class McMuffin extends LinearOpMode { + //TODO: Step 1, Replace all "wrist","hopper", etc with your servos + Servo wrist; + Servo elbow; + Servo leftFinger; + Servo rightFinger; + Servo shoulder; + Servo leftLift; + Servo rightLift; + + //DcMotor frontIntake; + //DcMotor rearIntake; + + double speedAmount; + //TODO: Add offset if needed w/ eg double lift + double LiftLeftOffset = -.05; + + //TODO: Step 3, set all of your servo names below + enum ServoTypes{ + SHOULDER, + ELBOW, + LEFT_FINGER, + RIGHT_FINGER, + WRIST, + LIFT; + } + ServoTypes which; + //TODO: Step 4, replace all names of Servos with yours, and replace all capitals with what you set them to from step 3 + + private void masterTuner() { + if (gamepad1.left_bumper) { + if (which == ServoTypes.SHOULDER) { + shoulder.setPosition(shoulder.getPosition() - speedAmount); + } + else if (which == ServoTypes.ELBOW) { + elbow.setPosition(elbow.getPosition() - speedAmount); + } + else if (which == ServoTypes.LEFT_FINGER) { + leftFinger.setPosition(leftFinger.getPosition() - speedAmount); + } + else if (which == ServoTypes.RIGHT_FINGER) { + rightFinger.setPosition(rightFinger.getPosition() - speedAmount); + } + else if (which == ServoTypes.WRIST) { + wrist.setPosition(wrist.getPosition() - speedAmount); + } + else if (which == ServoTypes.LIFT) { + leftLift.setPosition((rightLift.getPosition() + LiftLeftOffset) - speedAmount); + rightLift.setPosition(rightLift.getPosition() - speedAmount); + } + } + else if (gamepad1.right_bumper) { + if (which == ServoTypes.SHOULDER) { + shoulder.setPosition(shoulder.getPosition() + speedAmount); + } + else if (which == ServoTypes.ELBOW) { + elbow.setPosition(elbow.getPosition() + speedAmount); + } + else if (which == ServoTypes.LEFT_FINGER) { + leftFinger.setPosition(leftFinger.getPosition() + speedAmount); + } + else if (which == ServoTypes.RIGHT_FINGER) { + rightFinger.setPosition(rightFinger.getPosition() + speedAmount); + } + else if (which == ServoTypes.WRIST) { + wrist.setPosition(wrist.getPosition() + speedAmount); + } + else if (which == ServoTypes.LIFT) { + leftLift.setPosition((rightLift.getPosition() + LiftLeftOffset) + speedAmount); + rightLift.setPosition(rightLift.getPosition() + speedAmount); + } + } + } + //TODO: Step 5, replace all of your Servo functions below + private void setServo() { + if (gamepad1.y) { + which = ServoTypes.SHOULDER; + } + else if (gamepad1.x) { + which = ServoTypes.ELBOW; + } + else if (gamepad1.b) { + which = ServoTypes.WRIST; + } + else if (gamepad1.a) { + which = ServoTypes.LIFT; + } + telemetry.addData("Selected Servo = ", which.toString()); + } + private void setExtra() { + if (gamepad1.dpad_left) { + which = ServoTypes.LEFT_FINGER; + } + else if (gamepad1.dpad_right) { + which = ServoTypes.RIGHT_FINGER; + } + /* + else if (gamepad1.dpad_right) { + speedAmount = 0.01; + } + else if (gamepad1.dpad_up) { + speedAmount = 0.03; + } + telemetry.addData("Speed = ", speedAmount); + */ + } + + //TODO: Step 7, your done! This was written by Goober on 11/5/23 slouching in a chair at 10:35 in the morning. + // And as I'm writing this I wonder if anybody will actually use this. Problably not, + // but idk what to do while I wait for the robot to be ready for calibration. It's now 10:36 am. + // I wonder if future me will use this and or remember doing this. Goober out. Edit: I have to recode :( 11/28/23 + + //TODO: Step 6 replace all of the xyz.getPosition()0; with your servos and replace "xyz" with what that servo is + private void whatServoAt() { + telemetry.addData("Shoulder = ", shoulder.getPosition()); + telemetry.addData("Elbow = ", elbow.getPosition()); + telemetry.addData("Left Finger = ", leftFinger.getPosition()); + telemetry.addData("Right Finger = ", rightFinger.getPosition()); + telemetry.addData("Wrist = ", wrist.getPosition()); + telemetry.addData("Lift Left = ",leftLift.getPosition()); + telemetry.addData("Lift Right = ",rightLift.getPosition()); + } + + @Override + public void runOpMode() throws InterruptedException { + which = ServoTypes.SHOULDER; + speedAmount = 0.01; + //this is a coment to mAKE git update + + //TODO: Step 2, Replace the device names with your 4 (or more if you use two servos for one task) + wrist = hardwareMap.get(Servo.class, "wrist"); + elbow = hardwareMap.get(Servo.class, "elbow"); + leftFinger = hardwareMap.get(Servo.class, "lFinger"); + rightFinger = hardwareMap.get(Servo.class, "rFinger"); + shoulder = hardwareMap.get(Servo.class, "shoulder"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + + + wrist.setPosition(0.265); + shoulder.setPosition(0.455); + elbow.setPosition(0.5); + rightFinger.setPosition(0.5); + leftFinger.setPosition(0.5); + leftLift.setPosition(0.42 + LiftLeftOffset); + rightLift.setPosition(0.42); + + + // does intake exist????????????? + /*frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); + rearIntake = hardwareMap.get(DcMotor.class, "rearIntake"); + + frontIntake.setDirection(DcMotorSimple.Direction.FORWARD); + rearIntake.setDirection(DcMotorSimple.Direction.REVERSE); + frontIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rearIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + */ + + + telemetry.update(); + waitForStart(); + + while(opModeIsActive()) { + setServo(); + masterTuner(); + setExtra(); + whatServoAt(); + telemetry.addData("Selected", which.toString()); + telemetry.addLine("Y = Shoulder - X = Hopper - B = Wrist A = Lift"); + telemetry.update(); + sleep(100); + } + } +} From fe5e2995d3ca5714591c5e45f98809ca77f48cc2 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 28 Nov 2023 19:09:25 -0700 Subject: [PATCH 093/154] The time has come, where we will advance, no longer are we at the beggining, we have E V O L V E D. We will advance, moving forward. Evolution will bring more possibilities. Goodbye Begginings. o7 - Goober --- .../org/firstinspires/ftc/teamcode/Evo.java | 483 ++++++++++++++++++ .../firstinspires/ftc/teamcode/McMuffin.java | 13 +- 2 files changed, 485 insertions(+), 11 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evo.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evo.java new file mode 100644 index 0000000000..3fd4d3bc98 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evo.java @@ -0,0 +1,483 @@ +package org.firstinspires.ftc.teamcode; + +// imports + +//import com.acmerobotics.roadrunner.Pose2d; +//import com.acmerobotics.roadrunner.PoseVelocity2d; +//import com.acmerobotics.roadrunner.Vector2d; + +import com.qualcomm.hardware.rev.RevTouchSensor; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; + +@TeleOp +public class Evo extends LinearOpMode { + // Declare vars + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; + private Servo launcher; + private Servo rightLift; + private Servo leftLift; + private Servo shoulder; + private Servo wrist; + private Servo elbow; + private Servo leftFinger; + private Servo rightFinger; + + //MecanumDrive drive; + //OgDrive ogDrive; + private ElapsedTime runtime = new ElapsedTime(); + //Motors + //Drivetrain + private DcMotor rightFront; //front right 0 + private DcMotor leftFront; //front left 2 + private DcMotor rightBack; //rear right 1 + private DcMotor leftBack; //rear left 3 + private DcMotor leftHang; + private DcMotor rightHang; + + + // Servo prep + private static enum ArmPosition { + INTAKE, + DRIVE, + UPGO1, + UPGO2, + UPGO3, + UPGO4, + UPGO5; + } + + private ArmPosition currentArmPos; + double LiftLeftOffset = .04; + double LiftHeight; + private AndroidTextToSpeech androidTextToSpeech; + + + // Functions \/ + + + private void servo_shenanigans() { + setLiftHeight(0.4); + sleep(10000); + setLiftHeight(0.05); + } + + private void setLiftHeight(double inputLiftHeight) { + if (inputLiftHeight < 0.42) { + inputLiftHeight = 0.42; + } + if (inputLiftHeight > 1) { + inputLiftHeight = 1; + } + LiftHeight = inputLiftHeight; + leftLift.setPosition(LiftLeftOffset + LiftHeight); + rightLift.setPosition(LiftHeight); + + } + + private void worm() { + if (gamepad2.left_bumper) { + shoulder.setPosition(0.445); + wrist.setPosition(0.26); + } + if (gamepad2.right_bumper) { + shoulder.setPosition(0.92); + } + } + + private void airplane() { + if (gamepad2.back) { + launcher.setPosition(0.1); + } else { + launcher.setPosition(0.8); + } + } + + private void liftFunction() { + if (gamepad2.y) { + shoulder.setPosition(0.79); + setLiftHeight(0.42); + + wrist.setPosition(0.2); + sleep(600); + shoulder.setPosition(0.60); + sleep(600); + shoulder.setPosition(0.50); + sleep(600); + intakePos(); + } else if (gamepad2.x) { + setLiftHeight(0.71); + } else if (gamepad2.b) { + setLiftHeight(0.56); + } + } + private void armmDown() { + if (gamepad2.y) { + switch (currentArmPos) { + case UPGO1: + currentArmPos = ArmPosition.INTAKE; + break; + case UPGO2: + currentArmPos = ArmPosition.UPGO1; + break; + case UPGO3: + currentArmPos = ArmPosition.UPGO2; + break; + case UPGO4: + currentArmPos = ArmPosition.UPGO3; + break; + case UPGO5: + currentArmPos = ArmPosition.UPGO4; + break; + } + doArmm(); + telemetry.addLine(); + telemetry.addData("armpostion", currentArmPos.toString()); + sleep(100); + } + } + private void doArmm() { + switch (currentArmPos) { + case INTAKE: + intakePos(); + break; + case UPGO1: + shoulder.setPosition(0.55); + + wrist.setPosition(0.28); + //setLiftHeight(0.42); + break; + case UPGO2: + wrist.setPosition(0.22); + shoulder.setPosition(0.79); + + //setLiftHeight(0.42); + break; + case UPGO3: + + wrist.setPosition(0.6); + shoulder.setPosition(0.79); + //setLiftHeight(0.42); + break; + case UPGO4: + shoulder.setPosition(0.91); + + wrist.setPosition(0.6); + //setLiftHeight(0.42); + break; + case UPGO5: + wrist.setPosition(0.6); + shoulder.setPosition(1); + + //setLiftHeight(0.42); + break; + } + } + private void armmUp(){ + if (gamepad2.a) { + switch (currentArmPos) { + case INTAKE: + currentArmPos = ArmPosition.UPGO1; + break; + case UPGO1: + currentArmPos = ArmPosition.UPGO2; + break; + case UPGO2: + currentArmPos = ArmPosition.UPGO3; + break; + case UPGO3: + currentArmPos = ArmPosition.UPGO4; + break; + case UPGO4: + currentArmPos = ArmPosition.UPGO5; + break; + } + doArmm(); + telemetry.addLine(); + telemetry.addData("armpostion", currentArmPos.toString()); + sleep(100); + } + } + private void aroundthetop() { + if (gamepad2.start) { + shoulder.setPosition(0.56); + wrist.setPosition(0.5); + + } + } + private void dumpPrep() { + if (gamepad2.back) { + shoulder.setPosition(0.75); + sleep(400); + + wrist.setPosition(0.93); + shoulder.setPosition(0.91); + } + } + private void dumpPrepTwo() { + if (gamepad2.back) { + + } + } + private void SlideFunction() { + + } + + private void dumpLeft() { + if (gamepad2.left_bumper) { + + } + if (gamepad2.right_bumper) { + + } + } + private void driveAroundPos() { + if (gamepad2.dpad_down) { + intakePos(); + } + } + private void drivePos() { + if ((gamepad1.a) && (currentArmPos == ArmPosition.INTAKE)) { + shoulder.setPosition(0.49); + wrist.setPosition(0.55); + + //setLiftHeight(0.42); + currentArmPos = ArmPosition.DRIVE; + } + } + private void intakePos() { + //hopper.setPosition(0.02); + //shoulder.setPosition(0.44); + //wrist.setPosition(0.26); + wrist.setPosition(0.265); + shoulder.setPosition(0.455); + leftFinger.setPosition(0.5); + rightFinger.setPosition(0.5); + elbow.setPosition(0.5); + + //setLiftHeight(0.42); + currentArmPos = ArmPosition.INTAKE; + } + private void theJuke() { + if ((gamepad2.dpad_up || gamepad1.x) && (currentArmPos == ArmPosition.INTAKE)) { + shoulder.setPosition(0.48); + sleep(200); + intakePos(); + } + } + private void incrementalIntake() { + shoulder.setPosition(0.455); + sleep(653); + shoulder.setPosition(0.47); + wrist.setPosition(0.55); + sleep(531); + shoulder.setPosition(0.49); + wrist.setPosition(0.55); + } + private void launcherstartPos() { + launcher.setPosition(0.8); + } + + + +/* + private void driveCode() { + + double SLOW_DOWN_FACTOR; + SLOW_DOWN_FACTOR = 0.5; + telemetry.addData("Running FTC Wires (ftcwires.org) TeleOp Mode adopted for Team:","TEAM NUMBER"); + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y * SLOW_DOWN_FACTOR, + -gamepad1.left_stick_x * SLOW_DOWN_FACTOR + ), + -gamepad1.right_stick_x * SLOW_DOWN_FACTOR + )); + + drive.updatePoseEstimate(); + + //telemetry.addData("LF Encoder", drive.leftFront.getCurrentPosition()); + //telemetry.addData("LB Encoder", drive.leftBack.getCurrentPosition()); + //telemetry.addData("RF Encoder", drive.rightFront.getCurrentPosition()); + //telemetry.addData("RB Encoder", drive.rightBack.getCurrentPosition()); + + telemetry.addData("x", drive.pose.position.x); + telemetry.addLine("Current Pose"); + telemetry.addData("y", drive.pose.position.y); + telemetry.addData("heading", Math.toDegrees(drive.pose.heading.log())); + // telemetry.update(); + + } +*/ + + + @Override + // NOT loop \/ - Or int of vars + public void runOpMode() throws InterruptedException { + + androidTextToSpeech = new AndroidTextToSpeech(); + androidTextToSpeech.initialize(); + + // for wires driving + //drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + // for Og driving (the best driving) + //ogDrive = new OgDrive(hardwareMap); + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + rightFront = hardwareMap.get(DcMotor.class, "rightFront"); + leftFront = hardwareMap.get(DcMotor.class, "leftFront"); + rightBack = hardwareMap.get(DcMotor.class, "rightBack"); + leftBack = hardwareMap.get(DcMotor.class, "leftBack"); + + leftHang = hardwareMap.get(DcMotor.class, "leftHang"); + rightHang = hardwareMap.get(DcMotor.class, "rightHang"); + + rightFront.setDirection(DcMotor.Direction.REVERSE); + leftFront.setDirection(DcMotor.Direction.FORWARD); + rightBack.setDirection(DcMotor.Direction.REVERSE); + leftBack.setDirection(DcMotor.Direction.FORWARD); + + //Set motor modes + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + + telemetry.addData("Status", "OdoMec2 is ready to run!"); + telemetry.update(); + + + + launcher = hardwareMap.get(Servo.class, "launcher"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + wrist = hardwareMap.get(Servo.class, "wrist"); + elbow = hardwareMap.get(Servo.class, "elbow"); + leftFinger = hardwareMap.get(Servo.class, "lFinger"); + rightFinger = hardwareMap.get(Servo.class, "rFinger"); + + shoulder = hardwareMap.get(Servo.class, "shoulder"); + + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + launcher.setDirection(Servo.Direction.REVERSE); + + + + // sensors + leftUpper = hardwareMap.get(RevTouchSensor.class, "leftUpper"); + rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); + leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); + rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); + + // servos + launcherstartPos(); + intakePos(); + + //this is a coment to mAKE git update + double SLOW_DOWN_FACTOR = 0.5; + telemetry.addData("Initializing TeleOp",""); + telemetry.update(); + + + waitForStart(); + runtime.reset(); + // servo_shenanigans(); + // loop real + while(opModeIsActive()){ + //Drivetrain + double forward = gamepad1.left_stick_y; + double strafe = -gamepad1.left_stick_x; + double turn = -gamepad1.right_stick_x; + + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); + + double rightFrontPower = (forward - strafe - turn) / denominator; + double leftFrontPower = (forward + strafe + turn) / denominator; + double rightBackPower = (forward + strafe - turn) / denominator; + double leftBackPower = (forward - strafe + turn) / denominator; + + if (gamepad1.left_bumper) { + rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); + leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); + rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); + leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); + } else { + rightFrontPower = Range.clip(rightFrontPower, -0.8, 0.8); + leftFrontPower = Range.clip(leftFrontPower, -0.8, 0.8); + rightBackPower = Range.clip(rightBackPower, -0.8, 0.8); + leftBackPower = Range.clip(leftBackPower, -0.8, 0.8); + } + + + rightFront.setPower(rightFrontPower); + leftFront.setPower(leftFrontPower); + rightBack.setPower(rightBackPower); + leftBack.setPower(leftBackPower); + + if (gamepad1.right_bumper){ + leftHang.setDirection(DcMotor.Direction.FORWARD); + rightHang.setDirection(DcMotor.Direction.FORWARD); + + leftHang.setPower(.9); + rightHang.setPower(.9); + } else if (leftUpper.isPressed() || rightUpper.isPressed() || gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); + } + if (gamepad1.left_bumper){ + leftHang.setDirection(DcMotor.Direction.REVERSE); + rightHang.setDirection(DcMotor.Direction.REVERSE); + + leftHang.setPower(.9); + rightHang.setPower(.9); + } else if (leftLower.isPressed() || rightLower.isPressed() || gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); + } + + + + + telemetry.addData("Status", "Run " + runtime.toString()); + telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); + + //Code(); + airplane(); + //ogDrive.og_drive_code(gamepad1, telemetry); + //IsDrive.is_drive_code(gamepad1, telemetry); + driveAroundPos(); + //dumpPrepTwo(); + //homePrep(); + drivePos(); + dumpLeft(); + theJuke(); + //liftFunction(); + //worm(); + //stopMotion(); + //aroundthetop(); + armmUp(); + armmDown(); + SlideFunction(); + telemetry.update(); + sleep(100); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java index 0f12d1b060..ebb2460123 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java @@ -103,15 +103,6 @@ private void setExtra() { else if (gamepad1.dpad_right) { which = ServoTypes.RIGHT_FINGER; } - /* - else if (gamepad1.dpad_right) { - speedAmount = 0.01; - } - else if (gamepad1.dpad_up) { - speedAmount = 0.03; - } - telemetry.addData("Speed = ", speedAmount); - */ } //TODO: Step 7, your done! This was written by Goober on 11/5/23 slouching in a chair at 10:35 in the morning. @@ -179,8 +170,8 @@ public void runOpMode() throws InterruptedException { masterTuner(); setExtra(); whatServoAt(); - telemetry.addData("Selected", which.toString()); - telemetry.addLine("Y = Shoulder - X = Hopper - B = Wrist A = Lift"); + //telemetry.addData("Selected", which.toString()); + //telemetry.addLine("Y = Shoulder - X = Hopper - B = Wrist A = Lift"); telemetry.update(); sleep(100); } From 39481790e583ea771bb0f0e34a83cd9c495cbfc6 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 28 Nov 2023 19:13:08 -0700 Subject: [PATCH 094/154] Mutation* --- .../org/firstinspires/ftc/teamcode/{Evo.java => Mutation.java} | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Evo.java => Mutation.java} (99%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evo.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 3fd4d3bc98..70eeb8b878 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -10,7 +10,6 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; @@ -18,7 +17,7 @@ import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; @TeleOp -public class Evo extends LinearOpMode { +public class Mutation extends LinearOpMode { // Declare vars private RevTouchSensor rightUpper; private RevTouchSensor leftUpper; From e7bfacfc7a8e8385634a0015c9db7da9393770f6 Mon Sep 17 00:00:00 2001 From: digitalbreadllc Date: Fri, 1 Dec 2023 09:21:12 -0700 Subject: [PATCH 095/154] Luke, Gray and Coach Russ --- .../firstinspires/ftc/teamcode/DuckOps.java | 505 ++++++++++++++++++ .../firstinspires/ftc/teamcode/OdoMec.java | 4 +- 2 files changed, 508 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java new file mode 100644 index 0000000000..da35f4d26c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java @@ -0,0 +1,505 @@ +package org.firstinspires.ftc.teamcode; + +// imports + +//import com.acmerobotics.roadrunner.Pose2d; +//import com.acmerobotics.roadrunner.PoseVelocity2d; +//import com.acmerobotics.roadrunner.Vector2d; + +import com.qualcomm.hardware.rev.RevTouchSensor; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; + +@TeleOp +public class DuckOps extends LinearOpMode { + // Declare vars + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; + private Servo launcher; + private Servo rightLift; + private Servo leftLift; + private Servo shoulder; + private Servo wrist; + private Servo elbow; + private Servo leftFinger; + private Servo rightFinger; + + //Servo init values + private static final double SHOULDER_INT = 0.425; + private static final double ELBOW_INT = 0.5; + private static final double ELBOW_INTAKE = 0.8; + private static final double WRIST_INT = 0.55; + private static final double WRIST_INTAKE = 0.575; + private static final double WRIST_DRIVE = 0.265; + private static final double LEFT_FINGER_GRIP = 0.74; + private static final double LEFT_FINGER_DROP = 0.5; + private static final double LEFT_FINGER_INTAKE = 0.34; + private static final double RIGHT_FINGER_GRIP = .27; + private static final double RIGHT_FINGER_DROP = .5; + private static final double RIGHT_FINGER_INTAKE = 0.64; + + //MecanumDrive drive; + //OgDrive ogDrive; + private ElapsedTime runtime = new ElapsedTime(); + //Motors + //Drivetrain + private DcMotor rightFront; //front right 0 + private DcMotor leftFront; //front left 2 + private DcMotor rightBack; //rear right 1 + private DcMotor leftBack; //rear left 3 + private DcMotor leftHang; + private DcMotor rightHang; + + + // Servo prep + private static enum ArmPosition { + INTAKE, + DRIVE, + UPGO1, + UPGO2, + UPGO3, + UPGO4, + UPGO5; + } + + private ArmPosition currentArmPos; + double LiftLeftOffset = .04; + double LiftHeight; + private AndroidTextToSpeech androidTextToSpeech; + + + // Functions \/ + + + private void servo_shenanigans() { + setLiftHeight(0.4); + // sleep(10000); + setLiftHeight(0.05); + } + + private void setLiftHeight(double inputLiftHeight) { + if (inputLiftHeight < 0.42) { + inputLiftHeight = 0.42; + } + if (inputLiftHeight > 1) { + inputLiftHeight = 1; + } + LiftHeight = inputLiftHeight; + leftLift.setPosition(LiftLeftOffset + LiftHeight); + rightLift.setPosition(LiftHeight); + + } + + private void worm() { + if (gamepad2.left_bumper) { + shoulder.setPosition(0.445); + wrist.setPosition(0.26); + } + if (gamepad2.right_bumper) { + shoulder.setPosition(0.92); + } + } + + private void airplane() { + if (gamepad2.back) { + launcher.setPosition(0.1); + } else { + launcher.setPosition(0.8); + } + } + + private void liftFunction() { + if (gamepad2.y) { + shoulder.setPosition(0.79); + setLiftHeight(0.42); + + wrist.setPosition(0.2); + //sleep(600); + shoulder.setPosition(0.60); + // sleep(600); + shoulder.setPosition(0.50); + //sleep(600); + intakePos(); + } else if (gamepad2.x) { + setLiftHeight(0.71); + } else if (gamepad2.b) { + setLiftHeight(0.56); + } + } + private void armmDown() { + if (gamepad2.y) { + switch (currentArmPos) { + case UPGO1: + currentArmPos = ArmPosition.INTAKE; + break; + case UPGO2: + currentArmPos = ArmPosition.UPGO1; + break; + case UPGO3: + currentArmPos = ArmPosition.UPGO2; + break; + case UPGO4: + currentArmPos = ArmPosition.UPGO3; + break; + case UPGO5: + currentArmPos = ArmPosition.UPGO4; + break; + } + doArmm(); + telemetry.addLine(); + telemetry.addData("armpostion", currentArmPos.toString()); + //(100); + } + } + private void doArmm() { + switch (currentArmPos) { + case INTAKE: + intakePos(); + break; + case UPGO1: + shoulder.setPosition(0.55); + + wrist.setPosition(0.28); + //setLiftHeight(0.42); + break; + case UPGO2: + wrist.setPosition(0.22); + shoulder.setPosition(0.79); + + //setLiftHeight(0.42); + break; + case UPGO3: + + wrist.setPosition(0.6); + shoulder.setPosition(0.79); + //setLiftHeight(0.42); + break; + case UPGO4: + shoulder.setPosition(0.91); + + wrist.setPosition(0.6); + //setLiftHeight(0.42); + break; + case UPGO5: + wrist.setPosition(0.6); + shoulder.setPosition(1); + + //setLiftHeight(0.42); + break; + } + } + private void armmUp(){ + if (gamepad2.a) { + switch (currentArmPos) { + case INTAKE: + currentArmPos = ArmPosition.UPGO1; + break; + case UPGO1: + currentArmPos = ArmPosition.UPGO2; + break; + case UPGO2: + currentArmPos = ArmPosition.UPGO3; + break; + case UPGO3: + currentArmPos = ArmPosition.UPGO4; + break; + case UPGO4: + currentArmPos = ArmPosition.UPGO5; + break; + } + doArmm(); + telemetry.addLine(); + telemetry.addData("armpostion", currentArmPos.toString()); + //sleep(100); + } + } + private void aroundthetop() { + if (gamepad2.start) { + shoulder.setPosition(0.56); + wrist.setPosition(0.5); + + } + } + private void dumpPrep() { + if (gamepad2.back) { + shoulder.setPosition(0.75); + // sleep(400); + + wrist.setPosition(0.93); + shoulder.setPosition(0.91); + } + } + private void dumpPrepTwo() { + if (gamepad2.back) { + + } + } + private void SlideFunction() { + + } + + private void dumpLeft() { + if (gamepad2.left_bumper) { + + } + if (gamepad2.right_bumper) { + + } + } + private void driveAroundPos() { + if (gamepad2.dpad_down) { + intakePos(); + } + } + private void drivePos() { + if ((gamepad1.a) && (currentArmPos == ArmPosition.INTAKE)) { + shoulder.setPosition(0.49); + wrist.setPosition(0.55); + + //setLiftHeight(0.42); + currentArmPos = ArmPosition.DRIVE; + } + } + private void intakePos() { + //hopper.setPosition(0.02); + //shoulder.setPosition(0.44); + //wrist.setPosition(0.26); + wrist.setPosition(0.265); + shoulder.setPosition(0.455); + leftFinger.setPosition(0.5); + rightFinger.setPosition(0.5); + elbow.setPosition(0.5); + + //setLiftHeight(0.42); + currentArmPos = ArmPosition.INTAKE; + } + private void initPos() { + wrist.setPosition(WRIST_INT); + elbow.setPosition(ELBOW_INT); + shoulder.setPosition(SHOULDER_INT); + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } + + private void theJuke() { + if ((gamepad2.dpad_up || gamepad1.x) && (currentArmPos == ArmPosition.INTAKE)) { + shoulder.setPosition(0.48); + // sleep(200); + intakePos(); + } + } + private void incrementalIntake() { + shoulder.setPosition(0.455); + // sleep(653); + shoulder.setPosition(0.47); + wrist.setPosition(0.55); + // sleep(531); + shoulder.setPosition(0.49); + wrist.setPosition(0.55); + } + private void launcherstartPos() { + launcher.setPosition(0.8); + } + + + +/* + private void driveCode() { + + double SLOW_DOWN_FACTOR; + SLOW_DOWN_FACTOR = 0.5; + telemetry.addData("Running FTC Wires (ftcwires.org) TeleOp Mode adopted for Team:","TEAM NUMBER"); + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y * SLOW_DOWN_FACTOR, + -gamepad1.left_stick_x * SLOW_DOWN_FACTOR + ), + -gamepad1.right_stick_x * SLOW_DOWN_FACTOR + )); + + drive.updatePoseEstimate(); + + //telemetry.addData("LF Encoder", drive.leftFront.getCurrentPosition()); + //telemetry.addData("LB Encoder", drive.leftBack.getCurrentPosition()); + //telemetry.addData("RF Encoder", drive.rightFront.getCurrentPosition()); + //telemetry.addData("RB Encoder", drive.rightBack.getCurrentPosition()); + + telemetry.addData("x", drive.pose.position.x); + telemetry.addLine("Current Pose"); + telemetry.addData("y", drive.pose.position.y); + telemetry.addData("heading", Math.toDegrees(drive.pose.heading.log())); + // telemetry.update(); + + } +*/ + + + @Override + // NOT loop \/ - Or int of vars + public void runOpMode() throws InterruptedException { + + androidTextToSpeech = new AndroidTextToSpeech(); + androidTextToSpeech.initialize(); + + // for wires driving + //drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + // for Og driving (the best driving) + //ogDrive = new OgDrive(hardwareMap); + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + rightFront = hardwareMap.get(DcMotor.class, "rightFront"); + leftFront = hardwareMap.get(DcMotor.class, "leftFront"); + rightBack = hardwareMap.get(DcMotor.class, "rightBack"); + leftBack = hardwareMap.get(DcMotor.class, "leftBack"); + + leftHang = hardwareMap.get(DcMotor.class, "leftHang"); + rightHang = hardwareMap.get(DcMotor.class, "rightHang"); + + rightFront.setDirection(DcMotor.Direction.REVERSE); + leftFront.setDirection(DcMotor.Direction.FORWARD); + rightBack.setDirection(DcMotor.Direction.REVERSE); + leftBack.setDirection(DcMotor.Direction.FORWARD); + + //Set motor modes + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + + telemetry.addData("Status", "OdoMec2 is ready to run!"); + telemetry.update(); + + + + launcher = hardwareMap.get(Servo.class, "launcher"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + wrist = hardwareMap.get(Servo.class, "wrist"); + elbow = hardwareMap.get(Servo.class, "elbow"); + leftFinger = hardwareMap.get(Servo.class, "lFinger"); + rightFinger = hardwareMap.get(Servo.class, "rFinger"); + + shoulder = hardwareMap.get(Servo.class, "shoulder"); + + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + launcher.setDirection(Servo.Direction.REVERSE); + + + + // sensors + leftUpper = hardwareMap.get(RevTouchSensor.class, "leftUpper"); + rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); + leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); + rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); + + // servos + //launcherstartPos(); + //intakePos(); + + //this is a coment to mAKE git update + double SLOW_DOWN_FACTOR = 0.5; + telemetry.addData("Initializing TeleOp",""); + telemetry.update(); + + + waitForStart(); + runtime.reset(); + // servo_shenanigans(); + // loop real + while(opModeIsActive()){ + //Drivetrain + double forward = gamepad1.left_stick_y; + double strafe = -gamepad1.left_stick_x; + double turn = -gamepad1.right_stick_x; + + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); + + double rightFrontPower = (forward - strafe - turn) / denominator; + double leftFrontPower = (forward + strafe + turn) / denominator; + double rightBackPower = (forward + strafe - turn) / denominator; + double leftBackPower = (forward - strafe + turn) / denominator; + + if (gamepad1.left_bumper) { + rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); + leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); + rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); + leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); + } else { + rightFrontPower = Range.clip(rightFrontPower, -0.8, 0.8); + leftFrontPower = Range.clip(leftFrontPower, -0.8, 0.8); + rightBackPower = Range.clip(rightBackPower, -0.8, 0.8); + leftBackPower = Range.clip(leftBackPower, -0.8, 0.8); + } + + + rightFront.setPower(rightFrontPower); + leftFront.setPower(leftFrontPower); + rightBack.setPower(rightBackPower); + leftBack.setPower(leftBackPower); + + if (gamepad1.right_bumper){ + leftHang.setDirection(DcMotor.Direction.FORWARD); + rightHang.setDirection(DcMotor.Direction.FORWARD); + + leftHang.setPower(.9); + rightHang.setPower(.9); + } else if (leftUpper.isPressed() || rightUpper.isPressed() || gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); + } + if (gamepad1.left_bumper){ + leftHang.setDirection(DcMotor.Direction.REVERSE); + rightHang.setDirection(DcMotor.Direction.REVERSE); + + leftHang.setPower(.9); + rightHang.setPower(.9); + } else if (leftLower.isPressed() || rightLower.isPressed() || gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); + } + + + + + telemetry.addData("Status", "Run " + runtime.toString()); + telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); + + //Code(); + initPos(); + //airplane(); + //ogDrive.og_drive_code(gamepad1, telemetry); + //IsDrive.is_drive_code(gamepad1, telemetry); + //driveAroundPos(); + //dumpPrepTwo(); + //homePrep(); + // drivePos(); + // dumpLeft(); + //theJuke(); + //liftFunction(); + //worm(); + //stopMotion(); + //aroundthetop(); + //armmUp(); + //armmDown(); + //SlideFunction(); + telemetry.update(); + // sleep(100); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java index 66c975b533..8d42736a36 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -19,7 +19,9 @@ public class OdoMec extends LinearOpMode { private DcMotor rightBack; //rear right 1 private DcMotor leftBack; //rear left 3 - public void runOpMode() { + + + public void runOpMode() { telemetry.addData("Status", "Initialized"); telemetry.update(); From 58328a970b7ec0bcf77b9af7ade535c709a5d178 Mon Sep 17 00:00:00 2001 From: russmc Date: Fri, 1 Dec 2023 14:15:07 -0700 Subject: [PATCH 096/154] duckops with state machines and no sleeps --- .../firstinspires/ftc/teamcode/DuckOps.java | 623 ++++++++++-------- 1 file changed, 351 insertions(+), 272 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java index da35f4d26c..adcd0141f2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java @@ -18,6 +18,17 @@ @TeleOp public class DuckOps extends LinearOpMode { + class ScorePosition { + double shoulderPosition; + double wristPosition; + double elbowPosition; + + public ScorePosition(double shoulderPos, double wristPos, double elbowPos) { + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + } + } // Declare vars private RevTouchSensor rightUpper; private RevTouchSensor leftUpper; @@ -45,9 +56,66 @@ public class DuckOps extends LinearOpMode { private static final double RIGHT_FINGER_GRIP = .27; private static final double RIGHT_FINGER_DROP = .5; private static final double RIGHT_FINGER_INTAKE = 0.64; + // Define a threshold for trigger activation + private static final double TRIGGER_THRESHOLD = 0.5; + private static final double LAUNCHER_START_POS = 0.8; + + private static final double SERVO_TOLERANCE = 0.01; + + private enum intakeState { + IDLE, + MOVING_SHOULDER, + MOVING_ELBOW, + MOVING_WRIST, + MOVING_CLAWS, + COMPLETED + } + + private intakeState currentintakeState = intakeState.IDLE; + + // for picking up the top two off the stack of 5 + private enum intakeStackTopTwoState { + IDLE, + MOVING_SHOULDER, + MOVING_ELBOW, + MOVING_WRIST, + MOVING_CLAWS, + COMPLETED + } + // for picking up the second two off the stack of 3 + private intakeStackTopTwoState currentintakeStackTopTwoState = intakeStackTopTwoState.IDLE; + + private enum intakeStackSecondTwoState { + IDLE, + MOVING_SHOULDER, + MOVING_ELBOW, + MOVING_WRIST, + MOVING_CLAWS, + COMPLETED + } + + private intakeStackSecondTwoState currentintakeStackSecondTwoState = intakeStackSecondTwoState.IDLE; + + private enum driveState { + IDLE, + MOVING_ELBOW, + MOVING_WRIST, + MOVING_SHOULDER, + COMPLETED + } + + private driveState currentdriveState = driveState.IDLE; + + private enum scoreState { + IDLE, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + COMPLETED + } + + private scoreState currentscoreState = scoreState.IDLE; - //MecanumDrive drive; - //OgDrive ogDrive; private ElapsedTime runtime = new ElapsedTime(); //Motors //Drivetrain @@ -57,20 +125,6 @@ public class DuckOps extends LinearOpMode { private DcMotor leftBack; //rear left 3 private DcMotor leftHang; private DcMotor rightHang; - - - // Servo prep - private static enum ArmPosition { - INTAKE, - DRIVE, - UPGO1, - UPGO2, - UPGO3, - UPGO4, - UPGO5; - } - - private ArmPosition currentArmPos; double LiftLeftOffset = .04; double LiftHeight; private AndroidTextToSpeech androidTextToSpeech; @@ -78,268 +132,263 @@ private static enum ArmPosition { // Functions \/ + private void handleIntakeSequence() { + switch (currentintakeState) { + case IDLE: + // Start the sequence + currentintakeState = intakeState.MOVING_SHOULDER; + break; - private void servo_shenanigans() { - setLiftHeight(0.4); - // sleep(10000); - setLiftHeight(0.05); - } + case MOVING_SHOULDER: + // Move the shoulder to intake position + shoulder.setPosition(SHOULDER_INT); + if (isServoAtPosition(shoulder, SHOULDER_INT)) { + currentintakeState = intakeState.MOVING_ELBOW; + } + break; - private void setLiftHeight(double inputLiftHeight) { - if (inputLiftHeight < 0.42) { - inputLiftHeight = 0.42; - } - if (inputLiftHeight > 1) { - inputLiftHeight = 1; - } - LiftHeight = inputLiftHeight; - leftLift.setPosition(LiftLeftOffset + LiftHeight); - rightLift.setPosition(LiftHeight); + case MOVING_ELBOW: + // Move the elbow to intake position + elbow.setPosition(ELBOW_INTAKE); + if (isServoAtPosition(elbow, ELBOW_INTAKE)) { + currentintakeState = intakeState.MOVING_WRIST; + } + break; - } + case MOVING_WRIST: + // Move the wrist to intake position + wrist.setPosition(WRIST_INTAKE); + if (isServoAtPosition(wrist, WRIST_INTAKE)) { + currentintakeState = intakeState.MOVING_CLAWS; + } + break; + + case MOVING_CLAWS: + // Move the claws to intake position + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + if (isServoAtPosition(leftFinger, LEFT_FINGER_INTAKE) && isServoAtPosition(rightFinger, RIGHT_FINGER_INTAKE)) { + currentintakeState = intakeState.COMPLETED; + } + break; - private void worm() { - if (gamepad2.left_bumper) { - shoulder.setPosition(0.445); - wrist.setPosition(0.26); + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; } - if (gamepad2.right_bumper) { - shoulder.setPosition(0.92); + if (currentintakeState == intakeState.COMPLETED) { + currentintakeState = intakeState.IDLE; } } +// TODO Assign Buttons to this +// intaking the top two off a stack of 5 + private void handleIntakeStackTopTwoSequence() { + switch (currentintakeStackTopTwoState) { + case IDLE: + // Start the sequence + currentintakeStackTopTwoState = intakeStackTopTwoState.MOVING_SHOULDER; + break; - private void airplane() { - if (gamepad2.back) { - launcher.setPosition(0.1); - } else { - launcher.setPosition(0.8); - } - } + case MOVING_SHOULDER: + // Move the shoulder to intake position + shoulder.setPosition(0); // TODO set position with McTuner + if (isServoAtPosition(shoulder, 0)) { // TODO same position as above + currentintakeStackTopTwoState = intakeStackTopTwoState.MOVING_ELBOW; + } + break; + + case MOVING_ELBOW: + // Move the elbow to intake position + elbow.setPosition(ELBOW_INTAKE); // TODO set position with McTuner + if (isServoAtPosition(elbow, ELBOW_INTAKE)) { // TODO same position as above + currentintakeStackTopTwoState = intakeStackTopTwoState.MOVING_WRIST; + } + break; + + case MOVING_WRIST: + // Move the wrist to intake position + wrist.setPosition(WRIST_INTAKE); // TODO set position with McTuner + if (isServoAtPosition(wrist, WRIST_INTAKE)) { // TODO same position as above + currentintakeStackTopTwoState = intakeStackTopTwoState.MOVING_CLAWS; + } + break; - private void liftFunction() { - if (gamepad2.y) { - shoulder.setPosition(0.79); - setLiftHeight(0.42); - - wrist.setPosition(0.2); - //sleep(600); - shoulder.setPosition(0.60); - // sleep(600); - shoulder.setPosition(0.50); - //sleep(600); - intakePos(); - } else if (gamepad2.x) { - setLiftHeight(0.71); - } else if (gamepad2.b) { - setLiftHeight(0.56); + case MOVING_CLAWS: + // Move the claws to intake position + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + if (isServoAtPosition(leftFinger, LEFT_FINGER_INTAKE) && isServoAtPosition(rightFinger, RIGHT_FINGER_INTAKE)) { + currentintakeStackTopTwoState = intakeStackTopTwoState.COMPLETED; + } + break; + + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; } - } - private void armmDown() { - if (gamepad2.y) { - switch (currentArmPos) { - case UPGO1: - currentArmPos = ArmPosition.INTAKE; - break; - case UPGO2: - currentArmPos = ArmPosition.UPGO1; - break; - case UPGO3: - currentArmPos = ArmPosition.UPGO2; - break; - case UPGO4: - currentArmPos = ArmPosition.UPGO3; - break; - case UPGO5: - currentArmPos = ArmPosition.UPGO4; - break; - } - doArmm(); - telemetry.addLine(); - telemetry.addData("armpostion", currentArmPos.toString()); - //(100); + if (currentintakeStackTopTwoState == intakeStackTopTwoState.COMPLETED) { + currentintakeStackTopTwoState = intakeStackTopTwoState.IDLE; } } - private void doArmm() { - switch (currentArmPos) { - case INTAKE: - intakePos(); +// TODO Assign Buttons to this +// intaking the second two off a stack of 3 + private void handleIntakeStackSecondTwoSequence() { + switch (currentintakeStackSecondTwoState) { + case IDLE: + // Start the sequence + currentintakeStackSecondTwoState = intakeStackSecondTwoState.MOVING_SHOULDER; break; - case UPGO1: - shoulder.setPosition(0.55); - wrist.setPosition(0.28); - //setLiftHeight(0.42); + case MOVING_SHOULDER: + // Move the shoulder to intake position + shoulder.setPosition(0); // TODO set position with McTuner + if (isServoAtPosition(shoulder, 0)) { // TODO same position as above + currentintakeStackSecondTwoState = intakeStackSecondTwoState.MOVING_ELBOW; + } break; - case UPGO2: - wrist.setPosition(0.22); - shoulder.setPosition(0.79); - //setLiftHeight(0.42); + case MOVING_ELBOW: + // Move the elbow to intake position + elbow.setPosition(ELBOW_INTAKE); // TODO set position with McTuner + if (isServoAtPosition(elbow, ELBOW_INTAKE)) { // TODO same position as above + currentintakeStackSecondTwoState = intakeStackSecondTwoState.MOVING_WRIST; + } break; - case UPGO3: - wrist.setPosition(0.6); - shoulder.setPosition(0.79); - //setLiftHeight(0.42); + case MOVING_WRIST: + // Move the wrist to intake position + wrist.setPosition(WRIST_INTAKE); // TODO set position with McTuner + if (isServoAtPosition(wrist, WRIST_INTAKE)) { // TODO same position as above + currentintakeStackSecondTwoState = intakeStackSecondTwoState.MOVING_CLAWS; + } break; - case UPGO4: - shoulder.setPosition(0.91); - wrist.setPosition(0.6); - //setLiftHeight(0.42); + case MOVING_CLAWS: + // Move the claws to intake position + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + if (isServoAtPosition(leftFinger, LEFT_FINGER_INTAKE) && isServoAtPosition(rightFinger, RIGHT_FINGER_INTAKE)) { + currentintakeStackSecondTwoState = intakeStackSecondTwoState.COMPLETED; + } break; - case UPGO5: - wrist.setPosition(0.6); - shoulder.setPosition(1); - //setLiftHeight(0.42); + case COMPLETED: + // Sequence complete, reset the state or perform additional actions break; } - } - private void armmUp(){ - if (gamepad2.a) { - switch (currentArmPos) { - case INTAKE: - currentArmPos = ArmPosition.UPGO1; - break; - case UPGO1: - currentArmPos = ArmPosition.UPGO2; - break; - case UPGO2: - currentArmPos = ArmPosition.UPGO3; - break; - case UPGO3: - currentArmPos = ArmPosition.UPGO4; - break; - case UPGO4: - currentArmPos = ArmPosition.UPGO5; - break; - } - doArmm(); - telemetry.addLine(); - telemetry.addData("armpostion", currentArmPos.toString()); - //sleep(100); + if (currentintakeStackSecondTwoState == intakeStackSecondTwoState.COMPLETED) { + currentintakeStackSecondTwoState = intakeStackSecondTwoState.IDLE; } } - private void aroundthetop() { - if (gamepad2.start) { - shoulder.setPosition(0.56); - wrist.setPosition(0.5); + private void handleDriveSequence() { + switch (currentdriveState) { + case MOVING_ELBOW: + // Move the elbow to drive position + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + elbow.setPosition(ELBOW_INT); + if (isServoAtPosition(elbow, ELBOW_INT)) { + currentdriveState = driveState.MOVING_WRIST; + } + break; - } - } - private void dumpPrep() { - if (gamepad2.back) { - shoulder.setPosition(0.75); - // sleep(400); + case MOVING_WRIST: + // Move the wrist to drive position + wrist.setPosition(WRIST_DRIVE); + if (isServoAtPosition(wrist, WRIST_DRIVE)) { + currentdriveState = driveState.MOVING_SHOULDER; + } + break; - wrist.setPosition(0.93); - shoulder.setPosition(0.91); - } - } - private void dumpPrepTwo() { - if (gamepad2.back) { + case MOVING_SHOULDER: + // Move the shoulder to drive position + shoulder.setPosition(SHOULDER_INT); + if (isServoAtPosition(shoulder, SHOULDER_INT)) { + currentdriveState = driveState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; + } + // Check to reset the state to IDLE outside the switch + if (currentdriveState == driveState.COMPLETED) { + currentdriveState = driveState.IDLE; } } - private void SlideFunction() { - } + private void handleScorePosSequence(ScorePosition scorePos) { + switch (currentscoreState) { + case IDLE: + // Start the sequence + currentscoreState = scoreState.MOVING_SHOULDER; + break; - private void dumpLeft() { - if (gamepad2.left_bumper) { + case MOVING_SHOULDER: + // Move the shoulder to the specified position + shoulder.setPosition(scorePos.shoulderPosition); + if (isServoAtPosition(shoulder, scorePos.shoulderPosition)) { + currentscoreState = scoreState.MOVING_WRIST; + } + break; - } - if (gamepad2.right_bumper) { + case MOVING_WRIST: + // Move the wrist to the specified position + wrist.setPosition(scorePos.wristPosition); + if (isServoAtPosition(wrist, scorePos.wristPosition)) { + currentscoreState = scoreState.MOVING_ELBOW; + } + break; - } - } - private void driveAroundPos() { - if (gamepad2.dpad_down) { - intakePos(); - } - } - private void drivePos() { - if ((gamepad1.a) && (currentArmPos == ArmPosition.INTAKE)) { - shoulder.setPosition(0.49); - wrist.setPosition(0.55); + case MOVING_ELBOW: + // Move the elbow to the specified position + elbow.setPosition(scorePos.elbowPosition); + if (isServoAtPosition(elbow, scorePos.elbowPosition)) { + currentscoreState = scoreState.COMPLETED; + } + break; - //setLiftHeight(0.42); - currentArmPos = ArmPosition.DRIVE; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; } - } - private void intakePos() { - //hopper.setPosition(0.02); - //shoulder.setPosition(0.44); - //wrist.setPosition(0.26); - wrist.setPosition(0.265); - shoulder.setPosition(0.455); - leftFinger.setPosition(0.5); - rightFinger.setPosition(0.5); - elbow.setPosition(0.5); - - //setLiftHeight(0.42); - currentArmPos = ArmPosition.INTAKE; - } - private void initPos() { - wrist.setPosition(WRIST_INT); - elbow.setPosition(ELBOW_INT); - shoulder.setPosition(SHOULDER_INT); - leftFinger.setPosition(LEFT_FINGER_GRIP); - rightFinger.setPosition(RIGHT_FINGER_GRIP); - } - private void theJuke() { - if ((gamepad2.dpad_up || gamepad1.x) && (currentArmPos == ArmPosition.INTAKE)) { - shoulder.setPosition(0.48); - // sleep(200); - intakePos(); + if (currentscoreState == scoreState.COMPLETED) { + currentscoreState = scoreState.IDLE; } } - private void incrementalIntake() { - shoulder.setPosition(0.455); - // sleep(653); - shoulder.setPosition(0.47); - wrist.setPosition(0.55); - // sleep(531); - shoulder.setPosition(0.49); - wrist.setPosition(0.55); - } - private void launcherstartPos() { - launcher.setPosition(0.8); - } + private boolean isServoAtPosition(Servo servo, double position) { + return Math.abs(servo.getPosition() - position) < SERVO_TOLERANCE; + } -/* - private void driveCode() { - - double SLOW_DOWN_FACTOR; - SLOW_DOWN_FACTOR = 0.5; - telemetry.addData("Running FTC Wires (ftcwires.org) TeleOp Mode adopted for Team:","TEAM NUMBER"); - drive.setDrivePowers(new PoseVelocity2d( - new Vector2d( - -gamepad1.left_stick_y * SLOW_DOWN_FACTOR, - -gamepad1.left_stick_x * SLOW_DOWN_FACTOR - ), - -gamepad1.right_stick_x * SLOW_DOWN_FACTOR - )); - drive.updatePoseEstimate(); + private void setLiftHeight(double inputLiftHeight) { + if (inputLiftHeight < 0.42) { + inputLiftHeight = 0.42; + } + if (inputLiftHeight > 1) { + inputLiftHeight = 1; + } + LiftHeight = inputLiftHeight; + leftLift.setPosition(LiftLeftOffset + LiftHeight); + rightLift.setPosition(LiftHeight); - //telemetry.addData("LF Encoder", drive.leftFront.getCurrentPosition()); - //telemetry.addData("LB Encoder", drive.leftBack.getCurrentPosition()); - //telemetry.addData("RF Encoder", drive.rightFront.getCurrentPosition()); - //telemetry.addData("RB Encoder", drive.rightBack.getCurrentPosition()); + } - telemetry.addData("x", drive.pose.position.x); - telemetry.addLine("Current Pose"); - telemetry.addData("y", drive.pose.position.y); - telemetry.addData("heading", Math.toDegrees(drive.pose.heading.log())); - // telemetry.update(); + private void airplane() { + if (gamepad2.back) { + launcher.setPosition(0.1); + } else { + launcher.setPosition(0.8); + } + } + private void launcherstartPos() { + launcher.setPosition(LAUNCHER_START_POS); } -*/ @Override @@ -349,11 +398,6 @@ public void runOpMode() throws InterruptedException { androidTextToSpeech = new AndroidTextToSpeech(); androidTextToSpeech.initialize(); - // for wires driving - //drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); - - // for Og driving (the best driving) - //ogDrive = new OgDrive(hardwareMap); telemetry.addData("Status", "Initialized"); telemetry.update(); @@ -376,12 +420,6 @@ public void runOpMode() throws InterruptedException { rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - telemetry.addData("Status", "OdoMec2 is ready to run!"); - telemetry.update(); - - - launcher = hardwareMap.get(Servo.class, "launcher"); rightLift = hardwareMap.get(Servo.class, "rightLift"); leftLift = hardwareMap.get(Servo.class, "leftLift"); @@ -396,30 +434,33 @@ public void runOpMode() throws InterruptedException { wrist.setDirection(Servo.Direction.REVERSE); launcher.setDirection(Servo.Direction.REVERSE); - - // sensors leftUpper = hardwareMap.get(RevTouchSensor.class, "leftUpper"); rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); - // servos - //launcherstartPos(); - //intakePos(); + // servos init + wrist.setPosition(WRIST_INT); + elbow.setPosition(ELBOW_INT); + shoulder.setPosition(SHOULDER_INT); + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + + telemetry.addData("Status", "DuckOps is ready to run!"); + telemetry.update(); - //this is a coment to mAKE git update - double SLOW_DOWN_FACTOR = 0.5; telemetry.addData("Initializing TeleOp",""); telemetry.update(); waitForStart(); runtime.reset(); - // servo_shenanigans(); + // loop real while(opModeIsActive()){ - //Drivetrain + +// Mecanum Drive double forward = gamepad1.left_stick_y; double strafe = -gamepad1.left_stick_x; double turn = -gamepad1.right_stick_x; @@ -443,13 +484,13 @@ public void runOpMode() throws InterruptedException { leftBackPower = Range.clip(leftBackPower, -0.8, 0.8); } - rightFront.setPower(rightFrontPower); leftFront.setPower(leftFrontPower); rightBack.setPower(rightBackPower); leftBack.setPower(leftBackPower); - if (gamepad1.right_bumper){ +// Hanging + if (gamepad1.right_trigger > TRIGGER_THRESHOLD){ leftHang.setDirection(DcMotor.Direction.FORWARD); rightHang.setDirection(DcMotor.Direction.FORWARD); @@ -461,7 +502,8 @@ public void runOpMode() throws InterruptedException { leftHang.setPower(0); rightHang.setPower(0); } - if (gamepad1.left_bumper){ + + if (gamepad1.left_trigger > TRIGGER_THRESHOLD){ leftHang.setDirection(DcMotor.Direction.REVERSE); rightHang.setDirection(DcMotor.Direction.REVERSE); @@ -473,33 +515,70 @@ public void runOpMode() throws InterruptedException { leftHang.setPower(0); rightHang.setPower(0); } +// Intaking from the floor + + if (gamepad1.left_bumper && currentintakeState == intakeState.IDLE) { + currentintakeState = intakeState.MOVING_SHOULDER; + } + + handleIntakeSequence(); + +// Go to drive around position + // Check if the right bumper is pressed to start the drive sequence + if (gamepad1.right_bumper && (currentdriveState == driveState.IDLE || currentscoreState == scoreState.IDLE)) { + currentdriveState = driveState.MOVING_ELBOW; + } + handleDriveSequence(); +// Go to score position one + ScorePosition positionOne = null; + if (gamepad2.left_bumper && gamepad2.y && currentdriveState == driveState.IDLE) { + // Assign a new ScorePosition to positionOne inside the if block + positionOne = new ScorePosition(SHOULDER_INT, WRIST_INT, ELBOW_INTAKE); + } + + if (positionOne != null) { + handleScorePosSequence(positionOne); + } + +// Dropping on the backboard for scoring + // operate that claw dropping style + if (gamepad2.dpad_up && currentintakeState == intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_left && currentintakeState == intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + } else if (gamepad2.dpad_right && currentintakeState == intakeState.IDLE) { + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_down && currentintakeState == intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + +// Grabbing off the floor or stack + // operate that claw gripping style + if (gamepad2.right_bumper && gamepad2.dpad_up && (currentscoreState == scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentscoreState == scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentscoreState == scoreState.IDLE)) { + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentscoreState == scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + +// Telemetry telemetry.addData("Status", "Run " + runtime.toString()); telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); - //Code(); - initPos(); - //airplane(); - //ogDrive.og_drive_code(gamepad1, telemetry); - //IsDrive.is_drive_code(gamepad1, telemetry); - //driveAroundPos(); - //dumpPrepTwo(); - //homePrep(); - // drivePos(); - // dumpLeft(); - //theJuke(); - //liftFunction(); - //worm(); - //stopMotion(); - //aroundthetop(); - //armmUp(); - //armmDown(); - //SlideFunction(); + + airplane(); telemetry.update(); - // sleep(100); } } } From 20f51b17ed07321eaf06debf7b1d1e32bd3fa6ce Mon Sep 17 00:00:00 2001 From: digitalbreadllc Date: Sun, 3 Dec 2023 17:13:38 -0700 Subject: [PATCH 097/154] Luke, Gray and Coach Russ --- .../main/java/org/firstinspires/ftc/teamcode/DuckOps.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java index adcd0141f2..5b9173f3a1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java @@ -401,6 +401,11 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Status", "Initialized"); telemetry.update(); + telemetry.addData("Intake State", currentintakeState); +// ... other telemetry data ... + telemetry.update(); + + rightFront = hardwareMap.get(DcMotor.class, "rightFront"); leftFront = hardwareMap.get(DcMotor.class, "leftFront"); rightBack = hardwareMap.get(DcMotor.class, "rightBack"); From 38271aab5e2e54d96d30ef69b4edc4c08c0617e5 Mon Sep 17 00:00:00 2001 From: russmc Date: Sun, 3 Dec 2023 17:14:12 -0700 Subject: [PATCH 098/154] duckops with state machines and no sleeps --- .../ftc/teamcode/Beginnings.java | 2 +- .../firstinspires/ftc/teamcode/DuckOps.java | 5 +- .../ftc/teamcode/McTuner2000.java | 2 +- .../ftc/teamcode/McTuner3000.java | 2 +- .../firstinspires/ftc/teamcode/OdoMec.java | 253 ++++++++++++++---- 5 files changed, 199 insertions(+), 65 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java index 1580067d56..81250c9a5c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Beginnings.java @@ -22,7 +22,7 @@ import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; import org.firstinspires.ftc.teamcode.tuning.TuningOpModes; -@TeleOp +//@TeleOp public class Beginnings extends LinearOpMode { // Declare vars private RevTouchSensor rightUpper; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java index adcd0141f2..8f1188cc48 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java @@ -16,7 +16,7 @@ import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; -@TeleOp +//@TeleOp public class DuckOps extends LinearOpMode { class ScorePosition { double shoulderPosition; @@ -43,7 +43,7 @@ public ScorePosition(double shoulderPos, double wristPos, double elbowPos) { private Servo leftFinger; private Servo rightFinger; - //Servo init values + //Servo values private static final double SHOULDER_INT = 0.425; private static final double ELBOW_INT = 0.5; private static final double ELBOW_INTAKE = 0.8; @@ -59,7 +59,6 @@ public ScorePosition(double shoulderPos, double wristPos, double elbowPos) { // Define a threshold for trigger activation private static final double TRIGGER_THRESHOLD = 0.5; private static final double LAUNCHER_START_POS = 0.8; - private static final double SERVO_TOLERANCE = 0.01; private enum intakeState { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java index 1d6981c9b4..f8b23b6ccd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner2000.java @@ -4,7 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.Servo; -@TeleOp +//@TeleOp public class McTuner2000 extends LinearOpMode { Servo wrist; Servo hopper; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java index 815575f7a5..d39fba9e49 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McTuner3000.java @@ -8,7 +8,7 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagCanvasAnnotator; -@TeleOp +//@TeleOp public class McTuner3000 extends LinearOpMode { //TODO: Step 1, Replace all "wrist","hopper", etc with your servos Servo wrist; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java index 8d42736a36..b33c330066 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -1,90 +1,225 @@ package org.firstinspires.ftc.teamcode; -import com.qualcomm.hardware.bosch.BNO055IMU; -import com.qualcomm.hardware.rev.RevTouchSensor; + import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; -//@TeleOp(name="TeleMec") - +@TeleOp public class OdoMec extends LinearOpMode { + + // Declare vars private ElapsedTime runtime = new ElapsedTime(); - //Motors - //Drivetrain - private DcMotor rightFront; //front right 0 - private DcMotor leftFront; //front left 2 - private DcMotor rightBack; //rear right 1 - private DcMotor leftBack; //rear left 3 + private ElapsedTime servoTimer = new ElapsedTime(); + private Servo shoulder; + private Servo wrist; + private Servo elbow; + private Servo leftFinger; + private Servo rightFinger; + + private Servo rightLift; + private Servo leftLift; + + double LiftLeftOffset = -.05; + double LiftHeight; + private double servoposition = 0.0; + private double servodelta = 0.02; + private double servodelaytime = 0.03; + + //Servo values + private static final double SHOULDER_DRIVE = 0.425; // 0.425 + private static final double SHOULDER_INTAKE = 0.455; // 0.455 + private static final double ELBOW_DRIVE= 0.5; + private static final double ELBOW_INTAKE = 0.8; + private static final double WRIST_INT = 0.55; // int for auto & testing 0.55; // int for DRIVE 0.265 + private static final double WRIST_DRIVE = 0.265; // int for auto & testing 0.55; // int for DRIVE 0.265 + private static final double WRIST_INTAKE = 0.575; + private static final double LEFT_FINGER_GRIP = 0.74; + private static final double LEFT_FINGER_DROP = 0.5; + private static final double LEFT_FINGER_INTAKE = 0.34; + private static final double RIGHT_FINGER_GRIP = .27; + private static final double RIGHT_FINGER_DROP = .5; + private static final double RIGHT_FINGER_INTAKE = 0.64; + private static final double SERVO_TOLERANCE = 0.01; + + private static final double LIFT_HEIGHT_INT = 0.37; + + private enum intakeState { + IDLE, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + COMPLETED + } + private OdoMec.intakeState currentIntakeState = OdoMec.intakeState.IDLE; + + private enum driveState { + IDLE, + MOVING_SHOULDER, + MOVING_ELBOW, + MOVING_WRIST, + COMPLETED + } + private OdoMec.driveState currentDriveState = OdoMec.driveState.IDLE; + + private void handleIntakeSequence() { + switch (currentIntakeState) { + case MOVING_SHOULDER: + // Move the shoulder to intake position + shoulder.setPosition(SHOULDER_DRIVE); + if (isServoAtPosition(shoulder, SHOULDER_DRIVE)) { + currentIntakeState = OdoMec.intakeState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to intake position + wrist.setPosition(WRIST_INTAKE); + if (isServoAtPosition(wrist, WRIST_INTAKE)) { + currentIntakeState = intakeState.MOVING_ELBOW; + } + break; + + case MOVING_ELBOW: + // Move the elbow to preintake position so it don't slap the floor + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + moveServoGradually(elbow, ELBOW_INTAKE); + if (isServoAtPosition(elbow, ELBOW_INTAKE)) { + currentIntakeState = intakeState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; + } + // Check to reset the state to IDLE outside the switch + if (currentIntakeState == OdoMec.intakeState.COMPLETED) { + currentIntakeState = OdoMec.intakeState.IDLE; + } + } + + private void handleDriveSequence() { + switch (currentDriveState) { + case MOVING_SHOULDER: + // Move the shoulder to drive position + shoulder.setPosition(SHOULDER_DRIVE); + if (isServoAtPosition(shoulder, SHOULDER_DRIVE)) { + currentDriveState = OdoMec.driveState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: + // Move the elbow to drive position + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + elbow.setPosition(ELBOW_DRIVE); + if (isServoAtPosition(elbow, ELBOW_DRIVE)) { + currentDriveState = OdoMec.driveState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to drive position + wrist.setPosition(WRIST_DRIVE); + if (isServoAtPosition(wrist, WRIST_DRIVE)) { + currentDriveState = driveState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; + } + // Check to reset the state to IDLE outside the switch + if (currentDriveState == OdoMec.driveState.COMPLETED) { + currentDriveState = OdoMec.driveState.IDLE; + } + } + private boolean isServoAtPosition(Servo servo, double position) { + return Math.abs(servo.getPosition() - position) < SERVO_TOLERANCE; + } + + private void liftDown() { + leftLift.setPosition(LIFT_HEIGHT_INT); + rightLift.setPosition(LIFT_HEIGHT_INT); + } + private void moveServoGradually(Servo servo, double targetPosition) { + double currentPosition = servo.getPosition(); + // Check if enough time has passed since the last update + if (servoTimer.seconds() > servodelaytime) { + // Determine the direction of movement + double direction = targetPosition > currentPosition ? servodelta : -servodelta; + + // Calculate the new position + servoposition = currentPosition + direction; + servoposition = Range.clip(servoposition, 0, 1); // Ensure the position is within valid range + + // Update the servo position + servo.setPosition(servoposition); + + // Reset the timer + servoTimer.reset(); + } + } + @Override + // NOT loop \/ - Or int of vars + public void runOpMode() throws InterruptedException { - public void runOpMode() { telemetry.addData("Status", "Initialized"); telemetry.update(); - rightFront = hardwareMap.get(DcMotor.class, "rightFront"); - leftFront = hardwareMap.get(DcMotor.class, "leftFront"); - rightBack = hardwareMap.get(DcMotor.class, "rightBack"); - leftBack = hardwareMap.get(DcMotor.class, "leftBack"); +// hardware mappings and parameters + shoulder = hardwareMap.get(Servo.class, "shoulder"); + wrist = hardwareMap.get(Servo.class, "wrist"); + elbow = hardwareMap.get(Servo.class, "elbow"); + leftFinger = hardwareMap.get(Servo.class, "lFinger"); + rightFinger = hardwareMap.get(Servo.class, "rFinger"); + + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + +// init postiions + + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + liftDown(); + shoulder.setPosition(SHOULDER_DRIVE); + elbow.setPosition(ELBOW_DRIVE); + wrist.setPosition(WRIST_DRIVE); - rightFront.setDirection(DcMotor.Direction.REVERSE); - leftFront.setDirection(DcMotor.Direction.FORWARD); - rightBack.setDirection(DcMotor.Direction.REVERSE); - leftBack.setDirection(DcMotor.Direction.FORWARD); - //Set motor modes - rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - telemetry.addData("Status", "OdoMec2 is ready to run!"); + telemetry.addData("Status", "OdoMec is ready to run!"); + telemetry.addData("Initializing TeleOp",""); + telemetry.update(); - //Wait for press play + + waitForStart(); runtime.reset(); - while (opModeIsActive()) { - //Drivetrain - double forward = gamepad1.left_stick_y; - double strafe = -gamepad1.left_stick_x; - double turn = -gamepad1.right_stick_x; - - double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); - - double rightFrontPower = (forward - strafe - turn) / denominator; - double leftFrontPower = (forward + strafe + turn) / denominator; - double rightBackPower = (forward + strafe - turn) / denominator; - double leftBackPower = (forward - strafe + turn) / denominator; - - if (gamepad1.left_bumper) { - rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); - leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); - rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); - leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); - } else { - rightFrontPower = Range.clip(rightFrontPower, -0.8, 0.8); - leftFrontPower = Range.clip(leftFrontPower, -0.8, 0.8); - rightBackPower = Range.clip(rightBackPower, -0.8, 0.8); - leftBackPower = Range.clip(leftBackPower, -0.8, 0.8); - } + while(opModeIsActive()){ - rightFront.setPower(rightFrontPower); - leftFront.setPower(leftFrontPower); - rightBack.setPower(rightBackPower); - leftBack.setPower(leftBackPower); + if (gamepad1.left_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { + currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; + } + + handleIntakeSequence(); - telemetry.addData("Status", "Run " + runtime.toString()); - telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); - telemetry.update(); + if (gamepad1.right_bumper && currentDriveState == OdoMec.driveState.IDLE) { + currentDriveState = OdoMec.driveState.MOVING_SHOULDER; + } + handleDriveSequence(); - // run until the end of } + } -} +} \ No newline at end of file From 215563a3001f93afb0b66077973b4fa25bf39dbb Mon Sep 17 00:00:00 2001 From: russmc Date: Tue, 5 Dec 2023 13:47:47 -0700 Subject: [PATCH 099/154] OdoMec with Isabel's take on State Machines and no sleeps. --- .../firstinspires/ftc/teamcode/DuckOps.java | 588 ------------------ .../firstinspires/ftc/teamcode/McMuffin.java | 12 +- .../firstinspires/ftc/teamcode/OdoMec.java | 506 +++++++++++++-- .../ftc/teamcode/tuning/TuningOpModes.java | 2 +- 4 files changed, 473 insertions(+), 635 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java deleted file mode 100644 index e8c9749e17..0000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckOps.java +++ /dev/null @@ -1,588 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -// imports - -//import com.acmerobotics.roadrunner.Pose2d; -//import com.acmerobotics.roadrunner.PoseVelocity2d; -//import com.acmerobotics.roadrunner.Vector2d; - -import com.qualcomm.hardware.rev.RevTouchSensor; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.Range; - -import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; - -//@TeleOp -public class DuckOps extends LinearOpMode { - class ScorePosition { - double shoulderPosition; - double wristPosition; - double elbowPosition; - - public ScorePosition(double shoulderPos, double wristPos, double elbowPos) { - this.shoulderPosition = shoulderPos; - this.wristPosition = wristPos; - this.elbowPosition = elbowPos; - } - } - // Declare vars - private RevTouchSensor rightUpper; - private RevTouchSensor leftUpper; - private RevTouchSensor rightLower; - private RevTouchSensor leftLower; - private Servo launcher; - private Servo rightLift; - private Servo leftLift; - private Servo shoulder; - private Servo wrist; - private Servo elbow; - private Servo leftFinger; - private Servo rightFinger; - - //Servo values - private static final double SHOULDER_INT = 0.425; - private static final double ELBOW_INT = 0.5; - private static final double ELBOW_INTAKE = 0.8; - private static final double WRIST_INT = 0.55; - private static final double WRIST_INTAKE = 0.575; - private static final double WRIST_DRIVE = 0.265; - private static final double LEFT_FINGER_GRIP = 0.74; - private static final double LEFT_FINGER_DROP = 0.5; - private static final double LEFT_FINGER_INTAKE = 0.34; - private static final double RIGHT_FINGER_GRIP = .27; - private static final double RIGHT_FINGER_DROP = .5; - private static final double RIGHT_FINGER_INTAKE = 0.64; - // Define a threshold for trigger activation - private static final double TRIGGER_THRESHOLD = 0.5; - private static final double LAUNCHER_START_POS = 0.8; - private static final double SERVO_TOLERANCE = 0.01; - - private enum intakeState { - IDLE, - MOVING_SHOULDER, - MOVING_ELBOW, - MOVING_WRIST, - MOVING_CLAWS, - COMPLETED - } - - private intakeState currentintakeState = intakeState.IDLE; - - // for picking up the top two off the stack of 5 - private enum intakeStackTopTwoState { - IDLE, - MOVING_SHOULDER, - MOVING_ELBOW, - MOVING_WRIST, - MOVING_CLAWS, - COMPLETED - } - // for picking up the second two off the stack of 3 - private intakeStackTopTwoState currentintakeStackTopTwoState = intakeStackTopTwoState.IDLE; - - private enum intakeStackSecondTwoState { - IDLE, - MOVING_SHOULDER, - MOVING_ELBOW, - MOVING_WRIST, - MOVING_CLAWS, - COMPLETED - } - - private intakeStackSecondTwoState currentintakeStackSecondTwoState = intakeStackSecondTwoState.IDLE; - - private enum driveState { - IDLE, - MOVING_ELBOW, - MOVING_WRIST, - MOVING_SHOULDER, - COMPLETED - } - - private driveState currentdriveState = driveState.IDLE; - - private enum scoreState { - IDLE, - MOVING_SHOULDER, - MOVING_WRIST, - MOVING_ELBOW, - COMPLETED - } - - private scoreState currentscoreState = scoreState.IDLE; - - private ElapsedTime runtime = new ElapsedTime(); - //Motors - //Drivetrain - private DcMotor rightFront; //front right 0 - private DcMotor leftFront; //front left 2 - private DcMotor rightBack; //rear right 1 - private DcMotor leftBack; //rear left 3 - private DcMotor leftHang; - private DcMotor rightHang; - double LiftLeftOffset = .04; - double LiftHeight; - private AndroidTextToSpeech androidTextToSpeech; - - - // Functions \/ - - private void handleIntakeSequence() { - switch (currentintakeState) { - case IDLE: - // Start the sequence - currentintakeState = intakeState.MOVING_SHOULDER; - break; - - case MOVING_SHOULDER: - // Move the shoulder to intake position - shoulder.setPosition(SHOULDER_INT); - if (isServoAtPosition(shoulder, SHOULDER_INT)) { - currentintakeState = intakeState.MOVING_ELBOW; - } - break; - - case MOVING_ELBOW: - // Move the elbow to intake position - elbow.setPosition(ELBOW_INTAKE); - if (isServoAtPosition(elbow, ELBOW_INTAKE)) { - currentintakeState = intakeState.MOVING_WRIST; - } - break; - - case MOVING_WRIST: - // Move the wrist to intake position - wrist.setPosition(WRIST_INTAKE); - if (isServoAtPosition(wrist, WRIST_INTAKE)) { - currentintakeState = intakeState.MOVING_CLAWS; - } - break; - - case MOVING_CLAWS: - // Move the claws to intake position - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - if (isServoAtPosition(leftFinger, LEFT_FINGER_INTAKE) && isServoAtPosition(rightFinger, RIGHT_FINGER_INTAKE)) { - currentintakeState = intakeState.COMPLETED; - } - break; - - case COMPLETED: - // Sequence complete, reset the state or perform additional actions - break; - } - if (currentintakeState == intakeState.COMPLETED) { - currentintakeState = intakeState.IDLE; - } - } -// TODO Assign Buttons to this -// intaking the top two off a stack of 5 - private void handleIntakeStackTopTwoSequence() { - switch (currentintakeStackTopTwoState) { - case IDLE: - // Start the sequence - currentintakeStackTopTwoState = intakeStackTopTwoState.MOVING_SHOULDER; - break; - - case MOVING_SHOULDER: - // Move the shoulder to intake position - shoulder.setPosition(0); // TODO set position with McTuner - if (isServoAtPosition(shoulder, 0)) { // TODO same position as above - currentintakeStackTopTwoState = intakeStackTopTwoState.MOVING_ELBOW; - } - break; - - case MOVING_ELBOW: - // Move the elbow to intake position - elbow.setPosition(ELBOW_INTAKE); // TODO set position with McTuner - if (isServoAtPosition(elbow, ELBOW_INTAKE)) { // TODO same position as above - currentintakeStackTopTwoState = intakeStackTopTwoState.MOVING_WRIST; - } - break; - - case MOVING_WRIST: - // Move the wrist to intake position - wrist.setPosition(WRIST_INTAKE); // TODO set position with McTuner - if (isServoAtPosition(wrist, WRIST_INTAKE)) { // TODO same position as above - currentintakeStackTopTwoState = intakeStackTopTwoState.MOVING_CLAWS; - } - break; - - case MOVING_CLAWS: - // Move the claws to intake position - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - if (isServoAtPosition(leftFinger, LEFT_FINGER_INTAKE) && isServoAtPosition(rightFinger, RIGHT_FINGER_INTAKE)) { - currentintakeStackTopTwoState = intakeStackTopTwoState.COMPLETED; - } - break; - - case COMPLETED: - // Sequence complete, reset the state or perform additional actions - break; - } - if (currentintakeStackTopTwoState == intakeStackTopTwoState.COMPLETED) { - currentintakeStackTopTwoState = intakeStackTopTwoState.IDLE; - } - } -// TODO Assign Buttons to this -// intaking the second two off a stack of 3 - private void handleIntakeStackSecondTwoSequence() { - switch (currentintakeStackSecondTwoState) { - case IDLE: - // Start the sequence - currentintakeStackSecondTwoState = intakeStackSecondTwoState.MOVING_SHOULDER; - break; - - case MOVING_SHOULDER: - // Move the shoulder to intake position - shoulder.setPosition(0); // TODO set position with McTuner - if (isServoAtPosition(shoulder, 0)) { // TODO same position as above - currentintakeStackSecondTwoState = intakeStackSecondTwoState.MOVING_ELBOW; - } - break; - - case MOVING_ELBOW: - // Move the elbow to intake position - elbow.setPosition(ELBOW_INTAKE); // TODO set position with McTuner - if (isServoAtPosition(elbow, ELBOW_INTAKE)) { // TODO same position as above - currentintakeStackSecondTwoState = intakeStackSecondTwoState.MOVING_WRIST; - } - break; - - case MOVING_WRIST: - // Move the wrist to intake position - wrist.setPosition(WRIST_INTAKE); // TODO set position with McTuner - if (isServoAtPosition(wrist, WRIST_INTAKE)) { // TODO same position as above - currentintakeStackSecondTwoState = intakeStackSecondTwoState.MOVING_CLAWS; - } - break; - - case MOVING_CLAWS: - // Move the claws to intake position - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - if (isServoAtPosition(leftFinger, LEFT_FINGER_INTAKE) && isServoAtPosition(rightFinger, RIGHT_FINGER_INTAKE)) { - currentintakeStackSecondTwoState = intakeStackSecondTwoState.COMPLETED; - } - break; - - case COMPLETED: - // Sequence complete, reset the state or perform additional actions - break; - } - if (currentintakeStackSecondTwoState == intakeStackSecondTwoState.COMPLETED) { - currentintakeStackSecondTwoState = intakeStackSecondTwoState.IDLE; - } - } - private void handleDriveSequence() { - switch (currentdriveState) { - case MOVING_ELBOW: - // Move the elbow to drive position - leftFinger.setPosition(LEFT_FINGER_GRIP); - rightFinger.setPosition(RIGHT_FINGER_GRIP); - elbow.setPosition(ELBOW_INT); - if (isServoAtPosition(elbow, ELBOW_INT)) { - currentdriveState = driveState.MOVING_WRIST; - } - break; - - case MOVING_WRIST: - // Move the wrist to drive position - wrist.setPosition(WRIST_DRIVE); - if (isServoAtPosition(wrist, WRIST_DRIVE)) { - currentdriveState = driveState.MOVING_SHOULDER; - } - break; - - case MOVING_SHOULDER: - // Move the shoulder to drive position - shoulder.setPosition(SHOULDER_INT); - if (isServoAtPosition(shoulder, SHOULDER_INT)) { - currentdriveState = driveState.COMPLETED; - } - break; - - case COMPLETED: - // Sequence complete, reset the state or perform additional actions - break; - } - // Check to reset the state to IDLE outside the switch - if (currentdriveState == driveState.COMPLETED) { - currentdriveState = driveState.IDLE; - } - } - - private void handleScorePosSequence(ScorePosition scorePos) { - switch (currentscoreState) { - case IDLE: - // Start the sequence - currentscoreState = scoreState.MOVING_SHOULDER; - break; - - case MOVING_SHOULDER: - // Move the shoulder to the specified position - shoulder.setPosition(scorePos.shoulderPosition); - if (isServoAtPosition(shoulder, scorePos.shoulderPosition)) { - currentscoreState = scoreState.MOVING_WRIST; - } - break; - - case MOVING_WRIST: - // Move the wrist to the specified position - wrist.setPosition(scorePos.wristPosition); - if (isServoAtPosition(wrist, scorePos.wristPosition)) { - currentscoreState = scoreState.MOVING_ELBOW; - } - break; - - case MOVING_ELBOW: - // Move the elbow to the specified position - elbow.setPosition(scorePos.elbowPosition); - if (isServoAtPosition(elbow, scorePos.elbowPosition)) { - currentscoreState = scoreState.COMPLETED; - } - break; - - case COMPLETED: - // Sequence complete, reset the state or perform additional actions - break; - } - - if (currentscoreState == scoreState.COMPLETED) { - currentscoreState = scoreState.IDLE; - } - } - - - private boolean isServoAtPosition(Servo servo, double position) { - return Math.abs(servo.getPosition() - position) < SERVO_TOLERANCE; - } - - - private void setLiftHeight(double inputLiftHeight) { - if (inputLiftHeight < 0.42) { - inputLiftHeight = 0.42; - } - if (inputLiftHeight > 1) { - inputLiftHeight = 1; - } - LiftHeight = inputLiftHeight; - leftLift.setPosition(LiftLeftOffset + LiftHeight); - rightLift.setPosition(LiftHeight); - - } - - private void airplane() { - if (gamepad2.back) { - launcher.setPosition(0.1); - } else { - launcher.setPosition(0.8); - } - } - - private void launcherstartPos() { - launcher.setPosition(LAUNCHER_START_POS); - } - - - @Override - // NOT loop \/ - Or int of vars - public void runOpMode() throws InterruptedException { - - androidTextToSpeech = new AndroidTextToSpeech(); - androidTextToSpeech.initialize(); - - telemetry.addData("Status", "Initialized"); - telemetry.update(); - - telemetry.addData("Intake State", currentintakeState); -// ... other telemetry data ... - telemetry.update(); - - - rightFront = hardwareMap.get(DcMotor.class, "rightFront"); - leftFront = hardwareMap.get(DcMotor.class, "leftFront"); - rightBack = hardwareMap.get(DcMotor.class, "rightBack"); - leftBack = hardwareMap.get(DcMotor.class, "leftBack"); - - leftHang = hardwareMap.get(DcMotor.class, "leftHang"); - rightHang = hardwareMap.get(DcMotor.class, "rightHang"); - - rightFront.setDirection(DcMotor.Direction.REVERSE); - leftFront.setDirection(DcMotor.Direction.FORWARD); - rightBack.setDirection(DcMotor.Direction.REVERSE); - leftBack.setDirection(DcMotor.Direction.FORWARD); - - //Set motor modes - rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - launcher = hardwareMap.get(Servo.class, "launcher"); - rightLift = hardwareMap.get(Servo.class, "rightLift"); - leftLift = hardwareMap.get(Servo.class, "leftLift"); - wrist = hardwareMap.get(Servo.class, "wrist"); - elbow = hardwareMap.get(Servo.class, "elbow"); - leftFinger = hardwareMap.get(Servo.class, "lFinger"); - rightFinger = hardwareMap.get(Servo.class, "rFinger"); - - shoulder = hardwareMap.get(Servo.class, "shoulder"); - - shoulder.setDirection(Servo.Direction.REVERSE); - wrist.setDirection(Servo.Direction.REVERSE); - launcher.setDirection(Servo.Direction.REVERSE); - - // sensors - leftUpper = hardwareMap.get(RevTouchSensor.class, "leftUpper"); - rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); - leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); - rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); - - // servos init - wrist.setPosition(WRIST_INT); - elbow.setPosition(ELBOW_INT); - shoulder.setPosition(SHOULDER_INT); - leftFinger.setPosition(LEFT_FINGER_GRIP); - rightFinger.setPosition(RIGHT_FINGER_GRIP); - - telemetry.addData("Status", "DuckOps is ready to run!"); - telemetry.update(); - - telemetry.addData("Initializing TeleOp",""); - telemetry.update(); - - - waitForStart(); - runtime.reset(); - - // loop real - while(opModeIsActive()){ - -// Mecanum Drive - double forward = gamepad1.left_stick_y; - double strafe = -gamepad1.left_stick_x; - double turn = -gamepad1.right_stick_x; - - double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); - - double rightFrontPower = (forward - strafe - turn) / denominator; - double leftFrontPower = (forward + strafe + turn) / denominator; - double rightBackPower = (forward + strafe - turn) / denominator; - double leftBackPower = (forward - strafe + turn) / denominator; - - if (gamepad1.left_bumper) { - rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); - leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); - rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); - leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); - } else { - rightFrontPower = Range.clip(rightFrontPower, -0.8, 0.8); - leftFrontPower = Range.clip(leftFrontPower, -0.8, 0.8); - rightBackPower = Range.clip(rightBackPower, -0.8, 0.8); - leftBackPower = Range.clip(leftBackPower, -0.8, 0.8); - } - - rightFront.setPower(rightFrontPower); - leftFront.setPower(leftFrontPower); - rightBack.setPower(rightBackPower); - leftBack.setPower(leftBackPower); - -// Hanging - if (gamepad1.right_trigger > TRIGGER_THRESHOLD){ - leftHang.setDirection(DcMotor.Direction.FORWARD); - rightHang.setDirection(DcMotor.Direction.FORWARD); - - leftHang.setPower(.9); - rightHang.setPower(.9); - } else if (leftUpper.isPressed() || rightUpper.isPressed() || gamepad1.y) { - leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - leftHang.setPower(0); - rightHang.setPower(0); - } - - if (gamepad1.left_trigger > TRIGGER_THRESHOLD){ - leftHang.setDirection(DcMotor.Direction.REVERSE); - rightHang.setDirection(DcMotor.Direction.REVERSE); - - leftHang.setPower(.9); - rightHang.setPower(.9); - } else if (leftLower.isPressed() || rightLower.isPressed() || gamepad1.y) { - leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - leftHang.setPower(0); - rightHang.setPower(0); - } -// Intaking from the floor - - if (gamepad1.left_bumper && currentintakeState == intakeState.IDLE) { - currentintakeState = intakeState.MOVING_SHOULDER; - } - - handleIntakeSequence(); - -// Go to drive around position - // Check if the right bumper is pressed to start the drive sequence - if (gamepad1.right_bumper && (currentdriveState == driveState.IDLE || currentscoreState == scoreState.IDLE)) { - currentdriveState = driveState.MOVING_ELBOW; - } - - handleDriveSequence(); - -// Go to score position one - ScorePosition positionOne = null; - - if (gamepad2.left_bumper && gamepad2.y && currentdriveState == driveState.IDLE) { - // Assign a new ScorePosition to positionOne inside the if block - positionOne = new ScorePosition(SHOULDER_INT, WRIST_INT, ELBOW_INTAKE); - } - - if (positionOne != null) { - handleScorePosSequence(positionOne); - } - - -// Dropping on the backboard for scoring - // operate that claw dropping style - if (gamepad2.dpad_up && currentintakeState == intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_DROP); - rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_left && currentintakeState == intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_DROP); - } else if (gamepad2.dpad_right && currentintakeState == intakeState.IDLE) { - rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_down && currentintakeState == intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - } - -// Grabbing off the floor or stack - // operate that claw gripping style - if (gamepad2.right_bumper && gamepad2.dpad_up && (currentscoreState == scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_GRIP); - rightFinger.setPosition(RIGHT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentscoreState == scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentscoreState == scoreState.IDLE)) { - rightFinger.setPosition(RIGHT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentscoreState == scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - } - -// Telemetry - telemetry.addData("Status", "Run " + runtime.toString()); - telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); - - - airplane(); - telemetry.update(); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java index ebb2460123..622e86a344 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java @@ -22,7 +22,7 @@ public class McMuffin extends LinearOpMode { double speedAmount; //TODO: Add offset if needed w/ eg double lift - double LiftLeftOffset = -.05; + //double LiftLeftOffset = -.05; //TODO: Step 3, set all of your servo names below enum ServoTypes{ @@ -54,7 +54,7 @@ else if (which == ServoTypes.WRIST) { wrist.setPosition(wrist.getPosition() - speedAmount); } else if (which == ServoTypes.LIFT) { - leftLift.setPosition((rightLift.getPosition() + LiftLeftOffset) - speedAmount); + leftLift.setPosition(leftLift.getPosition() - speedAmount); rightLift.setPosition(rightLift.getPosition() - speedAmount); } } @@ -75,7 +75,7 @@ else if (which == ServoTypes.WRIST) { wrist.setPosition(wrist.getPosition() + speedAmount); } else if (which == ServoTypes.LIFT) { - leftLift.setPosition((rightLift.getPosition() + LiftLeftOffset) + speedAmount); + leftLift.setPosition(rightLift.getPosition() + speedAmount); rightLift.setPosition(rightLift.getPosition() + speedAmount); } } @@ -141,12 +141,12 @@ public void runOpMode() throws InterruptedException { wrist.setPosition(0.265); - shoulder.setPosition(0.455); + shoulder.setPosition(0.425); elbow.setPosition(0.5); rightFinger.setPosition(0.5); leftFinger.setPosition(0.5); - leftLift.setPosition(0.42 + LiftLeftOffset); - rightLift.setPosition(0.42); + leftLift.setPosition(0.37); + rightLift.setPosition(0.37); // does intake exist????????????? diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java index b33c330066..80bcf39eea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -3,6 +3,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.util.ElapsedTime; @@ -12,26 +13,37 @@ public class OdoMec extends LinearOpMode { // Declare vars + // timers private ElapsedTime runtime = new ElapsedTime(); private ElapsedTime servoTimer = new ElapsedTime(); + + // motors + private DcMotor rightFront; //front right 0 + private DcMotor leftFront; //front left 2 + private DcMotor rightBack; //rear right 1 + private DcMotor leftBack; //rear left 3 + private DcMotor leftHang; + private DcMotor rightHang; + + // servos + private Servo launcher; private Servo shoulder; private Servo wrist; private Servo elbow; private Servo leftFinger; private Servo rightFinger; - private Servo rightLift; private Servo leftLift; - double LiftLeftOffset = -.05; double LiftHeight; private double servoposition = 0.0; private double servodelta = 0.02; private double servodelaytime = 0.03; - //Servo values + + // servo values private static final double SHOULDER_DRIVE = 0.425; // 0.425 - private static final double SHOULDER_INTAKE = 0.455; // 0.455 + private static final double SHOULDER_INT = 0.43; // 0.455 private static final double ELBOW_DRIVE= 0.5; private static final double ELBOW_INTAKE = 0.8; private static final double WRIST_INT = 0.55; // int for auto & testing 0.55; // int for DRIVE 0.265 @@ -43,10 +55,91 @@ public class OdoMec extends LinearOpMode { private static final double RIGHT_FINGER_GRIP = .27; private static final double RIGHT_FINGER_DROP = .5; private static final double RIGHT_FINGER_INTAKE = 0.64; + private static final double TRIGGER_THRESHOLD = 0.5; + private static final double LAUNCHER_START_POS = 0.8; private static final double SERVO_TOLERANCE = 0.01; - - private static final double LIFT_HEIGHT_INT = 0.37; - + private static final double LIFT_DRIVE = 0.37; + private double liftTargetPosition = LIFT_DRIVE; + + // stack positions (top 2 o 5 and next 2 of 3 ) + // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin + // intake two off a stack of five + private static final double SHOULDER_TOP_TWO = SHOULDER_DRIVE; + private static final double WRIST_TOP_TWO = 0.55; + private static final double ELBOW_TOP_TWO = 0.6; + + // intake two off a stack of three + private static final double SHOULDER_NEXT_TWO = SHOULDER_DRIVE; + private static final double WRIST_NEXT_TWO = 0.44; + private static final double ELBOW_NEXT_TWO = 0.7; + + // score positions (11 rows on the board) + // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin + // score position one button map (gamepad2.y) + private static final double SCORE_ONE_SHOULDER = 0.99; + private static final double SCORE_ONE_WRIST = 0.717; + private static final double SCORE_ONE_ELBOW = 0.62; + private static final double SCORE_ONE_LIFT = LIFT_DRIVE; + // score position two button map (gamepad2.b) + private static final double SCORE_TWO_SHOULDER = SHOULDER_DRIVE; + private static final double SCORE_TWO_WRIST = WRIST_DRIVE; + private static final double SCORE_TWO_ELBOW = ELBOW_DRIVE; + private static final double SCORE_TWO_LIFT = LIFT_DRIVE; + // score position three button map (gamepad2.a) + private static final double SCORE_THREE_SHOULDER = SHOULDER_DRIVE; + private static final double SCORE_THREE_WRIST = WRIST_DRIVE; + private static final double SCORE_THREE_ELBOW = ELBOW_DRIVE; + private static final double SCORE_THREE_LIFT = LIFT_DRIVE; + // score position four button map (gamepad2.x) + private static final double SCORE_FOUR_SHOULDER = SHOULDER_DRIVE; + private static final double SCORE_FOUR_WRIST = WRIST_DRIVE; + private static final double SCORE_FOUR_ELBOW = ELBOW_DRIVE; + private static final double SCORE_FOUR_LIFT = LIFT_DRIVE; + // score position five button map (gamepad2.left_bumper && gamepad2.y) + private static final double SCORE_FIVE_SHOULDER = SHOULDER_DRIVE; + private static final double SCORE_FIVE_WRIST = WRIST_DRIVE; + private static final double SCORE_FIVE_ELBOW = ELBOW_DRIVE; + private static final double SCORE_FIVE_LIFT = LIFT_DRIVE; + // score position six button map (gamepad2.left_bumper && gamepad2.b) + private static final double SCORE_SIX_SHOULDER = SHOULDER_DRIVE; + private static final double SCORE_SIX_WRIST = WRIST_DRIVE; + private static final double SCORE_SIX_ELBOW = ELBOW_DRIVE; + private static final double SCORE_SIX_LIFT = LIFT_DRIVE; + // score position seven button map (gamepad2.left_bumper && gamepad2.a) + private static final double SCORE_SEVEN_SHOULDER = SHOULDER_DRIVE; + private static final double SCORE_SEVEN_WRIST = WRIST_DRIVE; + private static final double SCORE_SEVEN_ELBOW = ELBOW_DRIVE; + private static final double SCORE_SEVEN_LIFT = LIFT_DRIVE; + // score position eight button map (gamepad2.left_bumper && gamepad2.x) + private static final double SCORE_EIGHT_SHOULDER = SHOULDER_DRIVE; + private static final double SCORE_EIGHT_WRIST = WRIST_DRIVE; + private static final double SCORE_EIGHT_ELBOW = ELBOW_DRIVE; + private static final double SCORE_EIGHT_LIFT = LIFT_DRIVE; + // score position nine button map (gamepad2.left_trigger && gamepad2.y) + private static final double SCORE_NINE_SHOULDER = SHOULDER_DRIVE; + private static final double SCORE_NINE_WRIST = WRIST_DRIVE; + private static final double SCORE_NINE_ELBOW = ELBOW_DRIVE; + private static final double SCORE_NINE_LIFT = LIFT_DRIVE; + // score position ten button map (gamepad2.left_trigger && gamepad2.b) + private static final double SCORE_TEN_SHOULDER = SHOULDER_DRIVE; + private static final double SCORE_TEN_WRIST = WRIST_DRIVE; + private static final double SCORE_TEN_ELBOW = ELBOW_DRIVE; + private static final double SCORE_TEN_LIFT = LIFT_DRIVE; + // score position eleven button map (gamepad2.left_trigger && gamepad2.a) + private static final double SCORE_ELEVEN_SHOULDER = SHOULDER_DRIVE; + private static final double SCORE_ELEVEN_WRIST = WRIST_DRIVE; + private static final double SCORE_ELEVEN_ELBOW = ELBOW_DRIVE; + private static final double SCORE_ELEVEN_LIFT = LIFT_DRIVE; + + // sensors + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; + + + // state machines enumeration + // intake private enum intakeState { IDLE, MOVING_SHOULDER, @@ -55,7 +148,8 @@ private enum intakeState { COMPLETED } private OdoMec.intakeState currentIntakeState = OdoMec.intakeState.IDLE; - + private OdoMec.IntakePosition activeIntakePosition = null; + // drive private enum driveState { IDLE, MOVING_SHOULDER, @@ -63,31 +157,46 @@ private enum driveState { MOVING_WRIST, COMPLETED } + private OdoMec.driveState currentDriveState = OdoMec.driveState.IDLE; + // score + private enum scoreState { + IDLE, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + MOVING_LIFT, + COMPLETED + } + + private OdoMec.scoreState currentScoreState = OdoMec.scoreState.IDLE; + private OdoMec.ScorePosition activeScorePosition = null; - private void handleIntakeSequence() { + // state machine sequences to provide cascading actions with a single input (button press) + // intake + private void handleIntakeSequence(OdoMec.IntakePosition intakePos) { switch (currentIntakeState) { case MOVING_SHOULDER: // Move the shoulder to intake position - shoulder.setPosition(SHOULDER_DRIVE); - if (isServoAtPosition(shoulder, SHOULDER_DRIVE)) { + moveServoGradually(shoulder, intakePos.shoulderPosition + .05); + shoulder.setPosition(intakePos.shoulderPosition); + if (isServoAtPosition(shoulder, intakePos.shoulderPosition)) { currentIntakeState = OdoMec.intakeState.MOVING_WRIST; } break; case MOVING_WRIST: // Move the wrist to intake position - wrist.setPosition(WRIST_INTAKE); - if (isServoAtPosition(wrist, WRIST_INTAKE)) { + wrist.setPosition(intakePos.wristPosition); + if (isServoAtPosition(wrist, intakePos.wristPosition)) { currentIntakeState = intakeState.MOVING_ELBOW; } break; - case MOVING_ELBOW: - // Move the elbow to preintake position so it don't slap the floor + // Move the elbow to intake position so it don't slap the floor leftFinger.setPosition(LEFT_FINGER_INTAKE); rightFinger.setPosition(RIGHT_FINGER_INTAKE); - moveServoGradually(elbow, ELBOW_INTAKE); - if (isServoAtPosition(elbow, ELBOW_INTAKE)) { + moveServoGradually(elbow, intakePos.elbowPosition); + if (isServoAtPosition(elbow, intakePos.elbowPosition)) { currentIntakeState = intakeState.COMPLETED; } break; @@ -101,10 +210,12 @@ private void handleIntakeSequence() { } } + // drive private void handleDriveSequence() { switch (currentDriveState) { case MOVING_SHOULDER: // Move the shoulder to drive position + moveServoGradually(shoulder, SHOULDER_DRIVE + .05); shoulder.setPosition(SHOULDER_DRIVE); if (isServoAtPosition(shoulder, SHOULDER_DRIVE)) { currentDriveState = OdoMec.driveState.MOVING_ELBOW; @@ -123,7 +234,7 @@ private void handleDriveSequence() { // Move the wrist to drive position wrist.setPosition(WRIST_DRIVE); if (isServoAtPosition(wrist, WRIST_DRIVE)) { - currentDriveState = driveState.COMPLETED; + currentDriveState = OdoMec.driveState.COMPLETED; } break; case COMPLETED: @@ -135,13 +246,82 @@ private void handleDriveSequence() { currentDriveState = OdoMec.driveState.IDLE; } } - private boolean isServoAtPosition(Servo servo, double position) { - return Math.abs(servo.getPosition() - position) < SERVO_TOLERANCE; + + // score + private void handleScorePosSequence(OdoMec.ScorePosition scorePos) { + switch (currentScoreState) { + case MOVING_SHOULDER: + // Move the shoulder to the specified position + moveServoGradually(shoulder, scorePos.shoulderPosition - .05); + shoulder.setPosition(scorePos.shoulderPosition); + if (isServoAtPosition(shoulder, scorePos.shoulderPosition)) { + currentScoreState = OdoMec.scoreState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to the specified position + wrist.setPosition(scorePos.wristPosition); + if (isServoAtPosition(wrist, scorePos.wristPosition)) { + currentScoreState = OdoMec.scoreState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: + // Move the elbow to the specified position + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + elbow.setPosition(scorePos.elbowPosition); + if (isServoAtPosition(elbow, scorePos.elbowPosition)) { + currentScoreState = OdoMec.scoreState.MOVING_LIFT; + } + break; + case MOVING_LIFT: + setLiftPosition(scorePos.liftPosition); + if (isServoAtPosition(leftLift, scorePos.liftPosition) || isServoAtPosition(rightLift, scorePos.liftPosition)) { + currentScoreState = OdoMec.scoreState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + currentScoreState = OdoMec.scoreState.IDLE; + activeScorePosition = null; + break; + } + + if (currentScoreState == OdoMec.scoreState.COMPLETED) { + currentScoreState = OdoMec.scoreState.IDLE; + } + } + + // classes to support state machine actions + static class IntakePosition { + double shoulderPosition; + double wristPosition; + double elbowPosition; + + public IntakePosition(double shoulderPos, double wristPos, double elbowPos) { + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + } } - private void liftDown() { - leftLift.setPosition(LIFT_HEIGHT_INT); - rightLift.setPosition(LIFT_HEIGHT_INT); + static class ScorePosition { + double shoulderPosition; + double wristPosition; + double elbowPosition; + double liftPosition; + + public ScorePosition(double shoulderPos, double wristPos, double elbowPos, double liftPos) { + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + this.liftPosition = liftPos; + } + } + + // functions to support state machine actions + private boolean isServoAtPosition(Servo servo, double position) { + return Math.abs(servo.getPosition() - position) < SERVO_TOLERANCE; } private void moveServoGradually(Servo servo, double targetPosition) { @@ -163,6 +343,29 @@ private void moveServoGradually(Servo servo, double targetPosition) { servoTimer.reset(); } } + + // lift (has been adjusted mechanically to use the same height) + public void setLiftPosition(double targetPosition) { + // Ensure the target position is within the valid range + targetPosition = Math.max(0.0, Math.min(targetPosition, 1.0)); + + // Set the servo positions + leftLift.setPosition(targetPosition); + rightLift.setPosition(targetPosition); + } + + // launcher + private void airplane() { + if (gamepad2.back) { + launcher.setPosition(0.1); + } else { + launcher.setPosition(0.8); + } + } + private void launcherstartPos() { + launcher.setPosition(LAUNCHER_START_POS); + } + @Override // NOT loop \/ - Or int of vars public void runOpMode() throws InterruptedException { @@ -170,55 +373,278 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Status", "Initialized"); telemetry.update(); -// hardware mappings and parameters + // hardware mappings and parameters + + // motors + rightFront = hardwareMap.get(DcMotor.class, "rightFront"); + leftFront = hardwareMap.get(DcMotor.class, "leftFront"); + rightBack = hardwareMap.get(DcMotor.class, "rightBack"); + leftBack = hardwareMap.get(DcMotor.class, "leftBack"); + + leftHang = hardwareMap.get(DcMotor.class, "leftHang"); + rightHang = hardwareMap.get(DcMotor.class, "rightHang"); + + rightFront.setDirection(DcMotor.Direction.REVERSE); + leftFront.setDirection(DcMotor.Direction.FORWARD); + rightBack.setDirection(DcMotor.Direction.REVERSE); + leftBack.setDirection(DcMotor.Direction.FORWARD); + + // motor modes + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // servos shoulder = hardwareMap.get(Servo.class, "shoulder"); wrist = hardwareMap.get(Servo.class, "wrist"); elbow = hardwareMap.get(Servo.class, "elbow"); leftFinger = hardwareMap.get(Servo.class, "lFinger"); rightFinger = hardwareMap.get(Servo.class, "rFinger"); + launcher = hardwareMap.get(Servo.class, "launcher"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + // servo modes shoulder.setDirection(Servo.Direction.REVERSE); wrist.setDirection(Servo.Direction.REVERSE); + launcher.setDirection(Servo.Direction.REVERSE); - rightLift = hardwareMap.get(Servo.class, "rightLift"); - leftLift = hardwareMap.get(Servo.class, "leftLift"); - -// init postiions + // sensors + leftUpper = hardwareMap.get(RevTouchSensor.class, "leftUpper"); + rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); + leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); + rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); + // init positions leftFinger.setPosition(LEFT_FINGER_GRIP); rightFinger.setPosition(RIGHT_FINGER_GRIP); - liftDown(); - shoulder.setPosition(SHOULDER_DRIVE); + setLiftPosition(LIFT_DRIVE); + shoulder.setPosition(SHOULDER_INT); elbow.setPosition(ELBOW_DRIVE); wrist.setPosition(WRIST_DRIVE); + launcherstartPos(); + telemetry.addData("Status", "OdoMec is ready to run!"); + telemetry.addData("Initializing TeleOp",""); + waitForStart(); + runtime.reset(); - telemetry.addData("Status", "OdoMec is ready to run!"); - telemetry.addData("Initializing TeleOp",""); + while(opModeIsActive()){ - telemetry.update(); + // mecanum drive + double forward = gamepad1.left_stick_y; + double strafe = -gamepad1.left_stick_x; + double turn = -gamepad1.right_stick_x; + + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); + + double rightFrontPower = (forward - strafe - turn) / denominator; + double leftFrontPower = (forward + strafe + turn) / denominator; + double rightBackPower = (forward + strafe - turn) / denominator; + double leftBackPower = (forward - strafe + turn) / denominator; + + if (gamepad1.left_bumper) { + rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); + leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); + rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); + leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); + } else { + rightFrontPower = Range.clip(rightFrontPower, -0.8, 0.8); + leftFrontPower = Range.clip(leftFrontPower, -0.8, 0.8); + rightBackPower = Range.clip(rightBackPower, -0.8, 0.8); + leftBackPower = Range.clip(leftBackPower, -0.8, 0.8); + } + rightFront.setPower(rightFrontPower); + leftFront.setPower(leftFrontPower); + rightBack.setPower(rightBackPower); + leftBack.setPower(leftBackPower); + // hanging + if (gamepad1.right_trigger > TRIGGER_THRESHOLD){ + if (!leftUpper.isPressed()) { + leftHang.setDirection(DcMotor.Direction.FORWARD); + leftHang.setPower(.9); + } + if (!rightUpper.isPressed()) { + rightHang.setDirection(DcMotor.Direction.FORWARD); + rightHang.setPower(.9); + } + } else if (gamepad1.left_trigger > TRIGGER_THRESHOLD){ + if (!leftLower.isPressed()) { + leftHang.setDirection(DcMotor.Direction.REVERSE); + leftHang.setPower(.9); + } + if (!rightLower.isPressed()) { + rightHang.setDirection(DcMotor.Direction.REVERSE); + rightHang.setPower(.9); + } + } else { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setPower(0); + } - waitForStart(); - runtime.reset(); - while(opModeIsActive()){ + // emergency stop + if (gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); + } + // claw drive position + if (gamepad1.right_bumper && currentDriveState == OdoMec.driveState.IDLE) { + currentDriveState = OdoMec.driveState.MOVING_SHOULDER; + } + handleDriveSequence(); + + // intake + // claw intake from floor if (gamepad1.left_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { + activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE); + currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; + } + // claw intake the top 2 from a stack of 5 + if (gamepad1.b && currentIntakeState == OdoMec.intakeState.IDLE) { + activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_TOP_TWO, WRIST_TOP_TWO, ELBOW_TOP_TWO); + currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; + } + // claw intake the next 2 from a stack of 3 + if (gamepad1.a && currentIntakeState == OdoMec.intakeState.IDLE) { + activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_NEXT_TWO, WRIST_NEXT_TWO, ELBOW_NEXT_TWO); currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; } + if (activeIntakePosition != null) { + handleIntakeSequence(activeIntakePosition); + } + if (currentIntakeState == OdoMec.intakeState.COMPLETED) { + currentIntakeState = OdoMec.intakeState.IDLE; + activeIntakePosition = null; // Reset the active position + } - handleIntakeSequence(); + // scoring + // score position one + if (gamepad2.y && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, SCORE_ONE_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position two + if (gamepad2.b && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, SCORE_TWO_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position three + if (gamepad2.a && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_THREE_SHOULDER, SCORE_THREE_WRIST, SCORE_THREE_ELBOW, SCORE_THREE_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position four + if (gamepad2.x && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, SCORE_FOUR_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position five + if ((gamepad2.left_bumper && gamepad2.y) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_FIVE_SHOULDER, SCORE_FIVE_WRIST, SCORE_FIVE_ELBOW, SCORE_FIVE_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position six + if ((gamepad2.left_bumper && gamepad2.b) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_SIX_SHOULDER, SCORE_SIX_WRIST, SCORE_SIX_ELBOW, SCORE_SIX_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position seven + if ((gamepad2.left_bumper && gamepad2.a) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_SEVEN_SHOULDER, SCORE_SEVEN_WRIST, SCORE_SEVEN_ELBOW, SCORE_SEVEN_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position eight + if ((gamepad2.left_bumper && gamepad2.x) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_EIGHT_SHOULDER, SCORE_EIGHT_WRIST, SCORE_EIGHT_ELBOW, SCORE_EIGHT_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position nine + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.y) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_NINE_SHOULDER, SCORE_NINE_WRIST, SCORE_NINE_ELBOW, SCORE_NINE_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position ten + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.b) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_TEN_SHOULDER, SCORE_TEN_WRIST, SCORE_TEN_ELBOW, SCORE_TEN_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position eleven + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.a) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_ELBOW, SCORE_ELEVEN_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + if (activeScorePosition != null) { + handleScorePosSequence(activeScorePosition); + } + if (currentScoreState == OdoMec.scoreState.COMPLETED) { + currentScoreState = OdoMec.scoreState.IDLE; + activeScorePosition = null; // Reset the active position + } - if (gamepad1.right_bumper && currentDriveState == OdoMec.driveState.IDLE) { - currentDriveState = OdoMec.driveState.MOVING_SHOULDER; + // dropping on the backboard for scoring + if (gamepad2.dpad_up && currentIntakeState == OdoMec.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_left && currentIntakeState == OdoMec.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + } else if (gamepad2.dpad_right && currentIntakeState == OdoMec.intakeState.IDLE) { + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_down && currentIntakeState == OdoMec.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); } - handleDriveSequence(); + // grabbing off the floor or stack + if (gamepad2.right_bumper && gamepad2.dpad_up && (currentScoreState == OdoMec.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentScoreState == OdoMec.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentScoreState == OdoMec.scoreState.IDLE)) { + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentScoreState == OdoMec.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + + // telemetry + telemetry.addData("Status", "Run " + runtime.toString()); + telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); + telemetry.addData("Intake", currentIntakeState); + telemetry.addData("Drive", currentDriveState); + telemetry.addData("Score", currentScoreState); + + //telemetry.addData("Left Lower", leftLower.isPressed() ? "Pressed" : "Not Pressed"); + //telemetry.addData("Left Upper", leftUpper.isPressed() ? "Pressed" : "Not Pressed"); + //telemetry.addData("Right Lower", rightLower.isPressed() ? "Pressed" : "Not Pressed"); + //telemetry.addData("Right Upper", rightUpper.isPressed() ? "Pressed" : "Not Pressed"); + + // launcher + airplane(); + telemetry.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java index 658f790339..6706b80e5c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java @@ -46,7 +46,7 @@ private static OpModeMeta metaForClass(Class cls) { .build(); } - @OpModeRegistrar + //@OpModeRegistrar public static void register(OpModeManager manager) { if (DISABLED) return; From ec85475941816847ae3834a0ca8a9f091d119633 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 5 Dec 2023 18:11:47 -0700 Subject: [PATCH 100/154] I AM HIM --- .../java/org/firstinspires/ftc/teamcode/McMuffin.java | 2 +- .../java/org/firstinspires/ftc/teamcode/OdoMec.java | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java index 622e86a344..501a4df1ee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java @@ -140,7 +140,7 @@ public void runOpMode() throws InterruptedException { wrist.setDirection(Servo.Direction.REVERSE); - wrist.setPosition(0.265); + wrist.setPosition(0.575); shoulder.setPosition(0.425); elbow.setPosition(0.5); rightFinger.setPosition(0.5); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java index 80bcf39eea..dc6ef59fa0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -422,7 +422,7 @@ public void runOpMode() throws InterruptedException { setLiftPosition(LIFT_DRIVE); shoulder.setPosition(SHOULDER_INT); elbow.setPosition(ELBOW_DRIVE); - wrist.setPosition(WRIST_DRIVE); + wrist.setPosition(WRIST_INTAKE); launcherstartPos(); telemetry.addData("Status", "OdoMec is ready to run!"); @@ -604,14 +604,14 @@ public void runOpMode() throws InterruptedException { } // dropping on the backboard for scoring - if (gamepad2.dpad_up && currentIntakeState == OdoMec.intakeState.IDLE) { + if (gamepad2.dpad_up && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { leftFinger.setPosition(LEFT_FINGER_DROP); rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_left && currentIntakeState == OdoMec.intakeState.IDLE) { + } else if (gamepad2.dpad_left && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { leftFinger.setPosition(LEFT_FINGER_DROP); - } else if (gamepad2.dpad_right && currentIntakeState == OdoMec.intakeState.IDLE) { + } else if (gamepad2.dpad_right && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_down && currentIntakeState == OdoMec.intakeState.IDLE) { + } else if (gamepad2.dpad_down && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { leftFinger.setPosition(LEFT_FINGER_INTAKE); rightFinger.setPosition(RIGHT_FINGER_INTAKE); } From 48de0ed4e82175525a59a3063f42044e56bf0df1 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 5 Dec 2023 19:22:59 -0700 Subject: [PATCH 101/154] goodnight today was good day tele op works very good --- .../firstinspires/ftc/teamcode/McMuffin.java | 4 +- .../firstinspires/ftc/teamcode/OdoMec.java | 82 ++++++++++--------- 2 files changed, 45 insertions(+), 41 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java index 501a4df1ee..5d482d6c69 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java @@ -143,8 +143,8 @@ public void runOpMode() throws InterruptedException { wrist.setPosition(0.575); shoulder.setPosition(0.425); elbow.setPosition(0.5); - rightFinger.setPosition(0.5); - leftFinger.setPosition(0.5); + rightFinger.setPosition(0.27); + leftFinger.setPosition(0.74); leftLift.setPosition(0.37); rightLift.setPosition(0.37); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java index dc6ef59fa0..04c01f2936 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -72,64 +72,65 @@ public class OdoMec extends LinearOpMode { private static final double SHOULDER_NEXT_TWO = SHOULDER_DRIVE; private static final double WRIST_NEXT_TWO = 0.44; private static final double ELBOW_NEXT_TWO = 0.7; - + + //=^-^= // score positions (11 rows on the board) // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin // score position one button map (gamepad2.y) - private static final double SCORE_ONE_SHOULDER = 0.99; - private static final double SCORE_ONE_WRIST = 0.717; - private static final double SCORE_ONE_ELBOW = 0.62; + private static final double SCORE_ONE_SHOULDER = 0.936; + private static final double SCORE_ONE_WRIST = 0.685; + private static final double SCORE_ONE_ELBOW = 0.52; private static final double SCORE_ONE_LIFT = LIFT_DRIVE; // score position two button map (gamepad2.b) - private static final double SCORE_TWO_SHOULDER = SHOULDER_DRIVE; - private static final double SCORE_TWO_WRIST = WRIST_DRIVE; - private static final double SCORE_TWO_ELBOW = ELBOW_DRIVE; + private static final double SCORE_TWO_SHOULDER = 0.9; + private static final double SCORE_TWO_WRIST = 0.745; + private static final double SCORE_TWO_ELBOW = 0.55; private static final double SCORE_TWO_LIFT = LIFT_DRIVE; // score position three button map (gamepad2.a) - private static final double SCORE_THREE_SHOULDER = SHOULDER_DRIVE; - private static final double SCORE_THREE_WRIST = WRIST_DRIVE; - private static final double SCORE_THREE_ELBOW = ELBOW_DRIVE; + private static final double SCORE_THREE_SHOULDER = 0.92; + private static final double SCORE_THREE_WRIST = 0.85; + private static final double SCORE_THREE_ELBOW = 0.67; private static final double SCORE_THREE_LIFT = LIFT_DRIVE; // score position four button map (gamepad2.x) - private static final double SCORE_FOUR_SHOULDER = SHOULDER_DRIVE; - private static final double SCORE_FOUR_WRIST = WRIST_DRIVE; - private static final double SCORE_FOUR_ELBOW = ELBOW_DRIVE; + private static final double SCORE_FOUR_SHOULDER = 0.91; + private static final double SCORE_FOUR_WRIST =0.89; + private static final double SCORE_FOUR_ELBOW = 0.72; private static final double SCORE_FOUR_LIFT = LIFT_DRIVE; // score position five button map (gamepad2.left_bumper && gamepad2.y) - private static final double SCORE_FIVE_SHOULDER = SHOULDER_DRIVE; - private static final double SCORE_FIVE_WRIST = WRIST_DRIVE; - private static final double SCORE_FIVE_ELBOW = ELBOW_DRIVE; + private static final double SCORE_FIVE_SHOULDER = 0.93; + private static final double SCORE_FIVE_WRIST = 0.98; + private static final double SCORE_FIVE_ELBOW = 0.82; private static final double SCORE_FIVE_LIFT = LIFT_DRIVE; // score position six button map (gamepad2.left_bumper && gamepad2.b) - private static final double SCORE_SIX_SHOULDER = SHOULDER_DRIVE; - private static final double SCORE_SIX_WRIST = WRIST_DRIVE; - private static final double SCORE_SIX_ELBOW = ELBOW_DRIVE; + private static final double SCORE_SIX_SHOULDER = 0.92; + private static final double SCORE_SIX_WRIST = 1; + private static final double SCORE_SIX_ELBOW = 0.85; private static final double SCORE_SIX_LIFT = LIFT_DRIVE; // score position seven button map (gamepad2.left_bumper && gamepad2.a) - private static final double SCORE_SEVEN_SHOULDER = SHOULDER_DRIVE; - private static final double SCORE_SEVEN_WRIST = WRIST_DRIVE; - private static final double SCORE_SEVEN_ELBOW = ELBOW_DRIVE; - private static final double SCORE_SEVEN_LIFT = LIFT_DRIVE; + private static final double SCORE_SEVEN_SHOULDER = 0.9; + private static final double SCORE_SEVEN_WRIST = 1; + private static final double SCORE_SEVEN_ELBOW = 0.81; + private static final double SCORE_SEVEN_LIFT = 0.51; // score position eight button map (gamepad2.left_bumper && gamepad2.x) - private static final double SCORE_EIGHT_SHOULDER = SHOULDER_DRIVE; - private static final double SCORE_EIGHT_WRIST = WRIST_DRIVE; - private static final double SCORE_EIGHT_ELBOW = ELBOW_DRIVE; - private static final double SCORE_EIGHT_LIFT = LIFT_DRIVE; + private static final double SCORE_EIGHT_SHOULDER = 0.94; + private static final double SCORE_EIGHT_WRIST = 0.93; + private static final double SCORE_EIGHT_ELBOW = 0.8; + private static final double SCORE_EIGHT_LIFT = 0.8; // score position nine button map (gamepad2.left_trigger && gamepad2.y) - private static final double SCORE_NINE_SHOULDER = SHOULDER_DRIVE; - private static final double SCORE_NINE_WRIST = WRIST_DRIVE; - private static final double SCORE_NINE_ELBOW = ELBOW_DRIVE; - private static final double SCORE_NINE_LIFT = LIFT_DRIVE; + private static final double SCORE_NINE_SHOULDER = 0.93; + private static final double SCORE_NINE_WRIST = 0.93; + private static final double SCORE_NINE_ELBOW = 0.79; + private static final double SCORE_NINE_LIFT = 0.86; // score position ten button map (gamepad2.left_trigger && gamepad2.b) - private static final double SCORE_TEN_SHOULDER = SHOULDER_DRIVE; - private static final double SCORE_TEN_WRIST = WRIST_DRIVE; - private static final double SCORE_TEN_ELBOW = ELBOW_DRIVE; - private static final double SCORE_TEN_LIFT = LIFT_DRIVE; + private static final double SCORE_TEN_SHOULDER = 0.9; + private static final double SCORE_TEN_WRIST = 0.93; + private static final double SCORE_TEN_ELBOW = 0.76; + private static final double SCORE_TEN_LIFT = 0.98; // score position eleven button map (gamepad2.left_trigger && gamepad2.a) - private static final double SCORE_ELEVEN_SHOULDER = SHOULDER_DRIVE; - private static final double SCORE_ELEVEN_WRIST = WRIST_DRIVE; - private static final double SCORE_ELEVEN_ELBOW = ELBOW_DRIVE; - private static final double SCORE_ELEVEN_LIFT = LIFT_DRIVE; + private static final double SCORE_ELEVEN_SHOULDER = 0.9; + private static final double SCORE_ELEVEN_WRIST = 0.93; + private static final double SCORE_ELEVEN_ELBOW = 0.76; + private static final double SCORE_ELEVEN_LIFT = 0.98; // sensors private RevTouchSensor rightUpper; @@ -499,6 +500,7 @@ public void runOpMode() throws InterruptedException { } // claw drive position + //TODO: add safety, this is drive around pos if (gamepad1.right_bumper && currentDriveState == OdoMec.driveState.IDLE) { currentDriveState = OdoMec.driveState.MOVING_SHOULDER; } @@ -506,6 +508,7 @@ public void runOpMode() throws InterruptedException { // intake // claw intake from floor + //TODO: add saftey, this is intake from stuff if (gamepad1.left_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE); currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; @@ -548,6 +551,7 @@ public void runOpMode() throws InterruptedException { currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; } // score position four + //TODO: gotta put !gamepad2.x above if (gamepad2.x && (currentScoreState == OdoMec.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block activeScorePosition = new OdoMec.ScorePosition(SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, SCORE_FOUR_LIFT); From 24a1c08564a8c28706f75a180b6b5d9e5cc949f5 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Thu, 7 Dec 2023 17:23:20 -0700 Subject: [PATCH 102/154] bug fixing of score poses and auto --- .../ftc/teamcode/EnigmaAuto.java | 140 ++++++++---------- .../firstinspires/ftc/teamcode/OdoMec.java | 18 +-- 2 files changed, 70 insertions(+), 88 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 2500b12bc7..50839b8564 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -63,7 +63,7 @@ /** * ENIGMA Autonomous Example for only vision detection using openCv and park */ -@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "Beginnings") +@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "OdoMec") public class EnigmaAuto extends LinearOpMode { public static String TEAM_NAME = "ENIGMA"; //TODO: Enter team Name @@ -71,21 +71,31 @@ public class EnigmaAuto extends LinearOpMode { private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera - private RevTouchSensor rightUpper; - private RevTouchSensor leftUpper; - private RevTouchSensor rightLower; - private RevTouchSensor leftLower; - private Servo rightLift; - private Servo leftLift; + private DcMotor rightFront; //front right 0 + private DcMotor leftFront; //front left 2 + private DcMotor rightBack; //rear right 1 + private DcMotor leftBack; //rear left 3 + private DcMotor leftHang; + private DcMotor rightHang; + + // servos + private Servo launcher; private Servo shoulder; private Servo wrist; - private Servo hopper; - DcMotor frontIntake; - DcMotor rearIntake; - + private Servo elbow; + private Servo leftFinger; + private Servo rightFinger; + private Servo rightLift; + private Servo leftLift; double LiftLeftOffset = .04; double LiftHeight; + // sensors + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; + //Define and declare Robot Starting Locations public enum START_POSITION{ BLUE_LEFT, @@ -114,24 +124,47 @@ public enum IDENTIFIED_SPIKE_MARK_LOCATION { @Override public void runOpMode() throws InterruptedException { - wrist = hardwareMap.get(Servo.class, "wrist"); - hopper = hardwareMap.get(Servo.class, "hopper"); + // motors + rightFront = hardwareMap.get(DcMotor.class, "rightFront"); + leftFront = hardwareMap.get(DcMotor.class, "leftFront"); + rightBack = hardwareMap.get(DcMotor.class, "rightBack"); + leftBack = hardwareMap.get(DcMotor.class, "leftBack"); + + leftHang = hardwareMap.get(DcMotor.class, "leftHang"); + rightHang = hardwareMap.get(DcMotor.class, "rightHang"); + + rightFront.setDirection(DcMotor.Direction.REVERSE); + leftFront.setDirection(DcMotor.Direction.FORWARD); + rightBack.setDirection(DcMotor.Direction.REVERSE); + leftBack.setDirection(DcMotor.Direction.FORWARD); + + // motor modes + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // servos shoulder = hardwareMap.get(Servo.class, "shoulder"); + wrist = hardwareMap.get(Servo.class, "wrist"); + elbow = hardwareMap.get(Servo.class, "elbow"); + leftFinger = hardwareMap.get(Servo.class, "lFinger"); + rightFinger = hardwareMap.get(Servo.class, "rFinger"); + launcher = hardwareMap.get(Servo.class, "launcher"); rightLift = hardwareMap.get(Servo.class, "rightLift"); leftLift = hardwareMap.get(Servo.class, "leftLift"); + // servo modes shoulder.setDirection(Servo.Direction.REVERSE); wrist.setDirection(Servo.Direction.REVERSE); + launcher.setDirection(Servo.Direction.REVERSE); - frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); - rearIntake = hardwareMap.get(DcMotor.class, "rearIntake"); + // sensors + leftUpper = hardwareMap.get(RevTouchSensor.class, "leftUpper"); + rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); + leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); + rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); - frontIntake.setDirection(DcMotorSimple.Direction.FORWARD); - rearIntake.setDirection(DcMotorSimple.Direction.REVERSE); - frontIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rearIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rearIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - frontIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); // Vision OpenCV / Color Detection WebcamName webcamName = hardwareMap.get(WebcamName.class, "Webcam 1"); int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); @@ -331,13 +364,8 @@ public void runAutonoumousMode() { //TODO : Code to drop Purple Pixel on Spike Mark safeWaitSeconds(1); - frontIntake.setPower(.5); - //rearIntake.setPower(-1); - sleep(37); - frontIntake.setPower(0); - //rearIntake.setPower(0); - //drive pos \/ - shoulder.setPosition(0.48); + + //Move robot to midwayPose1 Actions.runBlocking( drive.actionBuilder(drive.pose) @@ -375,59 +403,13 @@ public void runAutonoumousMode() { //TODO : Code to drop Pixel on Backdrop safeWaitSeconds(1); - shoulder.setPosition(0.60); - hopper.setPosition(0.02); - wrist.setPosition(0.22); - setLiftHeight(0.42); - sleep(500); - wrist.setPosition(0.22); - shoulder.setPosition(0.79); - hopper.setPosition(0.02); - setLiftHeight(0.42); - sleep(500); - hopper.setPosition(0.58); - wrist.setPosition(0.6); - shoulder.setPosition(0.79); - setLiftHeight(0.42); - sleep(500); - shoulder.setPosition(0.91); - hopper.setPosition(0.58); - wrist.setPosition(0.6); - setLiftHeight(0.42); - sleep(500); - wrist.setPosition(0.6); - shoulder.setPosition(1); - hopper.setPosition(0.58); - setLiftHeight(0.42); - sleep(500); - //dump - hopper.setPosition(0.2); - sleep(1000); - wrist.setPosition(0.6); - shoulder.setPosition(1); - hopper.setPosition(0.58); - setLiftHeight(0.42); - sleep(200); - shoulder.setPosition(0.91); - hopper.setPosition(0.58); - wrist.setPosition(0.6); - setLiftHeight(0.42); - sleep(200); - hopper.setPosition(0.58); - wrist.setPosition(0.6); - shoulder.setPosition(0.79); - sleep(350); - wrist.setPosition(0.22); - shoulder.setPosition(0.79); - hopper.setPosition(0.02); - setLiftHeight(0.42); - sleep(500); - shoulder.setPosition(0.60); - hopper.setPosition(0.02); - wrist.setPosition(0.22); - setLiftHeight(0.42); + + + //Move robot to park in Backstage + + Actions.runBlocking( drive.actionBuilder(drive.pose) .strafeToLinearHeading(parkPose.position, parkPose.heading) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java index 04c01f2936..8102cd3399 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -533,50 +533,50 @@ public void runOpMode() throws InterruptedException { // scoring // score position one - if (gamepad2.y && (currentScoreState == OdoMec.scoreState.IDLE)) { + if (gamepad2.y && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block activeScorePosition = new OdoMec.ScorePosition(SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, SCORE_ONE_LIFT); currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; } // score position two - if (gamepad2.b && (currentScoreState == OdoMec.scoreState.IDLE)) { + if (gamepad2.b && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block activeScorePosition = new OdoMec.ScorePosition(SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, SCORE_TWO_LIFT); currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; } // score position three - if (gamepad2.a && (currentScoreState == OdoMec.scoreState.IDLE)) { + if (gamepad2.a && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block activeScorePosition = new OdoMec.ScorePosition(SCORE_THREE_SHOULDER, SCORE_THREE_WRIST, SCORE_THREE_ELBOW, SCORE_THREE_LIFT); currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; } // score position four - //TODO: gotta put !gamepad2.x above - if (gamepad2.x && (currentScoreState == OdoMec.scoreState.IDLE)) { + //TODO: gotta put !gamepad2.left_bumper above + if (gamepad2.x && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block activeScorePosition = new OdoMec.ScorePosition(SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, SCORE_FOUR_LIFT); currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; } // score position five - if ((gamepad2.left_bumper && gamepad2.y) && (currentScoreState == OdoMec.scoreState.IDLE)) { + if ((gamepad2.left_bumper && gamepad2.y) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block activeScorePosition = new OdoMec.ScorePosition(SCORE_FIVE_SHOULDER, SCORE_FIVE_WRIST, SCORE_FIVE_ELBOW, SCORE_FIVE_LIFT); currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; } // score position six - if ((gamepad2.left_bumper && gamepad2.b) && (currentScoreState == OdoMec.scoreState.IDLE)) { + if ((gamepad2.left_bumper && gamepad2.b) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block activeScorePosition = new OdoMec.ScorePosition(SCORE_SIX_SHOULDER, SCORE_SIX_WRIST, SCORE_SIX_ELBOW, SCORE_SIX_LIFT); currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; } // score position seven - if ((gamepad2.left_bumper && gamepad2.a) && (currentScoreState == OdoMec.scoreState.IDLE)) { + if ((gamepad2.left_bumper && gamepad2.a) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block activeScorePosition = new OdoMec.ScorePosition(SCORE_SEVEN_SHOULDER, SCORE_SEVEN_WRIST, SCORE_SEVEN_ELBOW, SCORE_SEVEN_LIFT); currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; } // score position eight - if ((gamepad2.left_bumper && gamepad2.x) && (currentScoreState == OdoMec.scoreState.IDLE)) { + if ((gamepad2.left_bumper && gamepad2.x) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block activeScorePosition = new OdoMec.ScorePosition(SCORE_EIGHT_SHOULDER, SCORE_EIGHT_WRIST, SCORE_EIGHT_ELBOW, SCORE_EIGHT_LIFT); currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; From 93c9de61f4ef19f9dd09001da67f3d637983cd8e Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Thu, 7 Dec 2023 19:11:25 -0700 Subject: [PATCH 103/154] good night --- .../org/firstinspires/ftc/teamcode/EnigmaAuto.java | 12 ++++++++++++ .../org/firstinspires/ftc/teamcode/OdoMec.java | 14 +++++++------- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 50839b8564..c11f0172b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -165,6 +165,17 @@ public void runOpMode() throws InterruptedException { leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); + //int pos + leftFinger.setPosition(0.74); + rightFinger.setPosition(0.27); + leftLift.setPosition(0.37); + rightLift.setPosition(0.37); + shoulder.setPosition(0.43); + elbow.setPosition(0.5); + wrist.setPosition(0.575); + launcher.setPosition(0.8); + + // Vision OpenCV / Color Detection WebcamName webcamName = hardwareMap.get(WebcamName.class, "Webcam 1"); int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); @@ -366,6 +377,7 @@ public void runAutonoumousMode() { safeWaitSeconds(1); + //Move robot to midwayPose1 Actions.runBlocking( drive.actionBuilder(drive.pose) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java index 8102cd3399..e837093b97 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -88,17 +88,17 @@ public class OdoMec extends LinearOpMode { private static final double SCORE_TWO_LIFT = LIFT_DRIVE; // score position three button map (gamepad2.a) private static final double SCORE_THREE_SHOULDER = 0.92; - private static final double SCORE_THREE_WRIST = 0.85; + private static final double SCORE_THREE_WRIST = 0.91; private static final double SCORE_THREE_ELBOW = 0.67; private static final double SCORE_THREE_LIFT = LIFT_DRIVE; // score position four button map (gamepad2.x) private static final double SCORE_FOUR_SHOULDER = 0.91; - private static final double SCORE_FOUR_WRIST =0.89; + private static final double SCORE_FOUR_WRIST =0.985; private static final double SCORE_FOUR_ELBOW = 0.72; private static final double SCORE_FOUR_LIFT = LIFT_DRIVE; // score position five button map (gamepad2.left_bumper && gamepad2.y) private static final double SCORE_FIVE_SHOULDER = 0.93; - private static final double SCORE_FIVE_WRIST = 0.98; + private static final double SCORE_FIVE_WRIST = 1; private static final double SCORE_FIVE_ELBOW = 0.82; private static final double SCORE_FIVE_LIFT = LIFT_DRIVE; // score position six button map (gamepad2.left_bumper && gamepad2.b) @@ -453,10 +453,10 @@ public void runOpMode() throws InterruptedException { rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); } else { - rightFrontPower = Range.clip(rightFrontPower, -0.8, 0.8); - leftFrontPower = Range.clip(leftFrontPower, -0.8, 0.8); - rightBackPower = Range.clip(rightBackPower, -0.8, 0.8); - leftBackPower = Range.clip(leftBackPower, -0.8, 0.8); + rightFrontPower = Range.clip(rightFrontPower, -1, 1); + leftFrontPower = Range.clip(leftFrontPower, -1, 1); + rightBackPower = Range.clip(rightBackPower, -1, 1); + leftBackPower = Range.clip(leftBackPower, -1, 1); } rightFront.setPower(rightFrontPower); From 6ad8537ea12f2ec9da819c0ea1eaa6443ce4e3c4 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 9 Dec 2023 12:36:48 -0700 Subject: [PATCH 104/154] refactor names --- .../firstinspires/ftc/teamcode/Mutation.java | 824 +++++++++++------- .../ftc/teamcode/MutationOld.java | 482 ++++++++++ .../firstinspires/ftc/teamcode/OdoMec.java | 655 -------------- 3 files changed, 980 insertions(+), 981 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MutationOld.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 70eeb8b878..e79b6b7629 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -1,42 +1,22 @@ package org.firstinspires.ftc.teamcode; -// imports - -//import com.acmerobotics.roadrunner.Pose2d; -//import com.acmerobotics.roadrunner.PoseVelocity2d; -//import com.acmerobotics.roadrunner.Vector2d; - -import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; -import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; - @TeleOp public class Mutation extends LinearOpMode { - // Declare vars - private RevTouchSensor rightUpper; - private RevTouchSensor leftUpper; - private RevTouchSensor rightLower; - private RevTouchSensor leftLower; - private Servo launcher; - private Servo rightLift; - private Servo leftLift; - private Servo shoulder; - private Servo wrist; - private Servo elbow; - private Servo leftFinger; - private Servo rightFinger; - //MecanumDrive drive; - //OgDrive ogDrive; + // Declare vars + // timers private ElapsedTime runtime = new ElapsedTime(); - //Motors - //Drivetrain + private ElapsedTime servoTimer = new ElapsedTime(); + + // motors private DcMotor rightFront; //front right 0 private DcMotor leftFront; //front left 2 private DcMotor rightBack; //rear right 1 @@ -44,297 +24,358 @@ public class Mutation extends LinearOpMode { private DcMotor leftHang; private DcMotor rightHang; - - // Servo prep - private static enum ArmPosition { - INTAKE, - DRIVE, - UPGO1, - UPGO2, - UPGO3, - UPGO4, - UPGO5; - } - - private ArmPosition currentArmPos; - double LiftLeftOffset = .04; + // servos + private Servo launcher; + private Servo shoulder; + private Servo wrist; + private Servo elbow; + private Servo leftFinger; + private Servo rightFinger; + private Servo rightLift; + private Servo leftLift; + double LiftLeftOffset = -.05; double LiftHeight; - private AndroidTextToSpeech androidTextToSpeech; - - - // Functions \/ + private double servoposition = 0.0; + private double servodelta = 0.02; + private double servodelaytime = 0.03; + + + // servo values + private static final double SHOULDER_DRIVE = 0.425; // 0.425 + private static final double SHOULDER_INT = 0.43; // 0.455 + private static final double ELBOW_DRIVE= 0.5; + private static final double ELBOW_INTAKE = 0.8; + private static final double WRIST_INT = 0.55; // int for auto & testing 0.55; // int for DRIVE 0.265 + private static final double WRIST_DRIVE = 0.265; // int for auto & testing 0.55; // int for DRIVE 0.265 + private static final double WRIST_INTAKE = 0.575; + private static final double LEFT_FINGER_GRIP = 0.74; + private static final double LEFT_FINGER_DROP = 0.5; + private static final double LEFT_FINGER_INTAKE = 0.34; + private static final double RIGHT_FINGER_GRIP = .27; + private static final double RIGHT_FINGER_DROP = .5; + private static final double RIGHT_FINGER_INTAKE = 0.64; + private static final double TRIGGER_THRESHOLD = 0.5; + private static final double LAUNCHER_START_POS = 0.8; + private static final double SERVO_TOLERANCE = 0.01; + private static final double LIFT_DRIVE = 0.37; + private double liftTargetPosition = LIFT_DRIVE; + + // stack positions (top 2 o 5 and next 2 of 3 ) + // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin + // intake two off a stack of five + private static final double SHOULDER_TOP_TWO = SHOULDER_DRIVE; + private static final double WRIST_TOP_TWO = 0.55; + private static final double ELBOW_TOP_TWO = 0.6; + + // intake two off a stack of three + private static final double SHOULDER_NEXT_TWO = SHOULDER_DRIVE; + private static final double WRIST_NEXT_TWO = 0.44; + private static final double ELBOW_NEXT_TWO = 0.7; + + //=^-^= + // score positions (11 rows on the board) + // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin + // score position one button map (gamepad2.y) + private static final double SCORE_ONE_SHOULDER = 0.936; + private static final double SCORE_ONE_WRIST = 0.685; + private static final double SCORE_ONE_ELBOW = 0.52; + private static final double SCORE_ONE_LIFT = LIFT_DRIVE; + // score position two button map (gamepad2.b) + private static final double SCORE_TWO_SHOULDER = 0.9; + private static final double SCORE_TWO_WRIST = 0.745; + private static final double SCORE_TWO_ELBOW = 0.55; + private static final double SCORE_TWO_LIFT = LIFT_DRIVE; + // score position three button map (gamepad2.a) + private static final double SCORE_THREE_SHOULDER = 0.92; + private static final double SCORE_THREE_WRIST = 0.91; + private static final double SCORE_THREE_ELBOW = 0.67; + private static final double SCORE_THREE_LIFT = LIFT_DRIVE; + // score position four button map (gamepad2.x) + private static final double SCORE_FOUR_SHOULDER = 0.91; + private static final double SCORE_FOUR_WRIST =0.985; + private static final double SCORE_FOUR_ELBOW = 0.72; + private static final double SCORE_FOUR_LIFT = LIFT_DRIVE; + // score position five button map (gamepad2.left_bumper && gamepad2.y) + private static final double SCORE_FIVE_SHOULDER = 0.93; + private static final double SCORE_FIVE_WRIST = 1; + private static final double SCORE_FIVE_ELBOW = 0.82; + private static final double SCORE_FIVE_LIFT = LIFT_DRIVE; + // score position six button map (gamepad2.left_bumper && gamepad2.b) + private static final double SCORE_SIX_SHOULDER = 0.92; + private static final double SCORE_SIX_WRIST = 1; + private static final double SCORE_SIX_ELBOW = 0.85; + private static final double SCORE_SIX_LIFT = LIFT_DRIVE; + // score position seven button map (gamepad2.left_bumper && gamepad2.a) + private static final double SCORE_SEVEN_SHOULDER = 0.9; + private static final double SCORE_SEVEN_WRIST = 1; + private static final double SCORE_SEVEN_ELBOW = 0.81; + private static final double SCORE_SEVEN_LIFT = 0.51; + // score position eight button map (gamepad2.left_bumper && gamepad2.x) + private static final double SCORE_EIGHT_SHOULDER = 0.94; + private static final double SCORE_EIGHT_WRIST = 0.93; + private static final double SCORE_EIGHT_ELBOW = 0.8; + private static final double SCORE_EIGHT_LIFT = 0.8; + // score position nine button map (gamepad2.left_trigger && gamepad2.y) + private static final double SCORE_NINE_SHOULDER = 0.93; + private static final double SCORE_NINE_WRIST = 0.93; + private static final double SCORE_NINE_ELBOW = 0.79; + private static final double SCORE_NINE_LIFT = 0.86; + // score position ten button map (gamepad2.left_trigger && gamepad2.b) + private static final double SCORE_TEN_SHOULDER = 0.9; + private static final double SCORE_TEN_WRIST = 0.93; + private static final double SCORE_TEN_ELBOW = 0.76; + private static final double SCORE_TEN_LIFT = 0.98; + // score position eleven button map (gamepad2.left_trigger && gamepad2.a) + private static final double SCORE_ELEVEN_SHOULDER = 0.9; + private static final double SCORE_ELEVEN_WRIST = 0.93; + private static final double SCORE_ELEVEN_ELBOW = 0.76; + private static final double SCORE_ELEVEN_LIFT = 0.98; + + // sensors + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; - private void servo_shenanigans() { - setLiftHeight(0.4); - sleep(10000); - setLiftHeight(0.05); + // state machines enumeration + // intake + private enum intakeState { + IDLE, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + COMPLETED } - - private void setLiftHeight(double inputLiftHeight) { - if (inputLiftHeight < 0.42) { - inputLiftHeight = 0.42; - } - if (inputLiftHeight > 1) { - inputLiftHeight = 1; - } - LiftHeight = inputLiftHeight; - leftLift.setPosition(LiftLeftOffset + LiftHeight); - rightLift.setPosition(LiftHeight); - + private Mutation.intakeState currentIntakeState = Mutation.intakeState.IDLE; + private Mutation.IntakePosition activeIntakePosition = null; + // drive + private enum driveState { + IDLE, + MOVING_SHOULDER, + MOVING_ELBOW, + MOVING_WRIST, + COMPLETED } - private void worm() { - if (gamepad2.left_bumper) { - shoulder.setPosition(0.445); - wrist.setPosition(0.26); - } - if (gamepad2.right_bumper) { - shoulder.setPosition(0.92); - } + private Mutation.driveState currentDriveState = Mutation.driveState.IDLE; + // score + private enum scoreState { + IDLE, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + MOVING_LIFT, + COMPLETED } - private void airplane() { - if (gamepad2.back) { - launcher.setPosition(0.1); - } else { - launcher.setPosition(0.8); + private Mutation.scoreState currentScoreState = Mutation.scoreState.IDLE; + private Mutation.ScorePosition activeScorePosition = null; + + // state machine sequences to provide cascading actions with a single input (button press) + // intake + private void handleIntakeSequence(Mutation.IntakePosition intakePos) { + switch (currentIntakeState) { + case MOVING_SHOULDER: + // Move the shoulder to intake position + moveServoGradually(shoulder, intakePos.shoulderPosition + .05); + shoulder.setPosition(intakePos.shoulderPosition); + if (isServoAtPosition(shoulder, intakePos.shoulderPosition)) { + currentIntakeState = Mutation.intakeState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to intake position + wrist.setPosition(intakePos.wristPosition); + if (isServoAtPosition(wrist, intakePos.wristPosition)) { + currentIntakeState = intakeState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: + // Move the elbow to intake position so it don't slap the floor + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + moveServoGradually(elbow, intakePos.elbowPosition); + if (isServoAtPosition(elbow, intakePos.elbowPosition)) { + currentIntakeState = intakeState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; + } + // Check to reset the state to IDLE outside the switch + if (currentIntakeState == Mutation.intakeState.COMPLETED) { + currentIntakeState = Mutation.intakeState.IDLE; } } - private void liftFunction() { - if (gamepad2.y) { - shoulder.setPosition(0.79); - setLiftHeight(0.42); - - wrist.setPosition(0.2); - sleep(600); - shoulder.setPosition(0.60); - sleep(600); - shoulder.setPosition(0.50); - sleep(600); - intakePos(); - } else if (gamepad2.x) { - setLiftHeight(0.71); - } else if (gamepad2.b) { - setLiftHeight(0.56); + // drive + private void handleDriveSequence() { + switch (currentDriveState) { + case MOVING_SHOULDER: + // Move the shoulder to drive position + moveServoGradually(shoulder, SHOULDER_DRIVE + .05); + shoulder.setPosition(SHOULDER_DRIVE); + if (isServoAtPosition(shoulder, SHOULDER_DRIVE)) { + currentDriveState = Mutation.driveState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: + // Move the elbow to drive position + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + elbow.setPosition(ELBOW_DRIVE); + if (isServoAtPosition(elbow, ELBOW_DRIVE)) { + currentDriveState = Mutation.driveState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to drive position + wrist.setPosition(WRIST_DRIVE); + if (isServoAtPosition(wrist, WRIST_DRIVE)) { + currentDriveState = Mutation.driveState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; } - } - private void armmDown() { - if (gamepad2.y) { - switch (currentArmPos) { - case UPGO1: - currentArmPos = ArmPosition.INTAKE; - break; - case UPGO2: - currentArmPos = ArmPosition.UPGO1; - break; - case UPGO3: - currentArmPos = ArmPosition.UPGO2; - break; - case UPGO4: - currentArmPos = ArmPosition.UPGO3; - break; - case UPGO5: - currentArmPos = ArmPosition.UPGO4; - break; - } - doArmm(); - telemetry.addLine(); - telemetry.addData("armpostion", currentArmPos.toString()); - sleep(100); + // Check to reset the state to IDLE outside the switch + if (currentDriveState == Mutation.driveState.COMPLETED) { + currentDriveState = Mutation.driveState.IDLE; } } - private void doArmm() { - switch (currentArmPos) { - case INTAKE: - intakePos(); - break; - case UPGO1: - shoulder.setPosition(0.55); - wrist.setPosition(0.28); - //setLiftHeight(0.42); + // score + private void handleScorePosSequence(Mutation.ScorePosition scorePos) { + switch (currentScoreState) { + case MOVING_SHOULDER: + // Move the shoulder to the specified position + moveServoGradually(shoulder, scorePos.shoulderPosition - .05); + shoulder.setPosition(scorePos.shoulderPosition); + if (isServoAtPosition(shoulder, scorePos.shoulderPosition)) { + currentScoreState = Mutation.scoreState.MOVING_WRIST; + } break; - case UPGO2: - wrist.setPosition(0.22); - shoulder.setPosition(0.79); - - //setLiftHeight(0.42); + case MOVING_WRIST: + // Move the wrist to the specified position + wrist.setPosition(scorePos.wristPosition); + if (isServoAtPosition(wrist, scorePos.wristPosition)) { + currentScoreState = Mutation.scoreState.MOVING_ELBOW; + } break; - case UPGO3: - - wrist.setPosition(0.6); - shoulder.setPosition(0.79); - //setLiftHeight(0.42); + case MOVING_ELBOW: + // Move the elbow to the specified position + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + elbow.setPosition(scorePos.elbowPosition); + if (isServoAtPosition(elbow, scorePos.elbowPosition)) { + currentScoreState = Mutation.scoreState.MOVING_LIFT; + } break; - case UPGO4: - shoulder.setPosition(0.91); - - wrist.setPosition(0.6); - //setLiftHeight(0.42); + case MOVING_LIFT: + setLiftPosition(scorePos.liftPosition); + if (isServoAtPosition(leftLift, scorePos.liftPosition) || isServoAtPosition(rightLift, scorePos.liftPosition)) { + currentScoreState = Mutation.scoreState.COMPLETED; + } break; - case UPGO5: - wrist.setPosition(0.6); - shoulder.setPosition(1); - - //setLiftHeight(0.42); + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + currentScoreState = Mutation.scoreState.IDLE; + activeScorePosition = null; break; } - } - private void armmUp(){ - if (gamepad2.a) { - switch (currentArmPos) { - case INTAKE: - currentArmPos = ArmPosition.UPGO1; - break; - case UPGO1: - currentArmPos = ArmPosition.UPGO2; - break; - case UPGO2: - currentArmPos = ArmPosition.UPGO3; - break; - case UPGO3: - currentArmPos = ArmPosition.UPGO4; - break; - case UPGO4: - currentArmPos = ArmPosition.UPGO5; - break; - } - doArmm(); - telemetry.addLine(); - telemetry.addData("armpostion", currentArmPos.toString()); - sleep(100); - } - } - private void aroundthetop() { - if (gamepad2.start) { - shoulder.setPosition(0.56); - wrist.setPosition(0.5); + if (currentScoreState == Mutation.scoreState.COMPLETED) { + currentScoreState = Mutation.scoreState.IDLE; } } - private void dumpPrep() { - if (gamepad2.back) { - shoulder.setPosition(0.75); - sleep(400); - wrist.setPosition(0.93); - shoulder.setPosition(0.91); + // classes to support state machine actions + static class IntakePosition { + double shoulderPosition; + double wristPosition; + double elbowPosition; + + public IntakePosition(double shoulderPos, double wristPos, double elbowPos) { + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; } } - private void dumpPrepTwo() { - if (gamepad2.back) { + static class ScorePosition { + double shoulderPosition; + double wristPosition; + double elbowPosition; + double liftPosition; + + public ScorePosition(double shoulderPos, double wristPos, double elbowPos, double liftPos) { + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + this.liftPosition = liftPos; } } - private void SlideFunction() { + // functions to support state machine actions + private boolean isServoAtPosition(Servo servo, double position) { + return Math.abs(servo.getPosition() - position) < SERVO_TOLERANCE; } - private void dumpLeft() { - if (gamepad2.left_bumper) { + private void moveServoGradually(Servo servo, double targetPosition) { + double currentPosition = servo.getPosition(); - } - if (gamepad2.right_bumper) { + // Check if enough time has passed since the last update + if (servoTimer.seconds() > servodelaytime) { + // Determine the direction of movement + double direction = targetPosition > currentPosition ? servodelta : -servodelta; - } - } - private void driveAroundPos() { - if (gamepad2.dpad_down) { - intakePos(); - } - } - private void drivePos() { - if ((gamepad1.a) && (currentArmPos == ArmPosition.INTAKE)) { - shoulder.setPosition(0.49); - wrist.setPosition(0.55); + // Calculate the new position + servoposition = currentPosition + direction; + servoposition = Range.clip(servoposition, 0, 1); // Ensure the position is within valid range + + // Update the servo position + servo.setPosition(servoposition); - //setLiftHeight(0.42); - currentArmPos = ArmPosition.DRIVE; + // Reset the timer + servoTimer.reset(); } } - private void intakePos() { - //hopper.setPosition(0.02); - //shoulder.setPosition(0.44); - //wrist.setPosition(0.26); - wrist.setPosition(0.265); - shoulder.setPosition(0.455); - leftFinger.setPosition(0.5); - rightFinger.setPosition(0.5); - elbow.setPosition(0.5); - - //setLiftHeight(0.42); - currentArmPos = ArmPosition.INTAKE; + + // lift (has been adjusted mechanically to use the same height) + public void setLiftPosition(double targetPosition) { + // Ensure the target position is within the valid range + targetPosition = Math.max(0.0, Math.min(targetPosition, 1.0)); + + // Set the servo positions + leftLift.setPosition(targetPosition); + rightLift.setPosition(targetPosition); } - private void theJuke() { - if ((gamepad2.dpad_up || gamepad1.x) && (currentArmPos == ArmPosition.INTAKE)) { - shoulder.setPosition(0.48); - sleep(200); - intakePos(); + + // launcher + private void airplane() { + if (gamepad2.back) { + launcher.setPosition(0.1); + } else { + launcher.setPosition(0.8); } } - private void incrementalIntake() { - shoulder.setPosition(0.455); - sleep(653); - shoulder.setPosition(0.47); - wrist.setPosition(0.55); - sleep(531); - shoulder.setPosition(0.49); - wrist.setPosition(0.55); - } private void launcherstartPos() { - launcher.setPosition(0.8); + launcher.setPosition(LAUNCHER_START_POS); } - - -/* - private void driveCode() { - - double SLOW_DOWN_FACTOR; - SLOW_DOWN_FACTOR = 0.5; - telemetry.addData("Running FTC Wires (ftcwires.org) TeleOp Mode adopted for Team:","TEAM NUMBER"); - drive.setDrivePowers(new PoseVelocity2d( - new Vector2d( - -gamepad1.left_stick_y * SLOW_DOWN_FACTOR, - -gamepad1.left_stick_x * SLOW_DOWN_FACTOR - ), - -gamepad1.right_stick_x * SLOW_DOWN_FACTOR - )); - - drive.updatePoseEstimate(); - - //telemetry.addData("LF Encoder", drive.leftFront.getCurrentPosition()); - //telemetry.addData("LB Encoder", drive.leftBack.getCurrentPosition()); - //telemetry.addData("RF Encoder", drive.rightFront.getCurrentPosition()); - //telemetry.addData("RB Encoder", drive.rightBack.getCurrentPosition()); - - telemetry.addData("x", drive.pose.position.x); - telemetry.addLine("Current Pose"); - telemetry.addData("y", drive.pose.position.y); - telemetry.addData("heading", Math.toDegrees(drive.pose.heading.log())); - // telemetry.update(); - - } -*/ - - @Override // NOT loop \/ - Or int of vars public void runOpMode() throws InterruptedException { - androidTextToSpeech = new AndroidTextToSpeech(); - androidTextToSpeech.initialize(); - - // for wires driving - //drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); - - // for Og driving (the best driving) - //ogDrive = new OgDrive(hardwareMap); telemetry.addData("Status", "Initialized"); telemetry.update(); + // hardware mappings and parameters + + // motors rightFront = hardwareMap.get(DcMotor.class, "rightFront"); leftFront = hardwareMap.get(DcMotor.class, "leftFront"); rightBack = hardwareMap.get(DcMotor.class, "rightBack"); @@ -348,56 +389,52 @@ public void runOpMode() throws InterruptedException { rightBack.setDirection(DcMotor.Direction.REVERSE); leftBack.setDirection(DcMotor.Direction.FORWARD); - //Set motor modes + // motor modes rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - telemetry.addData("Status", "OdoMec2 is ready to run!"); - telemetry.update(); - - - - launcher = hardwareMap.get(Servo.class, "launcher"); - rightLift = hardwareMap.get(Servo.class, "rightLift"); - leftLift = hardwareMap.get(Servo.class, "leftLift"); + // servos + shoulder = hardwareMap.get(Servo.class, "shoulder"); wrist = hardwareMap.get(Servo.class, "wrist"); elbow = hardwareMap.get(Servo.class, "elbow"); leftFinger = hardwareMap.get(Servo.class, "lFinger"); rightFinger = hardwareMap.get(Servo.class, "rFinger"); + launcher = hardwareMap.get(Servo.class, "launcher"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); - shoulder = hardwareMap.get(Servo.class, "shoulder"); - + // servo modes shoulder.setDirection(Servo.Direction.REVERSE); wrist.setDirection(Servo.Direction.REVERSE); launcher.setDirection(Servo.Direction.REVERSE); - - // sensors leftUpper = hardwareMap.get(RevTouchSensor.class, "leftUpper"); rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); - // servos + // init positions + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + setLiftPosition(LIFT_DRIVE); + shoulder.setPosition(SHOULDER_INT); + elbow.setPosition(ELBOW_DRIVE); + wrist.setPosition(WRIST_INTAKE); launcherstartPos(); - intakePos(); - //this is a coment to mAKE git update - double SLOW_DOWN_FACTOR = 0.5; + telemetry.addData("Status", "OdoMec is ready to run!"); telemetry.addData("Initializing TeleOp",""); - telemetry.update(); - waitForStart(); runtime.reset(); - // servo_shenanigans(); - // loop real + + while(opModeIsActive()){ - //Drivetrain + + // mecanum drive double forward = gamepad1.left_stick_y; double strafe = -gamepad1.left_stick_x; double turn = -gamepad1.right_stick_x; @@ -415,68 +452,203 @@ public void runOpMode() throws InterruptedException { rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); } else { - rightFrontPower = Range.clip(rightFrontPower, -0.8, 0.8); - leftFrontPower = Range.clip(leftFrontPower, -0.8, 0.8); - rightBackPower = Range.clip(rightBackPower, -0.8, 0.8); - leftBackPower = Range.clip(leftBackPower, -0.8, 0.8); + rightFrontPower = Range.clip(rightFrontPower, -1, 1); + leftFrontPower = Range.clip(leftFrontPower, -1, 1); + rightBackPower = Range.clip(rightBackPower, -1, 1); + leftBackPower = Range.clip(leftBackPower, -1, 1); } - rightFront.setPower(rightFrontPower); leftFront.setPower(leftFrontPower); rightBack.setPower(rightBackPower); leftBack.setPower(leftBackPower); - if (gamepad1.right_bumper){ - leftHang.setDirection(DcMotor.Direction.FORWARD); - rightHang.setDirection(DcMotor.Direction.FORWARD); - - leftHang.setPower(.9); - rightHang.setPower(.9); - } else if (leftUpper.isPressed() || rightUpper.isPressed() || gamepad1.y) { + // hanging + if (gamepad1.right_trigger > TRIGGER_THRESHOLD){ + if (!leftUpper.isPressed()) { + leftHang.setDirection(DcMotor.Direction.FORWARD); + leftHang.setPower(.9); + } + if (!rightUpper.isPressed()) { + rightHang.setDirection(DcMotor.Direction.FORWARD); + rightHang.setPower(.9); + } + } else if (gamepad1.left_trigger > TRIGGER_THRESHOLD){ + if (!leftLower.isPressed()) { + leftHang.setDirection(DcMotor.Direction.REVERSE); + leftHang.setPower(.9); + } + if (!rightLower.isPressed()) { + rightHang.setDirection(DcMotor.Direction.REVERSE); + rightHang.setPower(.9); + } + } else { leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); leftHang.setPower(0); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); rightHang.setPower(0); } - if (gamepad1.left_bumper){ - leftHang.setDirection(DcMotor.Direction.REVERSE); - rightHang.setDirection(DcMotor.Direction.REVERSE); - leftHang.setPower(.9); - rightHang.setPower(.9); - } else if (leftLower.isPressed() || rightLower.isPressed() || gamepad1.y) { + + // emergency stop + if (gamepad1.y) { leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); leftHang.setPower(0); rightHang.setPower(0); } + // claw drive position + //TODO: add safety, this is drive around pos + if (gamepad1.right_bumper && currentDriveState == Mutation.driveState.IDLE) { + currentDriveState = Mutation.driveState.MOVING_SHOULDER; + } + handleDriveSequence(); + + // intake + // claw intake from floor + //TODO: add saftey, this is intake from stuff + if (gamepad1.left_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + activeIntakePosition = new Mutation.IntakePosition(SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE); + currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; + } + // claw intake the top 2 from a stack of 5 + if (gamepad1.b && currentIntakeState == Mutation.intakeState.IDLE) { + activeIntakePosition = new Mutation.IntakePosition(SHOULDER_TOP_TWO, WRIST_TOP_TWO, ELBOW_TOP_TWO); + currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; + } + // claw intake the next 2 from a stack of 3 + if (gamepad1.a && currentIntakeState == Mutation.intakeState.IDLE) { + activeIntakePosition = new Mutation.IntakePosition(SHOULDER_NEXT_TWO, WRIST_NEXT_TWO, ELBOW_NEXT_TWO); + currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; + } + if (activeIntakePosition != null) { + handleIntakeSequence(activeIntakePosition); + } + if (currentIntakeState == Mutation.intakeState.COMPLETED) { + currentIntakeState = Mutation.intakeState.IDLE; + activeIntakePosition = null; // Reset the active position + } + + // scoring + // score position one + if (gamepad2.y && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, SCORE_ONE_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position two + if (gamepad2.b && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, SCORE_TWO_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position three + if (gamepad2.a && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_THREE_SHOULDER, SCORE_THREE_WRIST, SCORE_THREE_ELBOW, SCORE_THREE_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position four + //TODO: gotta put !gamepad2.left_bumper above + if (gamepad2.x && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, SCORE_FOUR_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position five + if ((gamepad2.left_bumper && gamepad2.y) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_FIVE_SHOULDER, SCORE_FIVE_WRIST, SCORE_FIVE_ELBOW, SCORE_FIVE_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position six + if ((gamepad2.left_bumper && gamepad2.b) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_SIX_SHOULDER, SCORE_SIX_WRIST, SCORE_SIX_ELBOW, SCORE_SIX_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position seven + if ((gamepad2.left_bumper && gamepad2.a) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_SEVEN_SHOULDER, SCORE_SEVEN_WRIST, SCORE_SEVEN_ELBOW, SCORE_SEVEN_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position eight + if ((gamepad2.left_bumper && gamepad2.x) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_EIGHT_SHOULDER, SCORE_EIGHT_WRIST, SCORE_EIGHT_ELBOW, SCORE_EIGHT_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position nine + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.y) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_NINE_SHOULDER, SCORE_NINE_WRIST, SCORE_NINE_ELBOW, SCORE_NINE_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position ten + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.b) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_TEN_SHOULDER, SCORE_TEN_WRIST, SCORE_TEN_ELBOW, SCORE_TEN_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position eleven + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.a) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_ELBOW, SCORE_ELEVEN_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + if (activeScorePosition != null) { + handleScorePosSequence(activeScorePosition); + } + if (currentScoreState == Mutation.scoreState.COMPLETED) { + currentScoreState = Mutation.scoreState.IDLE; + activeScorePosition = null; // Reset the active position + } + // dropping on the backboard for scoring + if (gamepad2.dpad_up && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_left && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + } else if (gamepad2.dpad_right && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_down && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + // grabbing off the floor or stack + if (gamepad2.right_bumper && gamepad2.dpad_up && (currentScoreState == Mutation.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentScoreState == Mutation.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentScoreState == Mutation.scoreState.IDLE)) { + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentScoreState == Mutation.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + // telemetry telemetry.addData("Status", "Run " + runtime.toString()); telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); + telemetry.addData("Intake", currentIntakeState); + telemetry.addData("Drive", currentDriveState); + telemetry.addData("Score", currentScoreState); + + //telemetry.addData("Left Lower", leftLower.isPressed() ? "Pressed" : "Not Pressed"); + //telemetry.addData("Left Upper", leftUpper.isPressed() ? "Pressed" : "Not Pressed"); + //telemetry.addData("Right Lower", rightLower.isPressed() ? "Pressed" : "Not Pressed"); + //telemetry.addData("Right Upper", rightUpper.isPressed() ? "Pressed" : "Not Pressed"); - //Code(); + // launcher airplane(); - //ogDrive.og_drive_code(gamepad1, telemetry); - //IsDrive.is_drive_code(gamepad1, telemetry); - driveAroundPos(); - //dumpPrepTwo(); - //homePrep(); - drivePos(); - dumpLeft(); - theJuke(); - //liftFunction(); - //worm(); - //stopMotion(); - //aroundthetop(); - armmUp(); - armmDown(); - SlideFunction(); + telemetry.update(); - sleep(100); } + } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MutationOld.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MutationOld.java new file mode 100644 index 0000000000..6e8ff36946 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MutationOld.java @@ -0,0 +1,482 @@ +package org.firstinspires.ftc.teamcode; + +// imports + +//import com.acmerobotics.roadrunner.Pose2d; +//import com.acmerobotics.roadrunner.PoseVelocity2d; +//import com.acmerobotics.roadrunner.Vector2d; + +import com.qualcomm.hardware.rev.RevTouchSensor; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; + +@TeleOp +public class MutationOld extends LinearOpMode { + // Declare vars + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; + private Servo launcher; + private Servo rightLift; + private Servo leftLift; + private Servo shoulder; + private Servo wrist; + private Servo elbow; + private Servo leftFinger; + private Servo rightFinger; + + //MecanumDrive drive; + //OgDrive ogDrive; + private ElapsedTime runtime = new ElapsedTime(); + //Motors + //Drivetrain + private DcMotor rightFront; //front right 0 + private DcMotor leftFront; //front left 2 + private DcMotor rightBack; //rear right 1 + private DcMotor leftBack; //rear left 3 + private DcMotor leftHang; + private DcMotor rightHang; + + + // Servo prep + private static enum ArmPosition { + INTAKE, + DRIVE, + UPGO1, + UPGO2, + UPGO3, + UPGO4, + UPGO5; + } + + private ArmPosition currentArmPos; + double LiftLeftOffset = .04; + double LiftHeight; + private AndroidTextToSpeech androidTextToSpeech; + + + // Functions \/ + + + private void servo_shenanigans() { + setLiftHeight(0.4); + sleep(10000); + setLiftHeight(0.05); + } + + private void setLiftHeight(double inputLiftHeight) { + if (inputLiftHeight < 0.42) { + inputLiftHeight = 0.42; + } + if (inputLiftHeight > 1) { + inputLiftHeight = 1; + } + LiftHeight = inputLiftHeight; + leftLift.setPosition(LiftLeftOffset + LiftHeight); + rightLift.setPosition(LiftHeight); + + } + + private void worm() { + if (gamepad2.left_bumper) { + shoulder.setPosition(0.445); + wrist.setPosition(0.26); + } + if (gamepad2.right_bumper) { + shoulder.setPosition(0.92); + } + } + + private void airplane() { + if (gamepad2.back) { + launcher.setPosition(0.1); + } else { + launcher.setPosition(0.8); + } + } + + private void liftFunction() { + if (gamepad2.y) { + shoulder.setPosition(0.79); + setLiftHeight(0.42); + + wrist.setPosition(0.2); + sleep(600); + shoulder.setPosition(0.60); + sleep(600); + shoulder.setPosition(0.50); + sleep(600); + intakePos(); + } else if (gamepad2.x) { + setLiftHeight(0.71); + } else if (gamepad2.b) { + setLiftHeight(0.56); + } + } + private void armmDown() { + if (gamepad2.y) { + switch (currentArmPos) { + case UPGO1: + currentArmPos = ArmPosition.INTAKE; + break; + case UPGO2: + currentArmPos = ArmPosition.UPGO1; + break; + case UPGO3: + currentArmPos = ArmPosition.UPGO2; + break; + case UPGO4: + currentArmPos = ArmPosition.UPGO3; + break; + case UPGO5: + currentArmPos = ArmPosition.UPGO4; + break; + } + doArmm(); + telemetry.addLine(); + telemetry.addData("armpostion", currentArmPos.toString()); + sleep(100); + } + } + private void doArmm() { + switch (currentArmPos) { + case INTAKE: + intakePos(); + break; + case UPGO1: + shoulder.setPosition(0.55); + + wrist.setPosition(0.28); + //setLiftHeight(0.42); + break; + case UPGO2: + wrist.setPosition(0.22); + shoulder.setPosition(0.79); + + //setLiftHeight(0.42); + break; + case UPGO3: + + wrist.setPosition(0.6); + shoulder.setPosition(0.79); + //setLiftHeight(0.42); + break; + case UPGO4: + shoulder.setPosition(0.91); + + wrist.setPosition(0.6); + //setLiftHeight(0.42); + break; + case UPGO5: + wrist.setPosition(0.6); + shoulder.setPosition(1); + + //setLiftHeight(0.42); + break; + } + } + private void armmUp(){ + if (gamepad2.a) { + switch (currentArmPos) { + case INTAKE: + currentArmPos = ArmPosition.UPGO1; + break; + case UPGO1: + currentArmPos = ArmPosition.UPGO2; + break; + case UPGO2: + currentArmPos = ArmPosition.UPGO3; + break; + case UPGO3: + currentArmPos = ArmPosition.UPGO4; + break; + case UPGO4: + currentArmPos = ArmPosition.UPGO5; + break; + } + doArmm(); + telemetry.addLine(); + telemetry.addData("armpostion", currentArmPos.toString()); + sleep(100); + } + } + private void aroundthetop() { + if (gamepad2.start) { + shoulder.setPosition(0.56); + wrist.setPosition(0.5); + + } + } + private void dumpPrep() { + if (gamepad2.back) { + shoulder.setPosition(0.75); + sleep(400); + + wrist.setPosition(0.93); + shoulder.setPosition(0.91); + } + } + private void dumpPrepTwo() { + if (gamepad2.back) { + + } + } + private void SlideFunction() { + + } + + private void dumpLeft() { + if (gamepad2.left_bumper) { + + } + if (gamepad2.right_bumper) { + + } + } + private void driveAroundPos() { + if (gamepad2.dpad_down) { + intakePos(); + } + } + private void drivePos() { + if ((gamepad1.a) && (currentArmPos == ArmPosition.INTAKE)) { + shoulder.setPosition(0.49); + wrist.setPosition(0.55); + + //setLiftHeight(0.42); + currentArmPos = ArmPosition.DRIVE; + } + } + private void intakePos() { + //hopper.setPosition(0.02); + //shoulder.setPosition(0.44); + //wrist.setPosition(0.26); + wrist.setPosition(0.265); + shoulder.setPosition(0.455); + leftFinger.setPosition(0.5); + rightFinger.setPosition(0.5); + elbow.setPosition(0.5); + + //setLiftHeight(0.42); + currentArmPos = ArmPosition.INTAKE; + } + private void theJuke() { + if ((gamepad2.dpad_up || gamepad1.x) && (currentArmPos == ArmPosition.INTAKE)) { + shoulder.setPosition(0.48); + sleep(200); + intakePos(); + } + } + private void incrementalIntake() { + shoulder.setPosition(0.455); + sleep(653); + shoulder.setPosition(0.47); + wrist.setPosition(0.55); + sleep(531); + shoulder.setPosition(0.49); + wrist.setPosition(0.55); + } + private void launcherstartPos() { + launcher.setPosition(0.8); + } + + + +/* + private void driveCode() { + + double SLOW_DOWN_FACTOR; + SLOW_DOWN_FACTOR = 0.5; + telemetry.addData("Running FTC Wires (ftcwires.org) TeleOp Mode adopted for Team:","TEAM NUMBER"); + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y * SLOW_DOWN_FACTOR, + -gamepad1.left_stick_x * SLOW_DOWN_FACTOR + ), + -gamepad1.right_stick_x * SLOW_DOWN_FACTOR + )); + + drive.updatePoseEstimate(); + + //telemetry.addData("LF Encoder", drive.leftFront.getCurrentPosition()); + //telemetry.addData("LB Encoder", drive.leftBack.getCurrentPosition()); + //telemetry.addData("RF Encoder", drive.rightFront.getCurrentPosition()); + //telemetry.addData("RB Encoder", drive.rightBack.getCurrentPosition()); + + telemetry.addData("x", drive.pose.position.x); + telemetry.addLine("Current Pose"); + telemetry.addData("y", drive.pose.position.y); + telemetry.addData("heading", Math.toDegrees(drive.pose.heading.log())); + // telemetry.update(); + + } +*/ + + + @Override + // NOT loop \/ - Or int of vars + public void runOpMode() throws InterruptedException { + + androidTextToSpeech = new AndroidTextToSpeech(); + androidTextToSpeech.initialize(); + + // for wires driving + //drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + // for Og driving (the best driving) + //ogDrive = new OgDrive(hardwareMap); + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + rightFront = hardwareMap.get(DcMotor.class, "rightFront"); + leftFront = hardwareMap.get(DcMotor.class, "leftFront"); + rightBack = hardwareMap.get(DcMotor.class, "rightBack"); + leftBack = hardwareMap.get(DcMotor.class, "leftBack"); + + leftHang = hardwareMap.get(DcMotor.class, "leftHang"); + rightHang = hardwareMap.get(DcMotor.class, "rightHang"); + + rightFront.setDirection(DcMotor.Direction.REVERSE); + leftFront.setDirection(DcMotor.Direction.FORWARD); + rightBack.setDirection(DcMotor.Direction.REVERSE); + leftBack.setDirection(DcMotor.Direction.FORWARD); + + //Set motor modes + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + + telemetry.addData("Status", "OdoMec2 is ready to run!"); + telemetry.update(); + + + + launcher = hardwareMap.get(Servo.class, "launcher"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + wrist = hardwareMap.get(Servo.class, "wrist"); + elbow = hardwareMap.get(Servo.class, "elbow"); + leftFinger = hardwareMap.get(Servo.class, "lFinger"); + rightFinger = hardwareMap.get(Servo.class, "rFinger"); + + shoulder = hardwareMap.get(Servo.class, "shoulder"); + + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + launcher.setDirection(Servo.Direction.REVERSE); + + + + // sensors + leftUpper = hardwareMap.get(RevTouchSensor.class, "leftUpper"); + rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); + leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); + rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); + + // servos + launcherstartPos(); + intakePos(); + + //this is a coment to mAKE git update + double SLOW_DOWN_FACTOR = 0.5; + telemetry.addData("Initializing TeleOp",""); + telemetry.update(); + + + waitForStart(); + runtime.reset(); + // servo_shenanigans(); + // loop real + while(opModeIsActive()){ + //Drivetrain + double forward = gamepad1.left_stick_y; + double strafe = -gamepad1.left_stick_x; + double turn = -gamepad1.right_stick_x; + + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); + + double rightFrontPower = (forward - strafe - turn) / denominator; + double leftFrontPower = (forward + strafe + turn) / denominator; + double rightBackPower = (forward + strafe - turn) / denominator; + double leftBackPower = (forward - strafe + turn) / denominator; + + if (gamepad1.left_bumper) { + rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); + leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); + rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); + leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); + } else { + rightFrontPower = Range.clip(rightFrontPower, -0.8, 0.8); + leftFrontPower = Range.clip(leftFrontPower, -0.8, 0.8); + rightBackPower = Range.clip(rightBackPower, -0.8, 0.8); + leftBackPower = Range.clip(leftBackPower, -0.8, 0.8); + } + + + rightFront.setPower(rightFrontPower); + leftFront.setPower(leftFrontPower); + rightBack.setPower(rightBackPower); + leftBack.setPower(leftBackPower); + + if (gamepad1.right_bumper){ + leftHang.setDirection(DcMotor.Direction.FORWARD); + rightHang.setDirection(DcMotor.Direction.FORWARD); + + leftHang.setPower(.9); + rightHang.setPower(.9); + } else if (leftUpper.isPressed() || rightUpper.isPressed() || gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); + } + if (gamepad1.left_bumper){ + leftHang.setDirection(DcMotor.Direction.REVERSE); + rightHang.setDirection(DcMotor.Direction.REVERSE); + + leftHang.setPower(.9); + rightHang.setPower(.9); + } else if (leftLower.isPressed() || rightLower.isPressed() || gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); + } + + + + + telemetry.addData("Status", "Run " + runtime.toString()); + telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); + + //Code(); + airplane(); + //ogDrive.og_drive_code(gamepad1, telemetry); + //IsDrive.is_drive_code(gamepad1, telemetry); + driveAroundPos(); + //dumpPrepTwo(); + //homePrep(); + drivePos(); + dumpLeft(); + theJuke(); + //liftFunction(); + //worm(); + //stopMotion(); + //aroundthetop(); + armmUp(); + armmDown(); + SlideFunction(); + telemetry.update(); + sleep(100); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java deleted file mode 100644 index e837093b97..0000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ /dev/null @@ -1,655 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.hardware.rev.RevTouchSensor; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.Range; - -@TeleOp -public class OdoMec extends LinearOpMode { - - // Declare vars - // timers - private ElapsedTime runtime = new ElapsedTime(); - private ElapsedTime servoTimer = new ElapsedTime(); - - // motors - private DcMotor rightFront; //front right 0 - private DcMotor leftFront; //front left 2 - private DcMotor rightBack; //rear right 1 - private DcMotor leftBack; //rear left 3 - private DcMotor leftHang; - private DcMotor rightHang; - - // servos - private Servo launcher; - private Servo shoulder; - private Servo wrist; - private Servo elbow; - private Servo leftFinger; - private Servo rightFinger; - private Servo rightLift; - private Servo leftLift; - double LiftLeftOffset = -.05; - double LiftHeight; - private double servoposition = 0.0; - private double servodelta = 0.02; - private double servodelaytime = 0.03; - - - // servo values - private static final double SHOULDER_DRIVE = 0.425; // 0.425 - private static final double SHOULDER_INT = 0.43; // 0.455 - private static final double ELBOW_DRIVE= 0.5; - private static final double ELBOW_INTAKE = 0.8; - private static final double WRIST_INT = 0.55; // int for auto & testing 0.55; // int for DRIVE 0.265 - private static final double WRIST_DRIVE = 0.265; // int for auto & testing 0.55; // int for DRIVE 0.265 - private static final double WRIST_INTAKE = 0.575; - private static final double LEFT_FINGER_GRIP = 0.74; - private static final double LEFT_FINGER_DROP = 0.5; - private static final double LEFT_FINGER_INTAKE = 0.34; - private static final double RIGHT_FINGER_GRIP = .27; - private static final double RIGHT_FINGER_DROP = .5; - private static final double RIGHT_FINGER_INTAKE = 0.64; - private static final double TRIGGER_THRESHOLD = 0.5; - private static final double LAUNCHER_START_POS = 0.8; - private static final double SERVO_TOLERANCE = 0.01; - private static final double LIFT_DRIVE = 0.37; - private double liftTargetPosition = LIFT_DRIVE; - - // stack positions (top 2 o 5 and next 2 of 3 ) - // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin - // intake two off a stack of five - private static final double SHOULDER_TOP_TWO = SHOULDER_DRIVE; - private static final double WRIST_TOP_TWO = 0.55; - private static final double ELBOW_TOP_TWO = 0.6; - - // intake two off a stack of three - private static final double SHOULDER_NEXT_TWO = SHOULDER_DRIVE; - private static final double WRIST_NEXT_TWO = 0.44; - private static final double ELBOW_NEXT_TWO = 0.7; - - //=^-^= - // score positions (11 rows on the board) - // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin - // score position one button map (gamepad2.y) - private static final double SCORE_ONE_SHOULDER = 0.936; - private static final double SCORE_ONE_WRIST = 0.685; - private static final double SCORE_ONE_ELBOW = 0.52; - private static final double SCORE_ONE_LIFT = LIFT_DRIVE; - // score position two button map (gamepad2.b) - private static final double SCORE_TWO_SHOULDER = 0.9; - private static final double SCORE_TWO_WRIST = 0.745; - private static final double SCORE_TWO_ELBOW = 0.55; - private static final double SCORE_TWO_LIFT = LIFT_DRIVE; - // score position three button map (gamepad2.a) - private static final double SCORE_THREE_SHOULDER = 0.92; - private static final double SCORE_THREE_WRIST = 0.91; - private static final double SCORE_THREE_ELBOW = 0.67; - private static final double SCORE_THREE_LIFT = LIFT_DRIVE; - // score position four button map (gamepad2.x) - private static final double SCORE_FOUR_SHOULDER = 0.91; - private static final double SCORE_FOUR_WRIST =0.985; - private static final double SCORE_FOUR_ELBOW = 0.72; - private static final double SCORE_FOUR_LIFT = LIFT_DRIVE; - // score position five button map (gamepad2.left_bumper && gamepad2.y) - private static final double SCORE_FIVE_SHOULDER = 0.93; - private static final double SCORE_FIVE_WRIST = 1; - private static final double SCORE_FIVE_ELBOW = 0.82; - private static final double SCORE_FIVE_LIFT = LIFT_DRIVE; - // score position six button map (gamepad2.left_bumper && gamepad2.b) - private static final double SCORE_SIX_SHOULDER = 0.92; - private static final double SCORE_SIX_WRIST = 1; - private static final double SCORE_SIX_ELBOW = 0.85; - private static final double SCORE_SIX_LIFT = LIFT_DRIVE; - // score position seven button map (gamepad2.left_bumper && gamepad2.a) - private static final double SCORE_SEVEN_SHOULDER = 0.9; - private static final double SCORE_SEVEN_WRIST = 1; - private static final double SCORE_SEVEN_ELBOW = 0.81; - private static final double SCORE_SEVEN_LIFT = 0.51; - // score position eight button map (gamepad2.left_bumper && gamepad2.x) - private static final double SCORE_EIGHT_SHOULDER = 0.94; - private static final double SCORE_EIGHT_WRIST = 0.93; - private static final double SCORE_EIGHT_ELBOW = 0.8; - private static final double SCORE_EIGHT_LIFT = 0.8; - // score position nine button map (gamepad2.left_trigger && gamepad2.y) - private static final double SCORE_NINE_SHOULDER = 0.93; - private static final double SCORE_NINE_WRIST = 0.93; - private static final double SCORE_NINE_ELBOW = 0.79; - private static final double SCORE_NINE_LIFT = 0.86; - // score position ten button map (gamepad2.left_trigger && gamepad2.b) - private static final double SCORE_TEN_SHOULDER = 0.9; - private static final double SCORE_TEN_WRIST = 0.93; - private static final double SCORE_TEN_ELBOW = 0.76; - private static final double SCORE_TEN_LIFT = 0.98; - // score position eleven button map (gamepad2.left_trigger && gamepad2.a) - private static final double SCORE_ELEVEN_SHOULDER = 0.9; - private static final double SCORE_ELEVEN_WRIST = 0.93; - private static final double SCORE_ELEVEN_ELBOW = 0.76; - private static final double SCORE_ELEVEN_LIFT = 0.98; - - // sensors - private RevTouchSensor rightUpper; - private RevTouchSensor leftUpper; - private RevTouchSensor rightLower; - private RevTouchSensor leftLower; - - - // state machines enumeration - // intake - private enum intakeState { - IDLE, - MOVING_SHOULDER, - MOVING_WRIST, - MOVING_ELBOW, - COMPLETED - } - private OdoMec.intakeState currentIntakeState = OdoMec.intakeState.IDLE; - private OdoMec.IntakePosition activeIntakePosition = null; - // drive - private enum driveState { - IDLE, - MOVING_SHOULDER, - MOVING_ELBOW, - MOVING_WRIST, - COMPLETED - } - - private OdoMec.driveState currentDriveState = OdoMec.driveState.IDLE; - // score - private enum scoreState { - IDLE, - MOVING_SHOULDER, - MOVING_WRIST, - MOVING_ELBOW, - MOVING_LIFT, - COMPLETED - } - - private OdoMec.scoreState currentScoreState = OdoMec.scoreState.IDLE; - private OdoMec.ScorePosition activeScorePosition = null; - - // state machine sequences to provide cascading actions with a single input (button press) - // intake - private void handleIntakeSequence(OdoMec.IntakePosition intakePos) { - switch (currentIntakeState) { - case MOVING_SHOULDER: - // Move the shoulder to intake position - moveServoGradually(shoulder, intakePos.shoulderPosition + .05); - shoulder.setPosition(intakePos.shoulderPosition); - if (isServoAtPosition(shoulder, intakePos.shoulderPosition)) { - currentIntakeState = OdoMec.intakeState.MOVING_WRIST; - } - break; - case MOVING_WRIST: - // Move the wrist to intake position - wrist.setPosition(intakePos.wristPosition); - if (isServoAtPosition(wrist, intakePos.wristPosition)) { - currentIntakeState = intakeState.MOVING_ELBOW; - } - break; - case MOVING_ELBOW: - // Move the elbow to intake position so it don't slap the floor - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - moveServoGradually(elbow, intakePos.elbowPosition); - if (isServoAtPosition(elbow, intakePos.elbowPosition)) { - currentIntakeState = intakeState.COMPLETED; - } - break; - case COMPLETED: - // Sequence complete, reset the state or perform additional actions - break; - } - // Check to reset the state to IDLE outside the switch - if (currentIntakeState == OdoMec.intakeState.COMPLETED) { - currentIntakeState = OdoMec.intakeState.IDLE; - } - } - - // drive - private void handleDriveSequence() { - switch (currentDriveState) { - case MOVING_SHOULDER: - // Move the shoulder to drive position - moveServoGradually(shoulder, SHOULDER_DRIVE + .05); - shoulder.setPosition(SHOULDER_DRIVE); - if (isServoAtPosition(shoulder, SHOULDER_DRIVE)) { - currentDriveState = OdoMec.driveState.MOVING_ELBOW; - } - break; - case MOVING_ELBOW: - // Move the elbow to drive position - leftFinger.setPosition(LEFT_FINGER_GRIP); - rightFinger.setPosition(RIGHT_FINGER_GRIP); - elbow.setPosition(ELBOW_DRIVE); - if (isServoAtPosition(elbow, ELBOW_DRIVE)) { - currentDriveState = OdoMec.driveState.MOVING_WRIST; - } - break; - case MOVING_WRIST: - // Move the wrist to drive position - wrist.setPosition(WRIST_DRIVE); - if (isServoAtPosition(wrist, WRIST_DRIVE)) { - currentDriveState = OdoMec.driveState.COMPLETED; - } - break; - case COMPLETED: - // Sequence complete, reset the state or perform additional actions - break; - } - // Check to reset the state to IDLE outside the switch - if (currentDriveState == OdoMec.driveState.COMPLETED) { - currentDriveState = OdoMec.driveState.IDLE; - } - } - - // score - private void handleScorePosSequence(OdoMec.ScorePosition scorePos) { - switch (currentScoreState) { - case MOVING_SHOULDER: - // Move the shoulder to the specified position - moveServoGradually(shoulder, scorePos.shoulderPosition - .05); - shoulder.setPosition(scorePos.shoulderPosition); - if (isServoAtPosition(shoulder, scorePos.shoulderPosition)) { - currentScoreState = OdoMec.scoreState.MOVING_WRIST; - } - break; - case MOVING_WRIST: - // Move the wrist to the specified position - wrist.setPosition(scorePos.wristPosition); - if (isServoAtPosition(wrist, scorePos.wristPosition)) { - currentScoreState = OdoMec.scoreState.MOVING_ELBOW; - } - break; - case MOVING_ELBOW: - // Move the elbow to the specified position - leftFinger.setPosition(LEFT_FINGER_GRIP); - rightFinger.setPosition(RIGHT_FINGER_GRIP); - elbow.setPosition(scorePos.elbowPosition); - if (isServoAtPosition(elbow, scorePos.elbowPosition)) { - currentScoreState = OdoMec.scoreState.MOVING_LIFT; - } - break; - case MOVING_LIFT: - setLiftPosition(scorePos.liftPosition); - if (isServoAtPosition(leftLift, scorePos.liftPosition) || isServoAtPosition(rightLift, scorePos.liftPosition)) { - currentScoreState = OdoMec.scoreState.COMPLETED; - } - break; - case COMPLETED: - // Sequence complete, reset the state or perform additional actions - currentScoreState = OdoMec.scoreState.IDLE; - activeScorePosition = null; - break; - } - - if (currentScoreState == OdoMec.scoreState.COMPLETED) { - currentScoreState = OdoMec.scoreState.IDLE; - } - } - - // classes to support state machine actions - static class IntakePosition { - double shoulderPosition; - double wristPosition; - double elbowPosition; - - public IntakePosition(double shoulderPos, double wristPos, double elbowPos) { - this.shoulderPosition = shoulderPos; - this.wristPosition = wristPos; - this.elbowPosition = elbowPos; - } - } - - static class ScorePosition { - double shoulderPosition; - double wristPosition; - double elbowPosition; - double liftPosition; - - public ScorePosition(double shoulderPos, double wristPos, double elbowPos, double liftPos) { - this.shoulderPosition = shoulderPos; - this.wristPosition = wristPos; - this.elbowPosition = elbowPos; - this.liftPosition = liftPos; - } - } - - // functions to support state machine actions - private boolean isServoAtPosition(Servo servo, double position) { - return Math.abs(servo.getPosition() - position) < SERVO_TOLERANCE; - } - - private void moveServoGradually(Servo servo, double targetPosition) { - double currentPosition = servo.getPosition(); - - // Check if enough time has passed since the last update - if (servoTimer.seconds() > servodelaytime) { - // Determine the direction of movement - double direction = targetPosition > currentPosition ? servodelta : -servodelta; - - // Calculate the new position - servoposition = currentPosition + direction; - servoposition = Range.clip(servoposition, 0, 1); // Ensure the position is within valid range - - // Update the servo position - servo.setPosition(servoposition); - - // Reset the timer - servoTimer.reset(); - } - } - - // lift (has been adjusted mechanically to use the same height) - public void setLiftPosition(double targetPosition) { - // Ensure the target position is within the valid range - targetPosition = Math.max(0.0, Math.min(targetPosition, 1.0)); - - // Set the servo positions - leftLift.setPosition(targetPosition); - rightLift.setPosition(targetPosition); - } - - // launcher - private void airplane() { - if (gamepad2.back) { - launcher.setPosition(0.1); - } else { - launcher.setPosition(0.8); - } - } - private void launcherstartPos() { - launcher.setPosition(LAUNCHER_START_POS); - } - - @Override - // NOT loop \/ - Or int of vars - public void runOpMode() throws InterruptedException { - - telemetry.addData("Status", "Initialized"); - telemetry.update(); - - // hardware mappings and parameters - - // motors - rightFront = hardwareMap.get(DcMotor.class, "rightFront"); - leftFront = hardwareMap.get(DcMotor.class, "leftFront"); - rightBack = hardwareMap.get(DcMotor.class, "rightBack"); - leftBack = hardwareMap.get(DcMotor.class, "leftBack"); - - leftHang = hardwareMap.get(DcMotor.class, "leftHang"); - rightHang = hardwareMap.get(DcMotor.class, "rightHang"); - - rightFront.setDirection(DcMotor.Direction.REVERSE); - leftFront.setDirection(DcMotor.Direction.FORWARD); - rightBack.setDirection(DcMotor.Direction.REVERSE); - leftBack.setDirection(DcMotor.Direction.FORWARD); - - // motor modes - rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - // servos - shoulder = hardwareMap.get(Servo.class, "shoulder"); - wrist = hardwareMap.get(Servo.class, "wrist"); - elbow = hardwareMap.get(Servo.class, "elbow"); - leftFinger = hardwareMap.get(Servo.class, "lFinger"); - rightFinger = hardwareMap.get(Servo.class, "rFinger"); - launcher = hardwareMap.get(Servo.class, "launcher"); - rightLift = hardwareMap.get(Servo.class, "rightLift"); - leftLift = hardwareMap.get(Servo.class, "leftLift"); - - // servo modes - shoulder.setDirection(Servo.Direction.REVERSE); - wrist.setDirection(Servo.Direction.REVERSE); - launcher.setDirection(Servo.Direction.REVERSE); - - // sensors - leftUpper = hardwareMap.get(RevTouchSensor.class, "leftUpper"); - rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); - leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); - rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); - - // init positions - leftFinger.setPosition(LEFT_FINGER_GRIP); - rightFinger.setPosition(RIGHT_FINGER_GRIP); - setLiftPosition(LIFT_DRIVE); - shoulder.setPosition(SHOULDER_INT); - elbow.setPosition(ELBOW_DRIVE); - wrist.setPosition(WRIST_INTAKE); - launcherstartPos(); - - telemetry.addData("Status", "OdoMec is ready to run!"); - telemetry.addData("Initializing TeleOp",""); - - waitForStart(); - runtime.reset(); - - - while(opModeIsActive()){ - - // mecanum drive - double forward = gamepad1.left_stick_y; - double strafe = -gamepad1.left_stick_x; - double turn = -gamepad1.right_stick_x; - - double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); - - double rightFrontPower = (forward - strafe - turn) / denominator; - double leftFrontPower = (forward + strafe + turn) / denominator; - double rightBackPower = (forward + strafe - turn) / denominator; - double leftBackPower = (forward - strafe + turn) / denominator; - - if (gamepad1.left_bumper) { - rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); - leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); - rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); - leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); - } else { - rightFrontPower = Range.clip(rightFrontPower, -1, 1); - leftFrontPower = Range.clip(leftFrontPower, -1, 1); - rightBackPower = Range.clip(rightBackPower, -1, 1); - leftBackPower = Range.clip(leftBackPower, -1, 1); - } - - rightFront.setPower(rightFrontPower); - leftFront.setPower(leftFrontPower); - rightBack.setPower(rightBackPower); - leftBack.setPower(leftBackPower); - - // hanging - if (gamepad1.right_trigger > TRIGGER_THRESHOLD){ - if (!leftUpper.isPressed()) { - leftHang.setDirection(DcMotor.Direction.FORWARD); - leftHang.setPower(.9); - } - if (!rightUpper.isPressed()) { - rightHang.setDirection(DcMotor.Direction.FORWARD); - rightHang.setPower(.9); - } - } else if (gamepad1.left_trigger > TRIGGER_THRESHOLD){ - if (!leftLower.isPressed()) { - leftHang.setDirection(DcMotor.Direction.REVERSE); - leftHang.setPower(.9); - } - if (!rightLower.isPressed()) { - rightHang.setDirection(DcMotor.Direction.REVERSE); - rightHang.setPower(.9); - } - } else { - leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - leftHang.setPower(0); - rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightHang.setPower(0); - } - - - // emergency stop - if (gamepad1.y) { - leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - leftHang.setPower(0); - rightHang.setPower(0); - } - - // claw drive position - //TODO: add safety, this is drive around pos - if (gamepad1.right_bumper && currentDriveState == OdoMec.driveState.IDLE) { - currentDriveState = OdoMec.driveState.MOVING_SHOULDER; - } - handleDriveSequence(); - - // intake - // claw intake from floor - //TODO: add saftey, this is intake from stuff - if (gamepad1.left_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { - activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE); - currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; - } - // claw intake the top 2 from a stack of 5 - if (gamepad1.b && currentIntakeState == OdoMec.intakeState.IDLE) { - activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_TOP_TWO, WRIST_TOP_TWO, ELBOW_TOP_TWO); - currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; - } - // claw intake the next 2 from a stack of 3 - if (gamepad1.a && currentIntakeState == OdoMec.intakeState.IDLE) { - activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_NEXT_TWO, WRIST_NEXT_TWO, ELBOW_NEXT_TWO); - currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; - } - if (activeIntakePosition != null) { - handleIntakeSequence(activeIntakePosition); - } - if (currentIntakeState == OdoMec.intakeState.COMPLETED) { - currentIntakeState = OdoMec.intakeState.IDLE; - activeIntakePosition = null; // Reset the active position - } - - // scoring - // score position one - if (gamepad2.y && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, SCORE_ONE_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position two - if (gamepad2.b && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, SCORE_TWO_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position three - if (gamepad2.a && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_THREE_SHOULDER, SCORE_THREE_WRIST, SCORE_THREE_ELBOW, SCORE_THREE_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position four - //TODO: gotta put !gamepad2.left_bumper above - if (gamepad2.x && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, SCORE_FOUR_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position five - if ((gamepad2.left_bumper && gamepad2.y) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_FIVE_SHOULDER, SCORE_FIVE_WRIST, SCORE_FIVE_ELBOW, SCORE_FIVE_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position six - if ((gamepad2.left_bumper && gamepad2.b) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_SIX_SHOULDER, SCORE_SIX_WRIST, SCORE_SIX_ELBOW, SCORE_SIX_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position seven - if ((gamepad2.left_bumper && gamepad2.a) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_SEVEN_SHOULDER, SCORE_SEVEN_WRIST, SCORE_SEVEN_ELBOW, SCORE_SEVEN_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position eight - if ((gamepad2.left_bumper && gamepad2.x) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_EIGHT_SHOULDER, SCORE_EIGHT_WRIST, SCORE_EIGHT_ELBOW, SCORE_EIGHT_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position nine - if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.y) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_NINE_SHOULDER, SCORE_NINE_WRIST, SCORE_NINE_ELBOW, SCORE_NINE_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position ten - if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.b) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_TEN_SHOULDER, SCORE_TEN_WRIST, SCORE_TEN_ELBOW, SCORE_TEN_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position eleven - if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.a) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_ELBOW, SCORE_ELEVEN_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - if (activeScorePosition != null) { - handleScorePosSequence(activeScorePosition); - } - if (currentScoreState == OdoMec.scoreState.COMPLETED) { - currentScoreState = OdoMec.scoreState.IDLE; - activeScorePosition = null; // Reset the active position - } - - // dropping on the backboard for scoring - if (gamepad2.dpad_up && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_DROP); - rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_left && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_DROP); - } else if (gamepad2.dpad_right && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { - rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_down && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - } - - // grabbing off the floor or stack - if (gamepad2.right_bumper && gamepad2.dpad_up && (currentScoreState == OdoMec.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_GRIP); - rightFinger.setPosition(RIGHT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentScoreState == OdoMec.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentScoreState == OdoMec.scoreState.IDLE)) { - rightFinger.setPosition(RIGHT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentScoreState == OdoMec.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - } - - // telemetry - telemetry.addData("Status", "Run " + runtime.toString()); - telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); - telemetry.addData("Intake", currentIntakeState); - telemetry.addData("Drive", currentDriveState); - telemetry.addData("Score", currentScoreState); - - //telemetry.addData("Left Lower", leftLower.isPressed() ? "Pressed" : "Not Pressed"); - //telemetry.addData("Left Upper", leftUpper.isPressed() ? "Pressed" : "Not Pressed"); - //telemetry.addData("Right Lower", rightLower.isPressed() ? "Pressed" : "Not Pressed"); - //telemetry.addData("Right Upper", rightUpper.isPressed() ? "Pressed" : "Not Pressed"); - - // launcher - airplane(); - - telemetry.update(); - } - - } -} \ No newline at end of file From 8b12cd3f21e37d9be4a29f90dfa52739fa96404e Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Sat, 9 Dec 2023 12:43:01 -0700 Subject: [PATCH 105/154] New intake pos Wrist: 0.625 shoulder: 0.425 elbow: 0.83 --- .../main/java/org/firstinspires/ftc/teamcode/Mutation.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 70eeb8b878..9ae3514513 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -258,11 +258,11 @@ private void intakePos() { //hopper.setPosition(0.02); //shoulder.setPosition(0.44); //wrist.setPosition(0.26); - wrist.setPosition(0.265); - shoulder.setPosition(0.455); + wrist.setPosition(0.625); + shoulder.setPosition(0.425); leftFinger.setPosition(0.5); rightFinger.setPosition(0.5); - elbow.setPosition(0.5); + elbow.setPosition(0.83); //setLiftHeight(0.42); currentArmPos = ArmPosition.INTAKE; From ac21f4b63d7154947163e7ecd4532894db44d7e6 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 9 Dec 2023 12:58:33 -0700 Subject: [PATCH 106/154] made da functions 4 da intake n score --- .../firstinspires/ftc/teamcode/Mutation.java | 393 +++++++++--------- 1 file changed, 202 insertions(+), 191 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index e79b6b7629..73ba8e868e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -246,6 +246,36 @@ private void handleDriveSequence() { currentDriveState = Mutation.driveState.IDLE; } } + private void driveCode() { + // mecanum drive + double forward = gamepad1.left_stick_y; + double strafe = -gamepad1.left_stick_x; + double turn = -gamepad1.right_stick_x; + + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); + + double rightFrontPower = (forward - strafe - turn) / denominator; + double leftFrontPower = (forward + strafe + turn) / denominator; + double rightBackPower = (forward + strafe - turn) / denominator; + double leftBackPower = (forward - strafe + turn) / denominator; + + if (gamepad1.left_bumper) { + rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); + leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); + rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); + leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); + } else { + rightFrontPower = Range.clip(rightFrontPower, -1, 1); + leftFrontPower = Range.clip(leftFrontPower, -1, 1); + rightBackPower = Range.clip(rightBackPower, -1, 1); + leftBackPower = Range.clip(leftBackPower, -1, 1); + } + + rightFront.setPower(rightFrontPower); + leftFront.setPower(leftFrontPower); + rightBack.setPower(rightBackPower); + leftBack.setPower(leftBackPower); + } // score private void handleScorePosSequence(Mutation.ScorePosition scorePos) { @@ -324,6 +354,84 @@ private boolean isServoAtPosition(Servo servo, double position) { return Math.abs(servo.getPosition() - position) < SERVO_TOLERANCE; } + private void scoringFunction() { + // scoring + // score position one + if (gamepad2.y && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, SCORE_ONE_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position two + if (gamepad2.b && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, SCORE_TWO_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position three + if (gamepad2.a && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_THREE_SHOULDER, SCORE_THREE_WRIST, SCORE_THREE_ELBOW, SCORE_THREE_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position four + //TODO: gotta put !gamepad2.left_bumper above + if (gamepad2.x && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, SCORE_FOUR_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position five + if ((gamepad2.left_bumper && gamepad2.y) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_FIVE_SHOULDER, SCORE_FIVE_WRIST, SCORE_FIVE_ELBOW, SCORE_FIVE_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position six + if ((gamepad2.left_bumper && gamepad2.b) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_SIX_SHOULDER, SCORE_SIX_WRIST, SCORE_SIX_ELBOW, SCORE_SIX_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position seven + if ((gamepad2.left_bumper && gamepad2.a) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_SEVEN_SHOULDER, SCORE_SEVEN_WRIST, SCORE_SEVEN_ELBOW, SCORE_SEVEN_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position eight + if ((gamepad2.left_bumper && gamepad2.x) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_EIGHT_SHOULDER, SCORE_EIGHT_WRIST, SCORE_EIGHT_ELBOW, SCORE_EIGHT_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position nine + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.y) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_NINE_SHOULDER, SCORE_NINE_WRIST, SCORE_NINE_ELBOW, SCORE_NINE_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position ten + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.b) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_TEN_SHOULDER, SCORE_TEN_WRIST, SCORE_TEN_ELBOW, SCORE_TEN_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + // score position eleven + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.a) && (currentScoreState == Mutation.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Mutation.ScorePosition(SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_ELBOW, SCORE_ELEVEN_LIFT); + currentScoreState = Mutation.scoreState.MOVING_SHOULDER; + } + if (activeScorePosition != null) { + handleScorePosSequence(activeScorePosition); + } + if (currentScoreState == Mutation.scoreState.COMPLETED) { + currentScoreState = Mutation.scoreState.IDLE; + activeScorePosition = null; // Reset the active position + } + } + private void moveServoGradually(Servo servo, double targetPosition) { double currentPosition = servo.getPosition(); @@ -344,6 +452,96 @@ private void moveServoGradually(Servo servo, double targetPosition) { } } + private void hangCode() { + // hanging + if (gamepad1.right_trigger > TRIGGER_THRESHOLD){ + if (!leftUpper.isPressed()) { + leftHang.setDirection(DcMotor.Direction.FORWARD); + leftHang.setPower(.9); + } + if (!rightUpper.isPressed()) { + rightHang.setDirection(DcMotor.Direction.FORWARD); + rightHang.setPower(.9); + } + } else if (gamepad1.left_trigger > TRIGGER_THRESHOLD){ + if (!leftLower.isPressed()) { + leftHang.setDirection(DcMotor.Direction.REVERSE); + leftHang.setPower(.9); + } + if (!rightLower.isPressed()) { + rightHang.setDirection(DcMotor.Direction.REVERSE); + rightHang.setPower(.9); + } + } else { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setPower(0); + } + } + + private void intakeFunction() { + // intake + // claw intake from floor + //TODO: add saftey + if (gamepad1.left_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + activeIntakePosition = new Mutation.IntakePosition(SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE); + currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; + } + // claw intake the top 2 from a stack of 5 + if (gamepad1.b && currentIntakeState == Mutation.intakeState.IDLE) { + activeIntakePosition = new Mutation.IntakePosition(SHOULDER_TOP_TWO, WRIST_TOP_TWO, ELBOW_TOP_TWO); + currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; + } + // claw intake the next 2 from a stack of 3 + if (gamepad1.a && currentIntakeState == Mutation.intakeState.IDLE) { + activeIntakePosition = new Mutation.IntakePosition(SHOULDER_NEXT_TWO, WRIST_NEXT_TWO, ELBOW_NEXT_TWO); + currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; + } + if (activeIntakePosition != null) { + handleIntakeSequence(activeIntakePosition); + } + if (currentIntakeState == Mutation.intakeState.COMPLETED) { + currentIntakeState = Mutation.intakeState.IDLE; + activeIntakePosition = null; // Reset the active position + } + + + // dropping on the backboard for scoring + if (gamepad2.dpad_up && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_left && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + } else if (gamepad2.dpad_right && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_down && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + + // grabbing off the floor or stack + if (gamepad2.right_bumper && gamepad2.dpad_up && (currentScoreState == Mutation.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentScoreState == Mutation.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentScoreState == Mutation.scoreState.IDLE)) { + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentScoreState == Mutation.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + } + private void emergencyStop() { + if (gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); + } + } + // lift (has been adjusted mechanically to use the same height) public void setLiftPosition(double targetPosition) { // Ensure the target position is within the valid range @@ -434,70 +632,6 @@ public void runOpMode() throws InterruptedException { while(opModeIsActive()){ - // mecanum drive - double forward = gamepad1.left_stick_y; - double strafe = -gamepad1.left_stick_x; - double turn = -gamepad1.right_stick_x; - - double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); - - double rightFrontPower = (forward - strafe - turn) / denominator; - double leftFrontPower = (forward + strafe + turn) / denominator; - double rightBackPower = (forward + strafe - turn) / denominator; - double leftBackPower = (forward - strafe + turn) / denominator; - - if (gamepad1.left_bumper) { - rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); - leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); - rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); - leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); - } else { - rightFrontPower = Range.clip(rightFrontPower, -1, 1); - leftFrontPower = Range.clip(leftFrontPower, -1, 1); - rightBackPower = Range.clip(rightBackPower, -1, 1); - leftBackPower = Range.clip(leftBackPower, -1, 1); - } - - rightFront.setPower(rightFrontPower); - leftFront.setPower(leftFrontPower); - rightBack.setPower(rightBackPower); - leftBack.setPower(leftBackPower); - - // hanging - if (gamepad1.right_trigger > TRIGGER_THRESHOLD){ - if (!leftUpper.isPressed()) { - leftHang.setDirection(DcMotor.Direction.FORWARD); - leftHang.setPower(.9); - } - if (!rightUpper.isPressed()) { - rightHang.setDirection(DcMotor.Direction.FORWARD); - rightHang.setPower(.9); - } - } else if (gamepad1.left_trigger > TRIGGER_THRESHOLD){ - if (!leftLower.isPressed()) { - leftHang.setDirection(DcMotor.Direction.REVERSE); - leftHang.setPower(.9); - } - if (!rightLower.isPressed()) { - rightHang.setDirection(DcMotor.Direction.REVERSE); - rightHang.setPower(.9); - } - } else { - leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - leftHang.setPower(0); - rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightHang.setPower(0); - } - - - // emergency stop - if (gamepad1.y) { - leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - leftHang.setPower(0); - rightHang.setPower(0); - } - // claw drive position //TODO: add safety, this is drive around pos if (gamepad1.right_bumper && currentDriveState == Mutation.driveState.IDLE) { @@ -505,136 +639,9 @@ public void runOpMode() throws InterruptedException { } handleDriveSequence(); - // intake - // claw intake from floor - //TODO: add saftey, this is intake from stuff - if (gamepad1.left_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - activeIntakePosition = new Mutation.IntakePosition(SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE); - currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; - } - // claw intake the top 2 from a stack of 5 - if (gamepad1.b && currentIntakeState == Mutation.intakeState.IDLE) { - activeIntakePosition = new Mutation.IntakePosition(SHOULDER_TOP_TWO, WRIST_TOP_TWO, ELBOW_TOP_TWO); - currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; - } - // claw intake the next 2 from a stack of 3 - if (gamepad1.a && currentIntakeState == Mutation.intakeState.IDLE) { - activeIntakePosition = new Mutation.IntakePosition(SHOULDER_NEXT_TWO, WRIST_NEXT_TWO, ELBOW_NEXT_TWO); - currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; - } - if (activeIntakePosition != null) { - handleIntakeSequence(activeIntakePosition); - } - if (currentIntakeState == Mutation.intakeState.COMPLETED) { - currentIntakeState = Mutation.intakeState.IDLE; - activeIntakePosition = null; // Reset the active position - } - - // scoring - // score position one - if (gamepad2.y && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, SCORE_ONE_LIFT); - currentScoreState = Mutation.scoreState.MOVING_SHOULDER; - } - // score position two - if (gamepad2.b && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, SCORE_TWO_LIFT); - currentScoreState = Mutation.scoreState.MOVING_SHOULDER; - } - // score position three - if (gamepad2.a && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_THREE_SHOULDER, SCORE_THREE_WRIST, SCORE_THREE_ELBOW, SCORE_THREE_LIFT); - currentScoreState = Mutation.scoreState.MOVING_SHOULDER; - } - // score position four - //TODO: gotta put !gamepad2.left_bumper above - if (gamepad2.x && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, SCORE_FOUR_LIFT); - currentScoreState = Mutation.scoreState.MOVING_SHOULDER; - } - // score position five - if ((gamepad2.left_bumper && gamepad2.y) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_FIVE_SHOULDER, SCORE_FIVE_WRIST, SCORE_FIVE_ELBOW, SCORE_FIVE_LIFT); - currentScoreState = Mutation.scoreState.MOVING_SHOULDER; - } - // score position six - if ((gamepad2.left_bumper && gamepad2.b) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_SIX_SHOULDER, SCORE_SIX_WRIST, SCORE_SIX_ELBOW, SCORE_SIX_LIFT); - currentScoreState = Mutation.scoreState.MOVING_SHOULDER; - } - // score position seven - if ((gamepad2.left_bumper && gamepad2.a) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_SEVEN_SHOULDER, SCORE_SEVEN_WRIST, SCORE_SEVEN_ELBOW, SCORE_SEVEN_LIFT); - currentScoreState = Mutation.scoreState.MOVING_SHOULDER; - } - // score position eight - if ((gamepad2.left_bumper && gamepad2.x) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_EIGHT_SHOULDER, SCORE_EIGHT_WRIST, SCORE_EIGHT_ELBOW, SCORE_EIGHT_LIFT); - currentScoreState = Mutation.scoreState.MOVING_SHOULDER; - } - // score position nine - if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.y) && (currentScoreState == Mutation.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_NINE_SHOULDER, SCORE_NINE_WRIST, SCORE_NINE_ELBOW, SCORE_NINE_LIFT); - currentScoreState = Mutation.scoreState.MOVING_SHOULDER; - } - // score position ten - if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.b) && (currentScoreState == Mutation.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_TEN_SHOULDER, SCORE_TEN_WRIST, SCORE_TEN_ELBOW, SCORE_TEN_LIFT); - currentScoreState = Mutation.scoreState.MOVING_SHOULDER; - } - // score position eleven - if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.a) && (currentScoreState == Mutation.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_ELBOW, SCORE_ELEVEN_LIFT); - currentScoreState = Mutation.scoreState.MOVING_SHOULDER; - } - if (activeScorePosition != null) { - handleScorePosSequence(activeScorePosition); - } - if (currentScoreState == Mutation.scoreState.COMPLETED) { - currentScoreState = Mutation.scoreState.IDLE; - activeScorePosition = null; // Reset the active position - } - - // dropping on the backboard for scoring - if (gamepad2.dpad_up && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_DROP); - rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_left && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_DROP); - } else if (gamepad2.dpad_right && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_down && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - } - - // grabbing off the floor or stack - if (gamepad2.right_bumper && gamepad2.dpad_up && (currentScoreState == Mutation.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_GRIP); - rightFinger.setPosition(RIGHT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentScoreState == Mutation.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentScoreState == Mutation.scoreState.IDLE)) { - rightFinger.setPosition(RIGHT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentScoreState == Mutation.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - } // telemetry telemetry.addData("Status", "Run " + runtime.toString()); - telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); telemetry.addData("Intake", currentIntakeState); telemetry.addData("Drive", currentDriveState); telemetry.addData("Score", currentScoreState); @@ -646,6 +653,10 @@ public void runOpMode() throws InterruptedException { // launcher airplane(); + driveCode(); + scoringFunction(); + intakeFunction(); + emergencyStop(); telemetry.update(); } From 28c172bef65fffd0b9ac0036efdab27db1291478 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 9 Dec 2023 13:04:28 -0700 Subject: [PATCH 107/154] added odomec --- .../firstinspires/ftc/teamcode/OdoMec.java | 665 ++++++++++++++++++ 1 file changed, 665 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java new file mode 100644 index 0000000000..2df311f65b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -0,0 +1,665 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.rev.RevTouchSensor; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +@TeleOp +public class OdoMec extends LinearOpMode { + + // Declare vars + // timers + private ElapsedTime runtime = new ElapsedTime(); + private ElapsedTime servoTimer = new ElapsedTime(); + + // motors + private DcMotor rightFront; //front right 0 + private DcMotor leftFront; //front left 2 + private DcMotor rightBack; //rear right 1 + private DcMotor leftBack; //rear left 3 + private DcMotor leftHang; + private DcMotor rightHang; + + // servos + private Servo launcher; + private Servo shoulder; + private Servo wrist; + private Servo elbow; + private Servo leftFinger; + private Servo rightFinger; + private Servo rightLift; + private Servo leftLift; + double LiftLeftOffset = -.05; + double LiftHeight; + private double servoposition = 0.0; + private double servodelta = 0.02; + private double servodelaytime = 0.03; + + + // servo values + private static final double SHOULDER_DRIVE = 0.425; // 0.425 + private static final double SHOULDER_INT = 0.43; // 0.455 + private static final double ELBOW_DRIVE= 0.5; + private static final double ELBOW_INTAKE = 0.8; + private static final double WRIST_INT = 0.55; // int for auto & testing 0.55; // int for DRIVE 0.265 + private static final double WRIST_DRIVE = 0.265; // int for auto & testing 0.55; // int for DRIVE 0.265 + private static final double WRIST_INTAKE = 0.575; + private static final double LEFT_FINGER_GRIP = 0.74; + private static final double LEFT_FINGER_DROP = 0.5; + private static final double LEFT_FINGER_INTAKE = 0.34; + private static final double RIGHT_FINGER_GRIP = .27; + private static final double RIGHT_FINGER_DROP = .5; + private static final double RIGHT_FINGER_INTAKE = 0.64; + private static final double TRIGGER_THRESHOLD = 0.5; + private static final double LAUNCHER_START_POS = 0.8; + private static final double SERVO_TOLERANCE = 0.01; + private static final double LIFT_DRIVE = 0.37; + private double liftTargetPosition = LIFT_DRIVE; + + // stack positions (top 2 o 5 and next 2 of 3 ) + // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin + // intake two off a stack of five + private static final double SHOULDER_TOP_TWO = SHOULDER_DRIVE; + private static final double WRIST_TOP_TWO = 0.55; + private static final double ELBOW_TOP_TWO = 0.6; + + // intake two off a stack of three + private static final double SHOULDER_NEXT_TWO = SHOULDER_DRIVE; + private static final double WRIST_NEXT_TWO = 0.44; + private static final double ELBOW_NEXT_TWO = 0.7; + + //=^-^= + // score positions (11 rows on the board) + // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin + // score position one button map (gamepad2.y) + private static final double SCORE_ONE_SHOULDER = 0.936; + private static final double SCORE_ONE_WRIST = 0.685; + private static final double SCORE_ONE_ELBOW = 0.52; + private static final double SCORE_ONE_LIFT = LIFT_DRIVE; + // score position two button map (gamepad2.b) + private static final double SCORE_TWO_SHOULDER = 0.9; + private static final double SCORE_TWO_WRIST = 0.745; + private static final double SCORE_TWO_ELBOW = 0.55; + private static final double SCORE_TWO_LIFT = LIFT_DRIVE; + // score position three button map (gamepad2.a) + private static final double SCORE_THREE_SHOULDER = 0.92; + private static final double SCORE_THREE_WRIST = 0.91; + private static final double SCORE_THREE_ELBOW = 0.67; + private static final double SCORE_THREE_LIFT = LIFT_DRIVE; + // score position four button map (gamepad2.x) + private static final double SCORE_FOUR_SHOULDER = 0.91; + private static final double SCORE_FOUR_WRIST =0.985; + private static final double SCORE_FOUR_ELBOW = 0.72; + private static final double SCORE_FOUR_LIFT = LIFT_DRIVE; + // score position five button map (gamepad2.left_bumper && gamepad2.y) + private static final double SCORE_FIVE_SHOULDER = 0.93; + private static final double SCORE_FIVE_WRIST = 1; + private static final double SCORE_FIVE_ELBOW = 0.82; + private static final double SCORE_FIVE_LIFT = LIFT_DRIVE; + // score position six button map (gamepad2.left_bumper && gamepad2.b) + private static final double SCORE_SIX_SHOULDER = 0.92; + private static final double SCORE_SIX_WRIST = 1; + private static final double SCORE_SIX_ELBOW = 0.85; + private static final double SCORE_SIX_LIFT = LIFT_DRIVE; + // score position seven button map (gamepad2.left_bumper && gamepad2.a) + private static final double SCORE_SEVEN_SHOULDER = 0.9; + private static final double SCORE_SEVEN_WRIST = 1; + private static final double SCORE_SEVEN_ELBOW = 0.81; + private static final double SCORE_SEVEN_LIFT = 0.51; + // score position eight button map (gamepad2.left_bumper && gamepad2.x) + private static final double SCORE_EIGHT_SHOULDER = 0.94; + private static final double SCORE_EIGHT_WRIST = 0.93; + private static final double SCORE_EIGHT_ELBOW = 0.8; + private static final double SCORE_EIGHT_LIFT = 0.8; + // score position nine button map (gamepad2.left_trigger && gamepad2.y) + private static final double SCORE_NINE_SHOULDER = 0.93; + private static final double SCORE_NINE_WRIST = 0.93; + private static final double SCORE_NINE_ELBOW = 0.79; + private static final double SCORE_NINE_LIFT = 0.86; + // score position ten button map (gamepad2.left_trigger && gamepad2.b) + private static final double SCORE_TEN_SHOULDER = 0.9; + private static final double SCORE_TEN_WRIST = 0.93; + private static final double SCORE_TEN_ELBOW = 0.76; + private static final double SCORE_TEN_LIFT = 0.98; + // score position eleven button map (gamepad2.left_trigger && gamepad2.a) + private static final double SCORE_ELEVEN_SHOULDER = 0.9; + private static final double SCORE_ELEVEN_WRIST = 0.93; + private static final double SCORE_ELEVEN_ELBOW = 0.76; + private static final double SCORE_ELEVEN_LIFT = 0.98; + + // sensors + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; + + + // state machines enumeration + // intake + private enum intakeState { + IDLE, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + COMPLETED + } + private OdoMec.intakeState currentIntakeState = OdoMec.intakeState.IDLE; + private OdoMec.IntakePosition activeIntakePosition = null; + // drive + private enum driveState { + IDLE, + MOVING_SHOULDER, + MOVING_ELBOW, + MOVING_WRIST, + COMPLETED + } + + private OdoMec.driveState currentDriveState = OdoMec.driveState.IDLE; + // score + private enum scoreState { + IDLE, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + MOVING_LIFT, + COMPLETED + } + + private OdoMec.scoreState currentScoreState = OdoMec.scoreState.IDLE; + private OdoMec.ScorePosition activeScorePosition = null; + + // state machine sequences to provide cascading actions with a single input (button press) + // intake + private void handleIntakeSequence(OdoMec.IntakePosition intakePos) { + switch (currentIntakeState) { + case MOVING_SHOULDER: + // Move the shoulder to intake position + moveServoGradually(shoulder, intakePos.shoulderPosition + .05); + shoulder.setPosition(intakePos.shoulderPosition); + if (isServoAtPosition(shoulder, intakePos.shoulderPosition)) { + currentIntakeState = OdoMec.intakeState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to intake position + wrist.setPosition(intakePos.wristPosition); + if (isServoAtPosition(wrist, intakePos.wristPosition)) { + currentIntakeState = intakeState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: + // Move the elbow to intake position so it don't slap the floor + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + moveServoGradually(elbow, intakePos.elbowPosition); + if (isServoAtPosition(elbow, intakePos.elbowPosition)) { + currentIntakeState = intakeState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; + } + // Check to reset the state to IDLE outside the switch + if (currentIntakeState == OdoMec.intakeState.COMPLETED) { + currentIntakeState = OdoMec.intakeState.IDLE; + } + } + + // drive + private void handleDriveSequence() { + switch (currentDriveState) { + case MOVING_SHOULDER: + // Move the shoulder to drive position + moveServoGradually(shoulder, SHOULDER_DRIVE + .05); + shoulder.setPosition(SHOULDER_DRIVE); + if (isServoAtPosition(shoulder, SHOULDER_DRIVE)) { + currentDriveState = OdoMec.driveState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: + // Move the elbow to drive position + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + elbow.setPosition(ELBOW_DRIVE); + if (isServoAtPosition(elbow, ELBOW_DRIVE)) { + currentDriveState = OdoMec.driveState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to drive position + wrist.setPosition(WRIST_DRIVE); + if (isServoAtPosition(wrist, WRIST_DRIVE)) { + currentDriveState = OdoMec.driveState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; + } + // Check to reset the state to IDLE outside the switch + if (currentDriveState == OdoMec.driveState.COMPLETED) { + currentDriveState = OdoMec.driveState.IDLE; + } + } + private void driveCode() { + // mecanum drive + double forward = gamepad1.left_stick_y; + double strafe = -gamepad1.left_stick_x; + double turn = -gamepad1.right_stick_x; + + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); + + double rightFrontPower = (forward - strafe - turn) / denominator; + double leftFrontPower = (forward + strafe + turn) / denominator; + double rightBackPower = (forward + strafe - turn) / denominator; + double leftBackPower = (forward - strafe + turn) / denominator; + + if (gamepad1.left_bumper) { + rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); + leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); + rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); + leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); + } else { + rightFrontPower = Range.clip(rightFrontPower, -1, 1); + leftFrontPower = Range.clip(leftFrontPower, -1, 1); + rightBackPower = Range.clip(rightBackPower, -1, 1); + leftBackPower = Range.clip(leftBackPower, -1, 1); + } + + rightFront.setPower(rightFrontPower); + leftFront.setPower(leftFrontPower); + rightBack.setPower(rightBackPower); + leftBack.setPower(leftBackPower); + } + + // score + private void handleScorePosSequence(OdoMec.ScorePosition scorePos) { + switch (currentScoreState) { + case MOVING_SHOULDER: + // Move the shoulder to the specified position + moveServoGradually(shoulder, scorePos.shoulderPosition - .05); + shoulder.setPosition(scorePos.shoulderPosition); + if (isServoAtPosition(shoulder, scorePos.shoulderPosition)) { + currentScoreState = OdoMec.scoreState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to the specified position + wrist.setPosition(scorePos.wristPosition); + if (isServoAtPosition(wrist, scorePos.wristPosition)) { + currentScoreState = OdoMec.scoreState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: + // Move the elbow to the specified position + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + elbow.setPosition(scorePos.elbowPosition); + if (isServoAtPosition(elbow, scorePos.elbowPosition)) { + currentScoreState = OdoMec.scoreState.MOVING_LIFT; + } + break; + case MOVING_LIFT: + setLiftPosition(scorePos.liftPosition); + if (isServoAtPosition(leftLift, scorePos.liftPosition) || isServoAtPosition(rightLift, scorePos.liftPosition)) { + currentScoreState = OdoMec.scoreState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + currentScoreState = OdoMec.scoreState.IDLE; + activeScorePosition = null; + break; + } + + if (currentScoreState == OdoMec.scoreState.COMPLETED) { + currentScoreState = OdoMec.scoreState.IDLE; + } + } + + // classes to support state machine actions + static class IntakePosition { + double shoulderPosition; + double wristPosition; + double elbowPosition; + + public IntakePosition(double shoulderPos, double wristPos, double elbowPos) { + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + } + } + + static class ScorePosition { + double shoulderPosition; + double wristPosition; + double elbowPosition; + double liftPosition; + + public ScorePosition(double shoulderPos, double wristPos, double elbowPos, double liftPos) { + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + this.liftPosition = liftPos; + } + } + + // functions to support state machine actions + private boolean isServoAtPosition(Servo servo, double position) { + return Math.abs(servo.getPosition() - position) < SERVO_TOLERANCE; + } + + private void scoringFunction() { + // scoring + // score position one + if (gamepad2.y && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, SCORE_ONE_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position two + if (gamepad2.b && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, SCORE_TWO_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position three + if (gamepad2.a && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_THREE_SHOULDER, SCORE_THREE_WRIST, SCORE_THREE_ELBOW, SCORE_THREE_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position four + //TODO: gotta put !gamepad2.left_bumper above + if (gamepad2.x && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, SCORE_FOUR_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position five + if ((gamepad2.left_bumper && gamepad2.y) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_FIVE_SHOULDER, SCORE_FIVE_WRIST, SCORE_FIVE_ELBOW, SCORE_FIVE_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position six + if ((gamepad2.left_bumper && gamepad2.b) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_SIX_SHOULDER, SCORE_SIX_WRIST, SCORE_SIX_ELBOW, SCORE_SIX_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position seven + if ((gamepad2.left_bumper && gamepad2.a) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_SEVEN_SHOULDER, SCORE_SEVEN_WRIST, SCORE_SEVEN_ELBOW, SCORE_SEVEN_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position eight + if ((gamepad2.left_bumper && gamepad2.x) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_EIGHT_SHOULDER, SCORE_EIGHT_WRIST, SCORE_EIGHT_ELBOW, SCORE_EIGHT_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position nine + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.y) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_NINE_SHOULDER, SCORE_NINE_WRIST, SCORE_NINE_ELBOW, SCORE_NINE_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position ten + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.b) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_TEN_SHOULDER, SCORE_TEN_WRIST, SCORE_TEN_ELBOW, SCORE_TEN_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position eleven + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.a) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_ELBOW, SCORE_ELEVEN_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + if (activeScorePosition != null) { + handleScorePosSequence(activeScorePosition); + } + if (currentScoreState == OdoMec.scoreState.COMPLETED) { + currentScoreState = OdoMec.scoreState.IDLE; + activeScorePosition = null; // Reset the active position + } + } + + private void moveServoGradually(Servo servo, double targetPosition) { + double currentPosition = servo.getPosition(); + + // Check if enough time has passed since the last update + if (servoTimer.seconds() > servodelaytime) { + // Determine the direction of movement + double direction = targetPosition > currentPosition ? servodelta : -servodelta; + + // Calculate the new position + servoposition = currentPosition + direction; + servoposition = Range.clip(servoposition, 0, 1); // Ensure the position is within valid range + + // Update the servo position + servo.setPosition(servoposition); + + // Reset the timer + servoTimer.reset(); + } + } + + private void hangCode() { + // hanging + if (gamepad1.right_trigger > TRIGGER_THRESHOLD){ + if (!leftUpper.isPressed()) { + leftHang.setDirection(DcMotor.Direction.FORWARD); + leftHang.setPower(.9); + } + if (!rightUpper.isPressed()) { + rightHang.setDirection(DcMotor.Direction.FORWARD); + rightHang.setPower(.9); + } + } else if (gamepad1.left_trigger > TRIGGER_THRESHOLD){ + if (!leftLower.isPressed()) { + leftHang.setDirection(DcMotor.Direction.REVERSE); + leftHang.setPower(.9); + } + if (!rightLower.isPressed()) { + rightHang.setDirection(DcMotor.Direction.REVERSE); + rightHang.setPower(.9); + } + } else { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setPower(0); + } + } + + private void intakeFunction() { + // intake + // claw intake from floor + //TODO: add saftey + if (gamepad1.left_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { + activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE); + currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; + } + // claw intake the top 2 from a stack of 5 + if (gamepad1.b && currentIntakeState == OdoMec.intakeState.IDLE) { + activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_TOP_TWO, WRIST_TOP_TWO, ELBOW_TOP_TWO); + currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; + } + // claw intake the next 2 from a stack of 3 + if (gamepad1.a && currentIntakeState == OdoMec.intakeState.IDLE) { + activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_NEXT_TWO, WRIST_NEXT_TWO, ELBOW_NEXT_TWO); + currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; + } + if (activeIntakePosition != null) { + handleIntakeSequence(activeIntakePosition); + } + if (currentIntakeState == OdoMec.intakeState.COMPLETED) { + currentIntakeState = OdoMec.intakeState.IDLE; + activeIntakePosition = null; // Reset the active position + } + + + // dropping on the backboard for scoring + if (gamepad2.dpad_up && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_left && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + } else if (gamepad2.dpad_right && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_down && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + + // grabbing off the floor or stack + if (gamepad2.right_bumper && gamepad2.dpad_up && (currentScoreState == OdoMec.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentScoreState == OdoMec.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentScoreState == OdoMec.scoreState.IDLE)) { + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentScoreState == OdoMec.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + } + private void emergencyStop() { + if (gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); + } + } + + // lift (has been adjusted mechanically to use the same height) + public void setLiftPosition(double targetPosition) { + // Ensure the target position is within the valid range + targetPosition = Math.max(0.0, Math.min(targetPosition, 1.0)); + + // Set the servo positions + leftLift.setPosition(targetPosition); + rightLift.setPosition(targetPosition); + } + + // launcher + private void airplane() { + if (gamepad2.back) { + launcher.setPosition(0.1); + } else { + launcher.setPosition(0.8); + } + } + private void launcherstartPos() { + launcher.setPosition(LAUNCHER_START_POS); + } + + @Override + // NOT loop \/ - Or int of vars + public void runOpMode() throws InterruptedException { + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + // hardware mappings and parameters + + // motors + rightFront = hardwareMap.get(DcMotor.class, "rightFront"); + leftFront = hardwareMap.get(DcMotor.class, "leftFront"); + rightBack = hardwareMap.get(DcMotor.class, "rightBack"); + leftBack = hardwareMap.get(DcMotor.class, "leftBack"); + + leftHang = hardwareMap.get(DcMotor.class, "leftHang"); + rightHang = hardwareMap.get(DcMotor.class, "rightHang"); + + rightFront.setDirection(DcMotor.Direction.REVERSE); + leftFront.setDirection(DcMotor.Direction.FORWARD); + rightBack.setDirection(DcMotor.Direction.REVERSE); + leftBack.setDirection(DcMotor.Direction.FORWARD); + + // motor modes + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // servos + shoulder = hardwareMap.get(Servo.class, "shoulder"); + wrist = hardwareMap.get(Servo.class, "wrist"); + elbow = hardwareMap.get(Servo.class, "elbow"); + leftFinger = hardwareMap.get(Servo.class, "lFinger"); + rightFinger = hardwareMap.get(Servo.class, "rFinger"); + launcher = hardwareMap.get(Servo.class, "launcher"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + + // servo modes + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + launcher.setDirection(Servo.Direction.REVERSE); + + // sensors + leftUpper = hardwareMap.get(RevTouchSensor.class, "leftUpper"); + rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); + leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); + rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); + + // init positions + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + setLiftPosition(LIFT_DRIVE); + shoulder.setPosition(SHOULDER_INT); + elbow.setPosition(ELBOW_DRIVE); + wrist.setPosition(WRIST_INTAKE); + launcherstartPos(); + + telemetry.addData("Status", "OdoMec is ready to run!"); + telemetry.addData("Initializing TeleOp",""); + + waitForStart(); + runtime.reset(); + + + while(opModeIsActive()){ + + // claw drive position + //TODO: add safety, this is drive around pos + if (gamepad1.right_bumper && currentDriveState == OdoMec.driveState.IDLE) { + currentDriveState = OdoMec.driveState.MOVING_SHOULDER; + } + handleDriveSequence(); + + + // telemetry + telemetry.addData("Status", "Run " + runtime.toString()); + telemetry.addData("Intake", currentIntakeState); + telemetry.addData("Drive", currentDriveState); + telemetry.addData("Score", currentScoreState); + + //telemetry.addData("Left Lower", leftLower.isPressed() ? "Pressed" : "Not Pressed"); + //telemetry.addData("Left Upper", leftUpper.isPressed() ? "Pressed" : "Not Pressed"); + //telemetry.addData("Right Lower", rightLower.isPressed() ? "Pressed" : "Not Pressed"); + //telemetry.addData("Right Upper", rightUpper.isPressed() ? "Pressed" : "Not Pressed"); + + // launcher + airplane(); + driveCode(); + scoringFunction(); + intakeFunction(); + emergencyStop(); + + telemetry.update(); + } + + } +} \ No newline at end of file From 00be0336762d5a16610c56e931aedc6bb5134ec8 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Sat, 9 Dec 2023 13:11:08 -0700 Subject: [PATCH 108/154] New intake pos Wrist: 0.625 shoulder: 0.425 elbow: 0.83 V2 --- .../main/java/org/firstinspires/ftc/teamcode/Mutation.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 73ba8e868e..6421f60e46 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -44,10 +44,10 @@ public class Mutation extends LinearOpMode { private static final double SHOULDER_DRIVE = 0.425; // 0.425 private static final double SHOULDER_INT = 0.43; // 0.455 private static final double ELBOW_DRIVE= 0.5; - private static final double ELBOW_INTAKE = 0.8; + private static final double ELBOW_INTAKE = 0.83; private static final double WRIST_INT = 0.55; // int for auto & testing 0.55; // int for DRIVE 0.265 private static final double WRIST_DRIVE = 0.265; // int for auto & testing 0.55; // int for DRIVE 0.265 - private static final double WRIST_INTAKE = 0.575; + private static final double WRIST_INTAKE = 0.625; private static final double LEFT_FINGER_GRIP = 0.74; private static final double LEFT_FINGER_DROP = 0.5; private static final double LEFT_FINGER_INTAKE = 0.34; From d6bc5a483cde6f9449df884be0499336c85dfe05 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Sat, 9 Dec 2023 13:55:08 -0700 Subject: [PATCH 109/154] new stuff --- .../firstinspires/ftc/teamcode/Mutation.java | 33 ++++++++++--------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 6421f60e46..267af6a079 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -63,14 +63,15 @@ public class Mutation extends LinearOpMode { // stack positions (top 2 o 5 and next 2 of 3 ) // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin // intake two off a stack of five - private static final double SHOULDER_TOP_TWO = SHOULDER_DRIVE; - private static final double WRIST_TOP_TWO = 0.55; - private static final double ELBOW_TOP_TWO = 0.6; + private static final double SHOULDER_TOP_TWO = 0.909; + private static final double WRIST_TOP_TWO = 0.1; + private static final double ELBOW_TOP_TWO = 0.72; // intake two off a stack of three - private static final double SHOULDER_NEXT_TWO = SHOULDER_DRIVE; - private static final double WRIST_NEXT_TWO = 0.44; - private static final double ELBOW_NEXT_TWO = 0.7; + private static final double SHOULDER_NEXT_TWO = 0.425; + private static final double WRIST_NEXT_TWO = 0.565; + private static final double ELBOW_NEXT_TWO = 0.76; + //=^-^= // score positions (11 rows on the board) @@ -96,27 +97,27 @@ public class Mutation extends LinearOpMode { private static final double SCORE_FOUR_ELBOW = 0.72; private static final double SCORE_FOUR_LIFT = LIFT_DRIVE; // score position five button map (gamepad2.left_bumper && gamepad2.y) - private static final double SCORE_FIVE_SHOULDER = 0.93; + private static final double SCORE_FIVE_SHOULDER = 0.92611111; private static final double SCORE_FIVE_WRIST = 1; - private static final double SCORE_FIVE_ELBOW = 0.82; - private static final double SCORE_FIVE_LIFT = LIFT_DRIVE; + private static final double SCORE_FIVE_ELBOW = 0.73; + private static final double SCORE_FIVE_LIFT = 0.59; // score position six button map (gamepad2.left_bumper && gamepad2.b) - private static final double SCORE_SIX_SHOULDER = 0.92; + private static final double SCORE_SIX_SHOULDER = 0.925; private static final double SCORE_SIX_WRIST = 1; - private static final double SCORE_SIX_ELBOW = 0.85; - private static final double SCORE_SIX_LIFT = LIFT_DRIVE; + private static final double SCORE_SIX_ELBOW = 0.75; + private static final double SCORE_SIX_LIFT = .71; // score position seven button map (gamepad2.left_bumper && gamepad2.a) - private static final double SCORE_SEVEN_SHOULDER = 0.9; + private static final double SCORE_SEVEN_SHOULDER = 0.91; private static final double SCORE_SEVEN_WRIST = 1; - private static final double SCORE_SEVEN_ELBOW = 0.81; - private static final double SCORE_SEVEN_LIFT = 0.51; + private static final double SCORE_SEVEN_ELBOW = 0.72; + private static final double SCORE_SEVEN_LIFT = 0.8; // score position eight button map (gamepad2.left_bumper && gamepad2.x) private static final double SCORE_EIGHT_SHOULDER = 0.94; private static final double SCORE_EIGHT_WRIST = 0.93; private static final double SCORE_EIGHT_ELBOW = 0.8; private static final double SCORE_EIGHT_LIFT = 0.8; // score position nine button map (gamepad2.left_trigger && gamepad2.y) - private static final double SCORE_NINE_SHOULDER = 0.93; + private static final double SCORE_NINE_SHOULDER = 0.93 ; private static final double SCORE_NINE_WRIST = 0.93; private static final double SCORE_NINE_ELBOW = 0.79; private static final double SCORE_NINE_LIFT = 0.86; From 5317a62b547343be0e7d0f75b14a1583cc9bbace Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 9 Dec 2023 13:56:09 -0700 Subject: [PATCH 110/154] Possibly borked auto --- .../ftc/teamcode/EnigmaAuto.java | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index c11f0172b4..1a95a4ffbd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -66,8 +66,8 @@ @Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "OdoMec") public class EnigmaAuto extends LinearOpMode { - public static String TEAM_NAME = "ENIGMA"; //TODO: Enter team Name - public static int TEAM_NUMBER = 16265; //TODO: Enter team Number + public static String TEAM_NAME = "ENIGMA"; + public static int TEAM_NUMBER = 16265; private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera @@ -89,6 +89,10 @@ public class EnigmaAuto extends LinearOpMode { private Servo leftLift; double LiftLeftOffset = .04; double LiftHeight; + private static final double ELBOW_DRIVE= 0.5; + private static final double ELBOW_INTAKE = 0.8; + private static final double WRIST_INTAKE = 0.575; + private static final double SHOULDER_DRIVE = 0.425; // 0.425 // sensors private RevTouchSensor rightUpper; @@ -375,6 +379,13 @@ public void runAutonoumousMode() { //TODO : Code to drop Purple Pixel on Spike Mark safeWaitSeconds(1); + shoulder.setPosition(SHOULDER_DRIVE); + elbow.setPosition(ELBOW_INTAKE); + wrist.setPosition(WRIST_INTAKE); + sleep(1000); + rightFinger.setPosition(0.64); + sleep(500); + @@ -440,10 +451,10 @@ public void selectStartingPosition() { TEAM_NAME, " ", TEAM_NUMBER); telemetry.addData("---------------------------------------",""); telemetry.addData("Select Starting Position using XYAB on Logitech (or ▢ΔOX on Playstayion) on gamepad 1:",""); - telemetry.addData(" Blue Left ", "(X / ▢)"); - telemetry.addData(" Blue Right ", "(Y / Δ)"); - telemetry.addData(" Red Left ", "(B / O)"); - telemetry.addData(" Red Right ", "(A / X)"); + telemetry.addData(" Blue Left ", "(X)"); + telemetry.addData(" Blue Right ", "(Y)"); + telemetry.addData(" Red Left ", "(B)"); + telemetry.addData(" Red Right ", "(A)"); if(gamepad1.x){ startPosition = START_POSITION.BLUE_LEFT; break; From df73319b9af7f1f4de90ab0210113082b7425985 Mon Sep 17 00:00:00 2001 From: NinjaPenguin Date: Sat, 9 Dec 2023 14:14:23 -0700 Subject: [PATCH 111/154] new intake stack 5 to 3 --- .../main/java/org/firstinspires/ftc/teamcode/Mutation.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 267af6a079..b251c0ef3f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -63,9 +63,9 @@ public class Mutation extends LinearOpMode { // stack positions (top 2 o 5 and next 2 of 3 ) // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin // intake two off a stack of five - private static final double SHOULDER_TOP_TWO = 0.909; - private static final double WRIST_TOP_TWO = 0.1; - private static final double ELBOW_TOP_TWO = 0.72; + private static final double SHOULDER_TOP_TWO = 0.425; + private static final double WRIST_TOP_TWO = 0.606; + private static final double ELBOW_TOP_TWO = 0.74; // intake two off a stack of three private static final double SHOULDER_NEXT_TWO = 0.425; From 6fe113143e529c48dafc65e6d7ac43a7bcaef582 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 9 Dec 2023 16:28:49 -0700 Subject: [PATCH 112/154] Working auto no 2 pixels :( --- .../ftc/teamcode/EnigmaAuto.java | 167 ++++++++++-------- .../ftc/teamcode/tuning/TuningOpModes.java | 2 +- 2 files changed, 95 insertions(+), 74 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 1a95a4ffbd..bf8ae6214b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -26,7 +26,7 @@ * 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. */ - +//do he auto and get a 2+2 package org.firstinspires.ftc.teamcode; import static com.qualcomm.robotcore.util.ElapsedTime.Resolution.SECONDS; @@ -42,6 +42,7 @@ import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; @@ -63,42 +64,40 @@ /** * ENIGMA Autonomous Example for only vision detection using openCv and park */ -@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "OdoMec") +@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "Beginnings") public class EnigmaAuto extends LinearOpMode { - public static String TEAM_NAME = "ENIGMA"; - public static int TEAM_NUMBER = 16265; + public static String TEAM_NAME = "ENIGMA"; //TODO: Enter team Name + public static int TEAM_NUMBER = 16265; //TODO: Enter team Number private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera - private DcMotor rightFront; //front right 0 - private DcMotor leftFront; //front left 2 - private DcMotor rightBack; //rear right 1 - private DcMotor leftBack; //rear left 3 - private DcMotor leftHang; - private DcMotor rightHang; + private ElapsedTime runtime = new ElapsedTime(); + private ElapsedTime servoTimer = new ElapsedTime(); - // servos - private Servo launcher; + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; + private Servo rightLift; + private Servo leftLift; private Servo shoulder; private Servo wrist; private Servo elbow; private Servo leftFinger; private Servo rightFinger; - private Servo rightLift; - private Servo leftLift; + double LiftLeftOffset = .04; double LiftHeight; + private static final double ELBOW_DRIVE= 0.5; private static final double ELBOW_INTAKE = 0.8; private static final double WRIST_INTAKE = 0.575; private static final double SHOULDER_DRIVE = 0.425; // 0.425 + private static final double SCORE_ONE_SHOULDER = 0.936; + private static final double SCORE_ONE_WRIST = 0.685; + private static final double SCORE_ONE_ELBOW = 0.52; - // sensors - private RevTouchSensor rightUpper; - private RevTouchSensor leftUpper; - private RevTouchSensor rightLower; - private RevTouchSensor leftLower; //Define and declare Robot Starting Locations public enum START_POSITION{ @@ -124,61 +123,53 @@ public enum IDENTIFIED_SPIKE_MARK_LOCATION { public static double centeravgfinoutput = 0; public static double rightavgfinoutput = 0; + private double servoposition = 0.0; + private double servodelta = 0.02; + private double servodelaytime = 0.03; + + private void moveServoGradually(Servo servo, double targetPosition) { + double currentPosition = servo.getPosition(); + + // Check if enough time has passed since the last update + if (servoTimer.seconds() > servodelaytime) { + // Determine the direction of movement + double direction = targetPosition > currentPosition ? servodelta : -servodelta; + + // Calculate the new position + servoposition = currentPosition + direction; + servoposition = Range.clip(servoposition, 0, 1); // Ensure the position is within valid range + // Update the servo position + servo.setPosition(servoposition); + + // Reset the timer + servoTimer.reset(); + } + } @Override public void runOpMode() throws InterruptedException { - // motors - rightFront = hardwareMap.get(DcMotor.class, "rightFront"); - leftFront = hardwareMap.get(DcMotor.class, "leftFront"); - rightBack = hardwareMap.get(DcMotor.class, "rightBack"); - leftBack = hardwareMap.get(DcMotor.class, "leftBack"); - - leftHang = hardwareMap.get(DcMotor.class, "leftHang"); - rightHang = hardwareMap.get(DcMotor.class, "rightHang"); - - rightFront.setDirection(DcMotor.Direction.REVERSE); - leftFront.setDirection(DcMotor.Direction.FORWARD); - rightBack.setDirection(DcMotor.Direction.REVERSE); - leftBack.setDirection(DcMotor.Direction.FORWARD); - - // motor modes - rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - // servos - shoulder = hardwareMap.get(Servo.class, "shoulder"); wrist = hardwareMap.get(Servo.class, "wrist"); + + shoulder = hardwareMap.get(Servo.class, "shoulder"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); elbow = hardwareMap.get(Servo.class, "elbow"); leftFinger = hardwareMap.get(Servo.class, "lFinger"); rightFinger = hardwareMap.get(Servo.class, "rFinger"); - launcher = hardwareMap.get(Servo.class, "launcher"); - rightLift = hardwareMap.get(Servo.class, "rightLift"); - leftLift = hardwareMap.get(Servo.class, "leftLift"); - // servo modes shoulder.setDirection(Servo.Direction.REVERSE); wrist.setDirection(Servo.Direction.REVERSE); - launcher.setDirection(Servo.Direction.REVERSE); - // sensors - leftUpper = hardwareMap.get(RevTouchSensor.class, "leftUpper"); - rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); - leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); - rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); - - //int pos - leftFinger.setPosition(0.74); - rightFinger.setPosition(0.27); + //init pos leftLift.setPosition(0.37); rightLift.setPosition(0.37); shoulder.setPosition(0.43); - elbow.setPosition(0.5); wrist.setPosition(0.575); - launcher.setPosition(0.8); - + elbow.setPosition(0.5); + sleep(1000); + leftFinger.setPosition(0.74); + rightFinger.setPosition(0.27); // Vision OpenCV / Color Detection WebcamName webcamName = hardwareMap.get(WebcamName.class, "Webcam 1"); @@ -264,23 +255,27 @@ public void runAutonoumousMode() { initPose = new Pose2d(0, 0, Math.toRadians(0)); //Starting pose moveBeyondTrussPose = new Pose2d(15,0,0); + //TODO: edit crap here switch (startPosition) { case BLUE_LEFT: drive = new MecanumDrive(hardwareMap, initPose); switch(identifiedSpikeMarkLocation){ case LEFT: dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up - dropPurplePixelPose = new Pose2d(23, 11, Math.toRadians(0)); + //dropPurplePixelPose = new Pose2d(23, 11, Math.toRadians(0)); + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); dropYellowPixelPose = new Pose2d(27, 36, Math.toRadians(-90)); break; case MIDDLE: - dropPurplePixelPosePush = new Pose2d(30.5, 0, Math.toRadians(0)); // change up - dropPurplePixelPose = new Pose2d(26.5, -3, Math.toRadians(0)); + dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up + //dropPurplePixelPose = new Pose2d(26.5, -3, Math.toRadians(0)); + dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); dropYellowPixelPose = new Pose2d(27, 36, Math.toRadians(-90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); - dropPurplePixelPose = new Pose2d(23.5, -7, Math.toRadians(-35)); + //dropPurplePixelPose = new Pose2d(23.5, -7, Math.toRadians(-35)); + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); dropYellowPixelPose = new Pose2d(27, 36, Math.toRadians(-90)); break; } @@ -380,14 +375,16 @@ public void runAutonoumousMode() { //TODO : Code to drop Purple Pixel on Spike Mark safeWaitSeconds(1); shoulder.setPosition(SHOULDER_DRIVE); - elbow.setPosition(ELBOW_INTAKE); wrist.setPosition(WRIST_INTAKE); + for(int c = 0; c<40; c++) { + moveServoGradually(elbow, ELBOW_INTAKE); + sleep(10); + } sleep(1000); rightFinger.setPosition(0.64); sleep(500); - - - + elbow.setPosition(ELBOW_DRIVE); + sleep(500); //Move robot to midwayPose1 Actions.runBlocking( @@ -426,13 +423,37 @@ public void runAutonoumousMode() { //TODO : Code to drop Pixel on Backdrop safeWaitSeconds(1); - + for(int s = 0; s<200; s++) { + moveServoGradually(shoulder, SCORE_ONE_SHOULDER); + sleep(7); + } + for(int w = 0; w<40; w++) { + moveServoGradually(wrist, SCORE_ONE_WRIST); + sleep(10); + } + for(int e = 0; e<20; e++) { + moveServoGradually(elbow, SCORE_ONE_ELBOW); + sleep(10); + } + sleep(300); + leftFinger.setPosition(0.5); + sleep(300); + for(int s = 0; s<200; s++) { + moveServoGradually(shoulder, SHOULDER_DRIVE); + sleep(7); + } + for(int w = 0; w<40; w++) { + moveServoGradually(wrist, WRIST_INTAKE); + sleep(10); + } + for(int e = 0; e<20; e++) { + moveServoGradually(elbow, ELBOW_DRIVE); + sleep(10); + } //Move robot to park in Backstage - - Actions.runBlocking( drive.actionBuilder(drive.pose) .strafeToLinearHeading(parkPose.position, parkPose.heading) @@ -451,10 +472,10 @@ public void selectStartingPosition() { TEAM_NAME, " ", TEAM_NUMBER); telemetry.addData("---------------------------------------",""); telemetry.addData("Select Starting Position using XYAB on Logitech (or ▢ΔOX on Playstayion) on gamepad 1:",""); - telemetry.addData(" Blue Left ", "(X)"); - telemetry.addData(" Blue Right ", "(Y)"); - telemetry.addData(" Red Left ", "(B)"); - telemetry.addData(" Red Right ", "(A)"); + telemetry.addData(" Blue Left ", "(X / ▢)"); + telemetry.addData(" Blue Right ", "(Y / Δ)"); + telemetry.addData(" Red Left ", "(B / O)"); + telemetry.addData(" Red Right ", "(A / X)"); if(gamepad1.x){ startPosition = START_POSITION.BLUE_LEFT; break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java index 6706b80e5c..658f790339 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java @@ -46,7 +46,7 @@ private static OpModeMeta metaForClass(Class cls) { .build(); } - //@OpModeRegistrar + @OpModeRegistrar public static void register(OpModeManager manager) { if (DISABLED) return; From 7fb5c0ab0ba451f7061dfb17b54e9328a7a8a225 Mon Sep 17 00:00:00 2001 From: Enigma-16265 Date: Sun, 10 Dec 2023 09:34:07 -0700 Subject: [PATCH 113/154] git working --- .../src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java index 2df311f65b..62f5b9f1ac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -10,7 +10,7 @@ @TeleOp public class OdoMec extends LinearOpMode { - +// adding for push test // Declare vars // timers private ElapsedTime runtime = new ElapsedTime(); From a15246fc8e70ad33f2a1e664b350c7fff2669c79 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 10 Dec 2023 13:23:31 -0700 Subject: [PATCH 114/154] Pushing untested auto, deleted a comment in McMuffin Expirimented in Mutation & OdoMec w/ Pos 5 and lift !NOTE: Contains Old Wrist Vaules, replaced wrist servo --- .../ftc/teamcode/EnigmaAuto.java | 85 ++-- .../firstinspires/ftc/teamcode/McMuffin.java | 13 - .../firstinspires/ftc/teamcode/Mutation.java | 110 ++--- .../firstinspires/ftc/teamcode/OdoMec.java | 398 +++++++++--------- 4 files changed, 295 insertions(+), 311 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index bf8ae6214b..fea0915805 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -90,13 +90,13 @@ public class EnigmaAuto extends LinearOpMode { double LiftLeftOffset = .04; double LiftHeight; - private static final double ELBOW_DRIVE= 0.5; - private static final double ELBOW_INTAKE = 0.8; - private static final double WRIST_INTAKE = 0.575; - private static final double SHOULDER_DRIVE = 0.425; // 0.425 - private static final double SCORE_ONE_SHOULDER = 0.936; - private static final double SCORE_ONE_WRIST = 0.685; - private static final double SCORE_ONE_ELBOW = 0.52; + private static final double ELBOW_DRIVE= Mutation.ELBOW_DRIVE; + private static final double ELBOW_INTAKE = Mutation.ELBOW_INTAKE; + private static final double WRIST_INTAKE = Mutation.WRIST_INTAKE; + //private static final double SHOULDER_DRIVE = 0.425; // 0.425 + private static final double SCORE_ONE_SHOULDER = Mutation.SCORE_ONE_SHOULDER; + private static final double SCORE_ONE_WRIST = Mutation.SCORE_ONE_WRIST; + private static final double SCORE_ONE_ELBOW = Mutation.SCORE_ONE_ELBOW; //Define and declare Robot Starting Locations @@ -262,21 +262,18 @@ public void runAutonoumousMode() { switch(identifiedSpikeMarkLocation){ case LEFT: dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up - //dropPurplePixelPose = new Pose2d(23, 11, Math.toRadians(0)); dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); - dropYellowPixelPose = new Pose2d(27, 36, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(23, 36, Math.toRadians(-90)); break; case MIDDLE: dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up - //dropPurplePixelPose = new Pose2d(26.5, -3, Math.toRadians(0)); dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(27, 36, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(30, 36, Math.toRadians(-90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); - //dropPurplePixelPose = new Pose2d(23.5, -7, Math.toRadians(-35)); dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); - dropYellowPixelPose = new Pose2d(27, 36, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(37, 36, Math.toRadians(-90)); break; } midwayPose1 = new Pose2d(14, 13, Math.toRadians(-45)); @@ -288,19 +285,19 @@ public void runAutonoumousMode() { drive = new MecanumDrive(hardwareMap, initPose); switch(identifiedSpikeMarkLocation){ case LEFT: - dropPurplePixelPosePush = new Pose2d(27, 9, Math.toRadians(45)); - dropPurplePixelPose = new Pose2d(23.5, 7, Math.toRadians(35)); - dropYellowPixelPose = new Pose2d(27, -36, Math.toRadians(90)); + dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); + dropYellowPixelPose = new Pose2d(21, -36, Math.toRadians(90)); break; case MIDDLE: - dropPurplePixelPosePush = new Pose2d(30.5, 0, Math.toRadians(0)); // change up - dropPurplePixelPose = new Pose2d(26.5, -3, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(27, -36, Math.toRadians(90)); + dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(29, -36, Math.toRadians(90)); break; case RIGHT: - dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up - dropPurplePixelPose = new Pose2d(23, -11, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(27, -36, Math.toRadians(90)); + dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); + dropYellowPixelPose = new Pose2d(37, -36, Math.toRadians(90)); break; } midwayPose1 = new Pose2d(14, -13, Math.toRadians(45)); @@ -312,19 +309,19 @@ public void runAutonoumousMode() { drive = new MecanumDrive(hardwareMap, initPose); switch(identifiedSpikeMarkLocation){ case LEFT: - dropPurplePixelPosePush = new Pose2d(27, 9, Math.toRadians(45)); - dropPurplePixelPose = new Pose2d(23.5, 7, Math.toRadians(35)); + dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); dropYellowPixelPose = new Pose2d(27, 86, Math.toRadians(-90)); break; case MIDDLE: - dropPurplePixelPosePush = new Pose2d(30.5, 0, Math.toRadians(0)); // change up - dropPurplePixelPose = new Pose2d(26.5, -3, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(27, 86, Math.toRadians(-90)); + dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(34, 86, Math.toRadians(-90)); break; case RIGHT: - dropPurplePixelPosePush = new Pose2d(27, -8, Math.toRadians(0)); // change up - dropPurplePixelPose = new Pose2d(23, -11, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(27, 86, Math.toRadians(-90)); + dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); + dropYellowPixelPose = new Pose2d(43, 86, Math.toRadians(-90)); break; } midwayPose1 = new Pose2d(8, -8, Math.toRadians(0)); @@ -340,18 +337,18 @@ public void runAutonoumousMode() { switch(identifiedSpikeMarkLocation){ case LEFT: dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up - dropPurplePixelPose = new Pose2d(23, 11, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(27, -86, Math.toRadians(90)); + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); + dropYellowPixelPose = new Pose2d(37, -86, Math.toRadians(90)); break; case MIDDLE: - dropPurplePixelPosePush = new Pose2d(30.5, 0, Math.toRadians(0)); // change up - dropPurplePixelPose = new Pose2d(26.5, -3, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(27, -86, Math.toRadians(90)); + dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(29, -86, Math.toRadians(90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); - dropPurplePixelPose = new Pose2d(23.5, -7, Math.toRadians(-35)); - dropYellowPixelPose = new Pose2d(27, -86, Math.toRadians(90)); + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); + dropYellowPixelPose = new Pose2d(21, -86, Math.toRadians(90)); break; } midwayPose1 = new Pose2d(8, 8, Math.toRadians(0)); @@ -374,8 +371,8 @@ public void runAutonoumousMode() { //TODO : Code to drop Purple Pixel on Spike Mark safeWaitSeconds(1); - shoulder.setPosition(SHOULDER_DRIVE); - wrist.setPosition(WRIST_INTAKE); + shoulder.setPosition(Mutation.SHOULDER_DRIVE); + wrist.setPosition(Mutation.WRIST_INTAKE); for(int c = 0; c<40; c++) { moveServoGradually(elbow, ELBOW_INTAKE); sleep(10); @@ -423,10 +420,6 @@ public void runAutonoumousMode() { //TODO : Code to drop Pixel on Backdrop safeWaitSeconds(1); - for(int s = 0; s<200; s++) { - moveServoGradually(shoulder, SCORE_ONE_SHOULDER); - sleep(7); - } for(int w = 0; w<40; w++) { moveServoGradually(wrist, SCORE_ONE_WRIST); sleep(10); @@ -435,11 +428,15 @@ public void runAutonoumousMode() { moveServoGradually(elbow, SCORE_ONE_ELBOW); sleep(10); } + for(int s = 0; s<200; s++) { + moveServoGradually(shoulder, SCORE_ONE_SHOULDER); + sleep(7); + } sleep(300); leftFinger.setPosition(0.5); sleep(300); for(int s = 0; s<200; s++) { - moveServoGradually(shoulder, SHOULDER_DRIVE); + moveServoGradually(shoulder, Mutation.SHOULDER_DRIVE); sleep(7); } for(int w = 0; w<40; w++) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java index 5d482d6c69..5e4c8faaf8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java @@ -149,19 +149,6 @@ public void runOpMode() throws InterruptedException { rightLift.setPosition(0.37); - // does intake exist????????????? - /*frontIntake = hardwareMap.get(DcMotor.class, "frontIntake"); - rearIntake = hardwareMap.get(DcMotor.class, "rearIntake"); - - frontIntake.setDirection(DcMotorSimple.Direction.FORWARD); - rearIntake.setDirection(DcMotorSimple.Direction.REVERSE); - frontIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rearIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rearIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - frontIntake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - */ - - telemetry.update(); waitForStart(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index b251c0ef3f..5bf5920ddb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -41,66 +41,66 @@ public class Mutation extends LinearOpMode { // servo values - private static final double SHOULDER_DRIVE = 0.425; // 0.425 - private static final double SHOULDER_INT = 0.43; // 0.455 - private static final double ELBOW_DRIVE= 0.5; - private static final double ELBOW_INTAKE = 0.83; - private static final double WRIST_INT = 0.55; // int for auto & testing 0.55; // int for DRIVE 0.265 - private static final double WRIST_DRIVE = 0.265; // int for auto & testing 0.55; // int for DRIVE 0.265 - private static final double WRIST_INTAKE = 0.625; - private static final double LEFT_FINGER_GRIP = 0.74; - private static final double LEFT_FINGER_DROP = 0.5; - private static final double LEFT_FINGER_INTAKE = 0.34; - private static final double RIGHT_FINGER_GRIP = .27; - private static final double RIGHT_FINGER_DROP = .5; - private static final double RIGHT_FINGER_INTAKE = 0.64; - private static final double TRIGGER_THRESHOLD = 0.5; - private static final double LAUNCHER_START_POS = 0.8; - private static final double SERVO_TOLERANCE = 0.01; - private static final double LIFT_DRIVE = 0.37; + public static final double SHOULDER_DRIVE = 0.425; // 0.425 + public static final double SHOULDER_INT = 0.43; // 0.455 + public static final double ELBOW_DRIVE= 0.5; + public static final double ELBOW_INTAKE = 0.83; + public static final double WRIST_INT = 0.55; // int for auto & testing 0.55; // int for DRIVE 0.265 + public static final double WRIST_DRIVE = 0.265; // int for auto & testing 0.55; // int for DRIVE 0.265 + public static final double WRIST_INTAKE = 0.625; + public static final double LEFT_FINGER_GRIP = 0.74; + public static final double LEFT_FINGER_DROP = 0.5; + public static final double LEFT_FINGER_INTAKE = 0.34; + public static final double RIGHT_FINGER_GRIP = .27; + public static final double RIGHT_FINGER_DROP = .5; + public static final double RIGHT_FINGER_INTAKE = 0.64; + public static final double TRIGGER_THRESHOLD = 0.5; + public static final double LAUNCHER_START_POS = 0.8; + public static final double SERVO_TOLERANCE = 0.01; + public static final double LIFT_DRIVE = 0.37; private double liftTargetPosition = LIFT_DRIVE; // stack positions (top 2 o 5 and next 2 of 3 ) // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin // intake two off a stack of five - private static final double SHOULDER_TOP_TWO = 0.425; - private static final double WRIST_TOP_TWO = 0.606; - private static final double ELBOW_TOP_TWO = 0.74; + public static final double SHOULDER_TOP_TWO = 0.425; + public static final double WRIST_TOP_TWO = 0.606; + public static final double ELBOW_TOP_TWO = 0.74; // intake two off a stack of three - private static final double SHOULDER_NEXT_TWO = 0.425; - private static final double WRIST_NEXT_TWO = 0.565; - private static final double ELBOW_NEXT_TWO = 0.76; + public static final double SHOULDER_NEXT_TWO = 0.425; + public static final double WRIST_NEXT_TWO = 0.565; + public static final double ELBOW_NEXT_TWO = 0.76; //=^-^= // score positions (11 rows on the board) // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin // score position one button map (gamepad2.y) - private static final double SCORE_ONE_SHOULDER = 0.936; - private static final double SCORE_ONE_WRIST = 0.685; - private static final double SCORE_ONE_ELBOW = 0.52; - private static final double SCORE_ONE_LIFT = LIFT_DRIVE; + public static final double SCORE_ONE_SHOULDER = 0.936; + public static final double SCORE_ONE_WRIST = 0.685; + public static final double SCORE_ONE_ELBOW = 0.52; + public static final double SCORE_ONE_LIFT = LIFT_DRIVE; // score position two button map (gamepad2.b) - private static final double SCORE_TWO_SHOULDER = 0.9; - private static final double SCORE_TWO_WRIST = 0.745; - private static final double SCORE_TWO_ELBOW = 0.55; - private static final double SCORE_TWO_LIFT = LIFT_DRIVE; + public static final double SCORE_TWO_SHOULDER = 0.9; + public static final double SCORE_TWO_WRIST = 0.745; + public static final double SCORE_TWO_ELBOW = 0.55; + public static final double SCORE_TWO_LIFT = LIFT_DRIVE; // score position three button map (gamepad2.a) - private static final double SCORE_THREE_SHOULDER = 0.92; - private static final double SCORE_THREE_WRIST = 0.91; - private static final double SCORE_THREE_ELBOW = 0.67; - private static final double SCORE_THREE_LIFT = LIFT_DRIVE; + public static final double SCORE_THREE_SHOULDER = 0.92; + public static final double SCORE_THREE_WRIST = 0.91; + public static final double SCORE_THREE_ELBOW = 0.67; + public static final double SCORE_THREE_LIFT = LIFT_DRIVE; // score position four button map (gamepad2.x) - private static final double SCORE_FOUR_SHOULDER = 0.91; - private static final double SCORE_FOUR_WRIST =0.985; - private static final double SCORE_FOUR_ELBOW = 0.72; - private static final double SCORE_FOUR_LIFT = LIFT_DRIVE; + public static final double SCORE_FOUR_SHOULDER = 0.91; + public static final double SCORE_FOUR_WRIST =0.985; + public static final double SCORE_FOUR_ELBOW = 0.72; + public static final double SCORE_FOUR_LIFT = LIFT_DRIVE; // score position five button map (gamepad2.left_bumper && gamepad2.y) - private static final double SCORE_FIVE_SHOULDER = 0.92611111; - private static final double SCORE_FIVE_WRIST = 1; - private static final double SCORE_FIVE_ELBOW = 0.73; - private static final double SCORE_FIVE_LIFT = 0.59; + public static final double SCORE_FIVE_SHOULDER = 0.92611111; + public static final double SCORE_FIVE_WRIST = 1; + public static final double SCORE_FIVE_ELBOW = 0.73; + public static final double SCORE_FIVE_LIFT = 0.59; // score position six button map (gamepad2.left_bumper && gamepad2.b) private static final double SCORE_SIX_SHOULDER = 0.925; private static final double SCORE_SIX_WRIST = 1; @@ -153,6 +153,7 @@ private enum intakeState { // drive private enum driveState { IDLE, + MOVING_LIFT, MOVING_SHOULDER, MOVING_ELBOW, MOVING_WRIST, @@ -216,12 +217,21 @@ private void handleDriveSequence() { switch (currentDriveState) { case MOVING_SHOULDER: // Move the shoulder to drive position - moveServoGradually(shoulder, SHOULDER_DRIVE + .05); - shoulder.setPosition(SHOULDER_DRIVE); + //moveServoGradually(shoulder, SHOULDER_DRIVE + .05); + //shoulder.setPosition(SHOULDER_DRIVE); + moveServoGradually(shoulder, SHOULDER_DRIVE); if (isServoAtPosition(shoulder, SHOULDER_DRIVE)) { - currentDriveState = Mutation.driveState.MOVING_ELBOW; + currentDriveState = driveState.MOVING_LIFT; } break; + case MOVING_LIFT: + rightLift.setPosition(LIFT_DRIVE); + leftLift.setPosition(LIFT_DRIVE); + if (isServoAtPosition(rightLift, LIFT_DRIVE) && isServoAtPosition(leftLift, LIFT_DRIVE)) { + currentDriveState = driveState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: // Move the elbow to drive position leftFinger.setPosition(LEFT_FINGER_GRIP); @@ -407,19 +417,19 @@ private void scoringFunction() { currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position nine - if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.y) && (currentScoreState == Mutation.scoreState.IDLE)) { + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.y) && !gamepad2.left_bumper && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block activeScorePosition = new Mutation.ScorePosition(SCORE_NINE_SHOULDER, SCORE_NINE_WRIST, SCORE_NINE_ELBOW, SCORE_NINE_LIFT); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position ten - if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.b) && (currentScoreState == Mutation.scoreState.IDLE)) { + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.b) && !gamepad2.left_bumper && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block activeScorePosition = new Mutation.ScorePosition(SCORE_TEN_SHOULDER, SCORE_TEN_WRIST, SCORE_TEN_ELBOW, SCORE_TEN_LIFT); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position eleven - if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.a) && (currentScoreState == Mutation.scoreState.IDLE)) { + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.a) && !gamepad2.left_bumper && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block activeScorePosition = new Mutation.ScorePosition(SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_ELBOW, SCORE_ELEVEN_LIFT); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; @@ -636,7 +646,7 @@ public void runOpMode() throws InterruptedException { // claw drive position //TODO: add safety, this is drive around pos if (gamepad1.right_bumper && currentDriveState == Mutation.driveState.IDLE) { - currentDriveState = Mutation.driveState.MOVING_SHOULDER; + currentDriveState = driveState.MOVING_SHOULDER; } handleDriveSequence(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java index 2df311f65b..233f3fb8ae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -1,10 +1,11 @@ package org.firstinspires.ftc.teamcode; -import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; @@ -130,7 +131,7 @@ public class OdoMec extends LinearOpMode { private static final double SCORE_ELEVEN_WRIST = 0.93; private static final double SCORE_ELEVEN_ELBOW = 0.76; private static final double SCORE_ELEVEN_LIFT = 0.98; - + // sensors private RevTouchSensor rightUpper; private RevTouchSensor leftUpper; @@ -246,36 +247,6 @@ private void handleDriveSequence() { currentDriveState = OdoMec.driveState.IDLE; } } - private void driveCode() { - // mecanum drive - double forward = gamepad1.left_stick_y; - double strafe = -gamepad1.left_stick_x; - double turn = -gamepad1.right_stick_x; - - double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); - - double rightFrontPower = (forward - strafe - turn) / denominator; - double leftFrontPower = (forward + strafe + turn) / denominator; - double rightBackPower = (forward + strafe - turn) / denominator; - double leftBackPower = (forward - strafe + turn) / denominator; - - if (gamepad1.left_bumper) { - rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); - leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); - rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); - leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); - } else { - rightFrontPower = Range.clip(rightFrontPower, -1, 1); - leftFrontPower = Range.clip(leftFrontPower, -1, 1); - rightBackPower = Range.clip(rightBackPower, -1, 1); - leftBackPower = Range.clip(leftBackPower, -1, 1); - } - - rightFront.setPower(rightFrontPower); - leftFront.setPower(leftFrontPower); - rightBack.setPower(rightBackPower); - leftBack.setPower(leftBackPower); - } // score private void handleScorePosSequence(OdoMec.ScorePosition scorePos) { @@ -354,84 +325,6 @@ private boolean isServoAtPosition(Servo servo, double position) { return Math.abs(servo.getPosition() - position) < SERVO_TOLERANCE; } - private void scoringFunction() { - // scoring - // score position one - if (gamepad2.y && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, SCORE_ONE_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position two - if (gamepad2.b && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, SCORE_TWO_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position three - if (gamepad2.a && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_THREE_SHOULDER, SCORE_THREE_WRIST, SCORE_THREE_ELBOW, SCORE_THREE_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position four - //TODO: gotta put !gamepad2.left_bumper above - if (gamepad2.x && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, SCORE_FOUR_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position five - if ((gamepad2.left_bumper && gamepad2.y) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_FIVE_SHOULDER, SCORE_FIVE_WRIST, SCORE_FIVE_ELBOW, SCORE_FIVE_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position six - if ((gamepad2.left_bumper && gamepad2.b) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_SIX_SHOULDER, SCORE_SIX_WRIST, SCORE_SIX_ELBOW, SCORE_SIX_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position seven - if ((gamepad2.left_bumper && gamepad2.a) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_SEVEN_SHOULDER, SCORE_SEVEN_WRIST, SCORE_SEVEN_ELBOW, SCORE_SEVEN_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position eight - if ((gamepad2.left_bumper && gamepad2.x) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_EIGHT_SHOULDER, SCORE_EIGHT_WRIST, SCORE_EIGHT_ELBOW, SCORE_EIGHT_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position nine - if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.y) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_NINE_SHOULDER, SCORE_NINE_WRIST, SCORE_NINE_ELBOW, SCORE_NINE_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position ten - if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.b) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_TEN_SHOULDER, SCORE_TEN_WRIST, SCORE_TEN_ELBOW, SCORE_TEN_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - // score position eleven - if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.a) && (currentScoreState == OdoMec.scoreState.IDLE)) { - // Assign a new ScorePosition inside the if block - activeScorePosition = new OdoMec.ScorePosition(SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_ELBOW, SCORE_ELEVEN_LIFT); - currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; - } - if (activeScorePosition != null) { - handleScorePosSequence(activeScorePosition); - } - if (currentScoreState == OdoMec.scoreState.COMPLETED) { - currentScoreState = OdoMec.scoreState.IDLE; - activeScorePosition = null; // Reset the active position - } - } - private void moveServoGradually(Servo servo, double targetPosition) { double currentPosition = servo.getPosition(); @@ -452,96 +345,6 @@ private void moveServoGradually(Servo servo, double targetPosition) { } } - private void hangCode() { - // hanging - if (gamepad1.right_trigger > TRIGGER_THRESHOLD){ - if (!leftUpper.isPressed()) { - leftHang.setDirection(DcMotor.Direction.FORWARD); - leftHang.setPower(.9); - } - if (!rightUpper.isPressed()) { - rightHang.setDirection(DcMotor.Direction.FORWARD); - rightHang.setPower(.9); - } - } else if (gamepad1.left_trigger > TRIGGER_THRESHOLD){ - if (!leftLower.isPressed()) { - leftHang.setDirection(DcMotor.Direction.REVERSE); - leftHang.setPower(.9); - } - if (!rightLower.isPressed()) { - rightHang.setDirection(DcMotor.Direction.REVERSE); - rightHang.setPower(.9); - } - } else { - leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - leftHang.setPower(0); - rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightHang.setPower(0); - } - } - - private void intakeFunction() { - // intake - // claw intake from floor - //TODO: add saftey - if (gamepad1.left_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { - activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE); - currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; - } - // claw intake the top 2 from a stack of 5 - if (gamepad1.b && currentIntakeState == OdoMec.intakeState.IDLE) { - activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_TOP_TWO, WRIST_TOP_TWO, ELBOW_TOP_TWO); - currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; - } - // claw intake the next 2 from a stack of 3 - if (gamepad1.a && currentIntakeState == OdoMec.intakeState.IDLE) { - activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_NEXT_TWO, WRIST_NEXT_TWO, ELBOW_NEXT_TWO); - currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; - } - if (activeIntakePosition != null) { - handleIntakeSequence(activeIntakePosition); - } - if (currentIntakeState == OdoMec.intakeState.COMPLETED) { - currentIntakeState = OdoMec.intakeState.IDLE; - activeIntakePosition = null; // Reset the active position - } - - - // dropping on the backboard for scoring - if (gamepad2.dpad_up && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_DROP); - rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_left && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_DROP); - } else if (gamepad2.dpad_right && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { - rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_down && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - } - - // grabbing off the floor or stack - if (gamepad2.right_bumper && gamepad2.dpad_up && (currentScoreState == OdoMec.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_GRIP); - rightFinger.setPosition(RIGHT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentScoreState == OdoMec.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentScoreState == OdoMec.scoreState.IDLE)) { - rightFinger.setPosition(RIGHT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentScoreState == OdoMec.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - } - } - private void emergencyStop() { - if (gamepad1.y) { - leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - leftHang.setPower(0); - rightHang.setPower(0); - } - } - // lift (has been adjusted mechanically to use the same height) public void setLiftPosition(double targetPosition) { // Ensure the target position is within the valid range @@ -632,6 +435,70 @@ public void runOpMode() throws InterruptedException { while(opModeIsActive()){ + // mecanum drive + double forward = gamepad1.left_stick_y; + double strafe = -gamepad1.left_stick_x; + double turn = -gamepad1.right_stick_x; + + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); + + double rightFrontPower = (forward - strafe - turn) / denominator; + double leftFrontPower = (forward + strafe + turn) / denominator; + double rightBackPower = (forward + strafe - turn) / denominator; + double leftBackPower = (forward - strafe + turn) / denominator; + + if (gamepad1.left_bumper) { + rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); + leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); + rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); + leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); + } else { + rightFrontPower = Range.clip(rightFrontPower, -1, 1); + leftFrontPower = Range.clip(leftFrontPower, -1, 1); + rightBackPower = Range.clip(rightBackPower, -1, 1); + leftBackPower = Range.clip(leftBackPower, -1, 1); + } + + rightFront.setPower(rightFrontPower); + leftFront.setPower(leftFrontPower); + rightBack.setPower(rightBackPower); + leftBack.setPower(leftBackPower); + + // hanging + if (gamepad1.right_trigger > TRIGGER_THRESHOLD){ + if (!leftUpper.isPressed()) { + leftHang.setDirection(DcMotor.Direction.FORWARD); + leftHang.setPower(.9); + } + if (!rightUpper.isPressed()) { + rightHang.setDirection(DcMotor.Direction.FORWARD); + rightHang.setPower(.9); + } + } else if (gamepad1.left_trigger > TRIGGER_THRESHOLD){ + if (!leftLower.isPressed()) { + leftHang.setDirection(DcMotor.Direction.REVERSE); + leftHang.setPower(.9); + } + if (!rightLower.isPressed()) { + rightHang.setDirection(DcMotor.Direction.REVERSE); + rightHang.setPower(.9); + } + } else { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setPower(0); + } + + + // emergency stop + if (gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); + } + // claw drive position //TODO: add safety, this is drive around pos if (gamepad1.right_bumper && currentDriveState == OdoMec.driveState.IDLE) { @@ -639,9 +506,136 @@ public void runOpMode() throws InterruptedException { } handleDriveSequence(); + // intake + // claw intake from floor + //TODO: add saftey, this is intake from stuff + if (gamepad1.left_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { + activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE); + currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; + } + // claw intake the top 2 from a stack of 5 + if (gamepad1.b && currentIntakeState == OdoMec.intakeState.IDLE) { + activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_TOP_TWO, WRIST_TOP_TWO, ELBOW_TOP_TWO); + currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; + } + // claw intake the next 2 from a stack of 3 + if (gamepad1.a && currentIntakeState == OdoMec.intakeState.IDLE) { + activeIntakePosition = new OdoMec.IntakePosition(SHOULDER_NEXT_TWO, WRIST_NEXT_TWO, ELBOW_NEXT_TWO); + currentIntakeState = OdoMec.intakeState.MOVING_SHOULDER; + } + if (activeIntakePosition != null) { + handleIntakeSequence(activeIntakePosition); + } + if (currentIntakeState == OdoMec.intakeState.COMPLETED) { + currentIntakeState = OdoMec.intakeState.IDLE; + activeIntakePosition = null; // Reset the active position + } + + // scoring + // score position one + if (gamepad2.y && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, SCORE_ONE_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position two + if (gamepad2.b && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, SCORE_TWO_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position three + if (gamepad2.a && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_THREE_SHOULDER, SCORE_THREE_WRIST, SCORE_THREE_ELBOW, SCORE_THREE_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position four + //TODO: gotta put !gamepad2.left_bumper above + if (gamepad2.x && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, SCORE_FOUR_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position five + if ((gamepad2.left_bumper && gamepad2.y) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_FIVE_SHOULDER, SCORE_FIVE_WRIST, SCORE_FIVE_ELBOW, SCORE_FIVE_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position six + if ((gamepad2.left_bumper && gamepad2.b) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_SIX_SHOULDER, SCORE_SIX_WRIST, SCORE_SIX_ELBOW, SCORE_SIX_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position seven + if ((gamepad2.left_bumper && gamepad2.a) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_SEVEN_SHOULDER, SCORE_SEVEN_WRIST, SCORE_SEVEN_ELBOW, SCORE_SEVEN_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position eight + if ((gamepad2.left_bumper && gamepad2.x) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_EIGHT_SHOULDER, SCORE_EIGHT_WRIST, SCORE_EIGHT_ELBOW, SCORE_EIGHT_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position nine + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.y) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_NINE_SHOULDER, SCORE_NINE_WRIST, SCORE_NINE_ELBOW, SCORE_NINE_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position ten + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.b) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_TEN_SHOULDER, SCORE_TEN_WRIST, SCORE_TEN_ELBOW, SCORE_TEN_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + // score position eleven + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.a) && (currentScoreState == OdoMec.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new OdoMec.ScorePosition(SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_ELBOW, SCORE_ELEVEN_LIFT); + currentScoreState = OdoMec.scoreState.MOVING_SHOULDER; + } + if (activeScorePosition != null) { + handleScorePosSequence(activeScorePosition); + } + if (currentScoreState == OdoMec.scoreState.COMPLETED) { + currentScoreState = OdoMec.scoreState.IDLE; + activeScorePosition = null; // Reset the active position + } + + // dropping on the backboard for scoring + if (gamepad2.dpad_up && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_left && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + } else if (gamepad2.dpad_right && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_down && !gamepad2.right_bumper && currentIntakeState == OdoMec.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + + // grabbing off the floor or stack + if (gamepad2.right_bumper && gamepad2.dpad_up && (currentScoreState == OdoMec.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentScoreState == OdoMec.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentScoreState == OdoMec.scoreState.IDLE)) { + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentScoreState == OdoMec.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } // telemetry telemetry.addData("Status", "Run " + runtime.toString()); + telemetry.addData("Motors", "forward (%.2f), strafe (%.2f),turn (%.2f)", forward, strafe, turn); telemetry.addData("Intake", currentIntakeState); telemetry.addData("Drive", currentDriveState); telemetry.addData("Score", currentScoreState); @@ -653,10 +647,6 @@ public void runOpMode() throws InterruptedException { // launcher airplane(); - driveCode(); - scoringFunction(); - intakeFunction(); - emergencyStop(); telemetry.update(); } From f41214733c2f30be447a23ee1dc74cbca89e9b84 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sun, 10 Dec 2023 14:13:58 -0700 Subject: [PATCH 115/154] Now the elbow? C'mon now. Added new poses up to Score_Four --- .../firstinspires/ftc/teamcode/Mutation.java | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 5bf5920ddb..784caf75ff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -44,10 +44,10 @@ public class Mutation extends LinearOpMode { public static final double SHOULDER_DRIVE = 0.425; // 0.425 public static final double SHOULDER_INT = 0.43; // 0.455 public static final double ELBOW_DRIVE= 0.5; - public static final double ELBOW_INTAKE = 0.83; + public static final double ELBOW_INTAKE = 0.8; public static final double WRIST_INT = 0.55; // int for auto & testing 0.55; // int for DRIVE 0.265 - public static final double WRIST_DRIVE = 0.265; // int for auto & testing 0.55; // int for DRIVE 0.265 - public static final double WRIST_INTAKE = 0.625; + public static final double WRIST_DRIVE = 0.82; // int for auto & testing 0.55; // int for DRIVE 0.265 + public static final double WRIST_INTAKE = 0.545; public static final double LEFT_FINGER_GRIP = 0.74; public static final double LEFT_FINGER_DROP = 0.5; public static final double LEFT_FINGER_INTAKE = 0.34; @@ -77,25 +77,25 @@ public class Mutation extends LinearOpMode { // score positions (11 rows on the board) // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin // score position one button map (gamepad2.y) - public static final double SCORE_ONE_SHOULDER = 0.936; - public static final double SCORE_ONE_WRIST = 0.685; - public static final double SCORE_ONE_ELBOW = 0.52; + public static final double SCORE_ONE_SHOULDER = 0.915; + public static final double SCORE_ONE_WRIST = 0.365; + public static final double SCORE_ONE_ELBOW = 0.57; public static final double SCORE_ONE_LIFT = LIFT_DRIVE; // score position two button map (gamepad2.b) - public static final double SCORE_TWO_SHOULDER = 0.9; - public static final double SCORE_TWO_WRIST = 0.745; - public static final double SCORE_TWO_ELBOW = 0.55; - public static final double SCORE_TWO_LIFT = LIFT_DRIVE; + public static final double SCORE_TWO_SHOULDER = SCORE_ONE_SHOULDER; + public static final double SCORE_TWO_WRIST = SCORE_ONE_WRIST; + public static final double SCORE_TWO_ELBOW = SCORE_ONE_ELBOW; + public static final double SCORE_TWO_LIFT = 0.57; // score position three button map (gamepad2.a) - public static final double SCORE_THREE_SHOULDER = 0.92; - public static final double SCORE_THREE_WRIST = 0.91; - public static final double SCORE_THREE_ELBOW = 0.67; - public static final double SCORE_THREE_LIFT = LIFT_DRIVE; + public static final double SCORE_THREE_SHOULDER = SCORE_ONE_SHOULDER; + public static final double SCORE_THREE_WRIST = SCORE_ONE_WRIST; + public static final double SCORE_THREE_ELBOW = SCORE_ONE_ELBOW; + public static final double SCORE_THREE_LIFT = 0.68; // score position four button map (gamepad2.x) - public static final double SCORE_FOUR_SHOULDER = 0.91; - public static final double SCORE_FOUR_WRIST =0.985; - public static final double SCORE_FOUR_ELBOW = 0.72; - public static final double SCORE_FOUR_LIFT = LIFT_DRIVE; + public static final double SCORE_FOUR_SHOULDER = SCORE_ONE_SHOULDER; + public static final double SCORE_FOUR_WRIST = SCORE_ONE_WRIST; + public static final double SCORE_FOUR_ELBOW = SCORE_ONE_ELBOW; + public static final double SCORE_FOUR_LIFT = 0.75; // score position five button map (gamepad2.left_bumper && gamepad2.y) public static final double SCORE_FIVE_SHOULDER = 0.92611111; public static final double SCORE_FIVE_WRIST = 1; From 0efa33c64ee5697ee00bc1cbbb54e1ed5739201e Mon Sep 17 00:00:00 2001 From: digitalbreadllc Date: Mon, 11 Dec 2023 17:47:44 -0700 Subject: [PATCH 116/154] new ArmWork stuff and some servo value adjustments --- .../firstinspires/ftc/teamcode/ArmWork.java | 468 ++++++++++++++++++ .../firstinspires/ftc/teamcode/McMuffin.java | 4 +- .../firstinspires/ftc/teamcode/Mutation.java | 12 +- .../firstinspires/ftc/teamcode/OdoMec.java | 4 +- 4 files changed, 476 insertions(+), 12 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmWork.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmWork.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmWork.java new file mode 100644 index 0000000000..4730cc0529 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmWork.java @@ -0,0 +1,468 @@ +package org.firstinspires.ftc.teamcode; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; +@TeleOp +public class ArmWork extends LinearOpMode{ + // Declare vars + // timers + private ElapsedTime runtime = new ElapsedTime(); + private ElapsedTime servoTimer = new ElapsedTime(); + + private Servo shoulder; + private Servo wrist; + private Servo elbow; + private Servo leftFinger; + private Servo rightFinger; + private Servo rightLift; + private Servo leftLift; + private double servoposition = 0.0; + private double servodelta = 0.02; + private double servodelaytime = 0.03; + + // servo values + private static final double SHOULDER_DRIVE = 0.425; // 0.425 + private static final double SHOULDER_INT = 0.43; // 0.455 + private static final double ELBOW_DRIVE= 0.47; + private static final double ELBOW_INTAKE = 0.82; + private static final double WRIST_INT = 0.55; // int for auto & testing 0.55; // int for DRIVE 0.265 + private static final double WRIST_DRIVE = 0.825; // int for auto & testing 0.55; // int for DRIVE 0.265 + private static final double WRIST_INTAKE = 0.525; + private static final double LEFT_FINGER_GRIP = 0.74; + private static final double LEFT_FINGER_DROP = 0.5; + private static final double LEFT_FINGER_INTAKE = 0.34; + private static final double RIGHT_FINGER_GRIP = .27; + private static final double RIGHT_FINGER_DROP = .5; + private static final double RIGHT_FINGER_INTAKE = 0.64; + private static final double TRIGGER_THRESHOLD = 0.5; + private static final double LAUNCHER_START_POS = 0.8; + private static final double SERVO_TOLERANCE = 0.01; + private static final double LIFT_DRIVE = 0.069; + private double liftTargetPosition = LIFT_DRIVE; + + // score position one button map (gamepad2.y) + private static final double SCORE_ONE_SHOULDER = 0.936; + private static final double SCORE_ONE_WRIST = 0.685; + private static final double SCORE_ONE_ELBOW = 0.52; + private static final double SCORE_ONE_LIFT = LIFT_DRIVE; + // score position two button map (gamepad2.b) + private static final double SCORE_TWO_SHOULDER = 0.9; + private static final double SCORE_TWO_WRIST = 0.745; + private static final double SCORE_TWO_ELBOW = 0.55; + private static final double SCORE_TWO_LIFT = LIFT_DRIVE; + // score position three button map (gamepad2.a) + private static final double SCORE_THREE_SHOULDER = 0.92; + private static final double SCORE_THREE_WRIST = 0.91; + private static final double SCORE_THREE_ELBOW = 0.67; + private static final double SCORE_THREE_LIFT = LIFT_DRIVE; + // score position four button map (gamepad2.x) + private static final double SCORE_FOUR_SHOULDER = 0.91; + private static final double SCORE_FOUR_WRIST =0.985; + private static final double SCORE_FOUR_ELBOW = 0.72; + private static final double SCORE_FOUR_LIFT = LIFT_DRIVE; + private static final double LOW_ACC = 7.25; + private static final double LOW_VEL = 8.5; + private static final double MED_ACC = 10.25; + private static final double MED_VEL = 11.5; + private static final double HIGH_ACC = 11.25; + private static final double HIGH_VEL = 13.5; + + // state machines enumeration + // intake + + private enum intakeState { + IDLE, + MOVING_LIFT, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + COMPLETED + } + + private ArmWork.intakeState currentintakeState = ArmWork.intakeState.IDLE; + private ArmWork.IntakePosition activeIntakePosition = null; + + + static class IntakePosition { + double liftPosition; + double shoulderPosition; + double wristPosition; + double elbowPosition; + double accelerationMax; + double velocityMax; + + public IntakePosition(double liftPos, double shoulderPos, double wristPos, double elbowPos, double accMax, double velMax) { + this.liftPosition = liftPos; + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + this.accelerationMax = accMax; + this.velocityMax = velMax; + } + } + + private void handleIntakeSequence(ArmWork.IntakePosition intakePos) { + switch (currentintakeState) { + case MOVING_SHOULDER: + // Move the shoulder to intake position + moveServoWithTrapezoidalVelocity(shoulder, intakePos.shoulderPosition, intakePos.accelerationMax, intakePos.velocityMax); + if (isServoAtPosition(shoulder, intakePos.shoulderPosition, SERVO_TOLERANCE)) { + currentintakeState = intakeState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to intake position + moveServoWithTrapezoidalVelocity(wrist, intakePos.wristPosition, intakePos.accelerationMax, intakePos.velocityMax); + if (isServoAtPosition(wrist, intakePos.wristPosition, SERVO_TOLERANCE)) { + currentintakeState = intakeState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: + // Move the elbow to intake position so it don't slap the floor + moveServoWithTrapezoidalVelocity(elbow, intakePos.elbowPosition, intakePos.accelerationMax, intakePos.velocityMax); + if (isServoAtPosition(elbow, intakePos.elbowPosition, SERVO_TOLERANCE)) { + // Check if the elbow is 70% down and open the claws if it is + if (elbow.getPosition() > (intakePos.elbowPosition - 0.4)) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + currentintakeState = ArmWork.intakeState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; + } + // Check to reset the state to IDLE outside the switch + if (currentintakeState == ArmWork.intakeState.COMPLETED) { + currentintakeState = ArmWork.intakeState.IDLE; + } + } + + + private enum driveState { + IDLE, + MOVING_LIFT, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + COMPLETED + } + + private ArmWork.driveState currentdriveState = ArmWork.driveState.IDLE; + private ArmWork.DrivePosition activeDrivePosition = null; + + + static class DrivePosition { + double liftPosition; + double shoulderPosition; + double wristPosition; + double elbowPosition; + double accelerationMax; + double velocityMax; + + public DrivePosition(double liftPos, double shoulderPos, double wristPos, double elbowPos, double accMax, double velMax) { + this.liftPosition = liftPos; + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + this.accelerationMax = accMax; + this.velocityMax = velMax; + } + } + private void handleDriveSequence(ArmWork.DrivePosition drivePos) { + switch (currentdriveState) { + case MOVING_LIFT: + setLiftPosition(LIFT_DRIVE); + break; + case MOVING_SHOULDER: + // Move the shoulder to intake position + moveServoWithTrapezoidalVelocity(shoulder, drivePos.shoulderPosition, drivePos.accelerationMax, drivePos.velocityMax); + if (isServoAtPosition(shoulder, drivePos.shoulderPosition, SERVO_TOLERANCE)) { + currentdriveState = ArmWork.driveState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: + // Move the elbow to intake position so it don't slap the floor + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + moveServoWithTrapezoidalVelocity(elbow, drivePos.elbowPosition, drivePos.accelerationMax, drivePos.velocityMax); + if (isServoAtPosition(elbow, drivePos.elbowPosition, SERVO_TOLERANCE)) { + currentdriveState = ArmWork.driveState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to intake position + moveServoWithTrapezoidalVelocity(wrist, drivePos.wristPosition, drivePos.accelerationMax, drivePos.velocityMax); + if (isServoAtPosition(wrist, drivePos.wristPosition, SERVO_TOLERANCE)) { + currentdriveState = driveState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; + } + // Check to reset the state to IDLE outside the switch + if (currentdriveState == ArmWork.driveState.COMPLETED) { + currentdriveState = ArmWork.driveState.IDLE; + } + } + + private enum scoreState { + IDLE, + MOVING_LIFT, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + COMPLETED + } + + private ArmWork.scoreState currentscoreState = ArmWork.scoreState.IDLE; + private ArmWork.ScorePosition activeScorePosition = null; + + + static class ScorePosition { + double liftPosition; + double shoulderPosition; + double wristPosition; + double elbowPosition; + double accelerationMax; + double velocityMax; + + public ScorePosition(double liftPos, double shoulderPos, double wristPos, double elbowPos, double accMax, double velMax) { + this.liftPosition = liftPos; + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + this.accelerationMax = accMax; + this.velocityMax = velMax; + } + } + private void handleScoreSequence(ArmWork.ScorePosition scorePos) { + switch (currentscoreState) { + + case MOVING_SHOULDER: + // Move the shoulder to intake position + moveServoWithTrapezoidalVelocity(shoulder, scorePos.shoulderPosition, scorePos.accelerationMax, scorePos.velocityMax); + if (isServoAtPosition(shoulder, scorePos.shoulderPosition, SERVO_TOLERANCE)) { + currentscoreState = ArmWork.scoreState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: + // Move the elbow to intake position so it don't slap the floor + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + moveServoWithTrapezoidalVelocity(elbow, scorePos.elbowPosition, scorePos.accelerationMax, scorePos.velocityMax); + if (isServoAtPosition(elbow, scorePos.elbowPosition, SERVO_TOLERANCE)) { + currentscoreState = ArmWork.scoreState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to intake position + moveServoWithTrapezoidalVelocity(wrist, scorePos.wristPosition, scorePos.accelerationMax, scorePos.velocityMax); + if (isServoAtPosition(wrist, scorePos.wristPosition, SERVO_TOLERANCE)) { + currentscoreState = scoreState.MOVING_LIFT; + } + break; + case MOVING_LIFT: + setLiftPosition(scorePos.liftPosition); + if (isServoAtPosition(leftLift, scorePos.liftPosition, SERVO_TOLERANCE) && isServoAtPosition(rightLift, scorePos.liftPosition, SERVO_TOLERANCE)) { + currentscoreState = scoreState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; + } + // Check to reset the state to IDLE outside the switch + if (currentscoreState == ArmWork.scoreState.COMPLETED) { + currentscoreState = ArmWork.scoreState.IDLE; + } + } + // functions to support state machine actions + private boolean isServoAtPosition(Servo servo, double targetPosition, double tolerance) { + double currentPosition = servo.getPosition(); + double normalizedTarget = Range.clip(targetPosition, 0.0, 1.0); // Ensure target is within valid range + double normalizedCurrent = Range.clip(currentPosition, 0.0, 1.0); // Ensure current position is within valid range + + return Math.abs(normalizedCurrent - normalizedTarget) < tolerance; + } + + + private void moveServoWithTrapezoidalVelocity(Servo servo, double targetPosition, double amax, double vmax) { + double currentPosition = servo.getPosition(); + double currentVelocity = 0.0; // Initial velocity is zero + double p0 = 0.0; // Distance needed to come to rest + + ElapsedTime timer = new ElapsedTime(); + timer.reset(); + + while (Math.abs(currentPosition - targetPosition) > SERVO_TOLERANCE && opModeIsActive()) { + // Calculate the elapsed time since the last update + double elapsedTime = timer.seconds(); + timer.reset(); + + // Calculate the distance needed to come to rest given the current velocity + p0 = 2 * currentVelocity + Math.abs(currentVelocity) * currentVelocity / (2 * amax); + + // Calculate the desired velocity based on the position error + double velocityError = targetPosition - currentPosition - p0; + double sign = Math.signum(velocityError); + double desiredVelocity = currentVelocity + sign * amax; + desiredVelocity = Range.clip(desiredVelocity, -vmax, vmax); // Constrain to vmax + + // Update the servo position based on the desired velocity and elapsed time + currentPosition += desiredVelocity * elapsedTime; + currentPosition = Range.clip(currentPosition, 0, 1); // Ensure the position is within valid range + servo.setPosition(currentPosition); + + // Update the current velocity for the next iteration + currentVelocity = desiredVelocity; + + telemetry.addData("Servo Position", currentPosition); + telemetry.update(); + } + } + + private void moveServoGradually(Servo servo, double targetPosition) { + double currentPosition = servo.getPosition(); + + // Check if enough time has passed since the last update + if (servoTimer.seconds() > servodelaytime) { + // Determine the direction of movement + double direction = targetPosition > currentPosition ? servodelta : -servodelta; + + // Calculate the new position + servoposition = currentPosition + direction; + servoposition = Range.clip(servoposition, 0, 1); // Ensure the position is within valid range + + // Update the servo position + servo.setPosition(servoposition); + + // Reset the timer + servoTimer.reset(); + } + } + + // lift (has been adjusted mechanically to use the same height) + public Servo setLiftPosition(double targetPosition) { + // Ensure the target position is within the valid range + targetPosition = Math.max(0.0, Math.min(targetPosition, 1.0)); + + // Set the servo positions + leftLift.setPosition(targetPosition); + rightLift.setPosition(targetPosition); + return null; + } + + @Override + + public void runOpMode() throws InterruptedException { + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + // servos + shoulder = hardwareMap.get(Servo.class, "shoulder"); + wrist = hardwareMap.get(Servo.class, "wrist"); + elbow = hardwareMap.get(Servo.class, "elbow"); + leftFinger = hardwareMap.get(Servo.class, "lFinger"); + rightFinger = hardwareMap.get(Servo.class, "rFinger"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + + // servo modes + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + + // init positions + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + setLiftPosition(LIFT_DRIVE); + shoulder.setPosition(SHOULDER_INT); + elbow.setPosition(ELBOW_DRIVE); + wrist.setPosition(WRIST_DRIVE); + + telemetry.addData("Status", "Armwork is ready to run!"); + telemetry.addData("Initializing TeleOp",""); + + waitForStart(); + runtime.reset(); + + while (opModeIsActive()) { + // Check if the left bumper is pressed and the intake state is IDLE + if (gamepad1.left_bumper && currentintakeState == ArmWork.intakeState.IDLE) { + activeIntakePosition = new ArmWork.IntakePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE, MED_ACC, MED_VEL); + currentintakeState = ArmWork.intakeState.MOVING_SHOULDER; + } + + // Check if the right bumper is pressed and the drive state is IDLE + if (gamepad1.right_bumper && currentdriveState == driveState.IDLE) { + activeDrivePosition = new ArmWork.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_DRIVE, ELBOW_DRIVE, MED_ACC, MED_VEL); + currentdriveState = ArmWork.driveState.MOVING_SHOULDER; + } + + if (activeIntakePosition != null) { + handleIntakeSequence(activeIntakePosition); + } + + if (currentintakeState == ArmWork.intakeState.COMPLETED) { + currentintakeState = ArmWork.intakeState.IDLE; + activeIntakePosition = null; // Reset the active position + } + + if (activeDrivePosition != null) { + handleDriveSequence(activeDrivePosition); + } + + if (currentdriveState == ArmWork.driveState.COMPLETED) { + currentdriveState = ArmWork.driveState.IDLE; + activeDrivePosition = null; // Reset the active position + } + + // score position one + if (gamepad2.y && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentscoreState == ArmWork.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new ArmWork.ScorePosition(SCORE_ONE_LIFT, SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, MED_ACC, MED_VEL); + currentscoreState = ArmWork.scoreState.MOVING_SHOULDER; + } + // score position two + if (gamepad2.b && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentscoreState == ArmWork.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new ArmWork.ScorePosition(SCORE_TWO_LIFT, SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, MED_ACC, MED_VEL); + currentscoreState = ArmWork.scoreState.MOVING_SHOULDER; + } + // score position three + if (gamepad2.a && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentscoreState == ArmWork.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new ArmWork.ScorePosition(SCORE_THREE_LIFT, SCORE_THREE_SHOULDER, SCORE_THREE_WRIST, SCORE_THREE_ELBOW, MED_ACC, MED_VEL); + currentscoreState = ArmWork.scoreState.MOVING_SHOULDER; + } + // score position four + //TODO: gotta put !gamepad2.left_bumper above + if (gamepad2.x && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentscoreState == ArmWork.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new ArmWork.ScorePosition(SCORE_FOUR_LIFT, SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, MED_ACC, MED_VEL); + currentscoreState = ArmWork.scoreState.MOVING_SHOULDER; + } + + if (activeScorePosition != null) { + handleScoreSequence(activeScorePosition); + } + if (currentscoreState == ArmWork.scoreState.COMPLETED) { + currentscoreState = ArmWork.scoreState.IDLE; + activeScorePosition = null; // Reset the active position + } + + telemetry.addData("Intake", currentintakeState); + telemetry.addData("Drive", currentdriveState); + telemetry.addData("Shoulder Position", shoulder.getPosition()); + telemetry.addData("Wrist Position", wrist.getPosition()); + telemetry.addData("Elbow Position", elbow.getPosition()); + telemetry.addData("Intake State", currentintakeState); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java index 5e4c8faaf8..e658e4d88c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/McMuffin.java @@ -145,8 +145,8 @@ public void runOpMode() throws InterruptedException { elbow.setPosition(0.5); rightFinger.setPosition(0.27); leftFinger.setPosition(0.74); - leftLift.setPosition(0.37); - rightLift.setPosition(0.37); + leftLift.setPosition(0.06); + rightLift.setPosition(0.06); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 784caf75ff..238ce51287 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -33,8 +33,6 @@ public class Mutation extends LinearOpMode { private Servo rightFinger; private Servo rightLift; private Servo leftLift; - double LiftLeftOffset = -.05; - double LiftHeight; private double servoposition = 0.0; private double servodelta = 0.02; private double servodelaytime = 0.03; @@ -57,8 +55,8 @@ public class Mutation extends LinearOpMode { public static final double TRIGGER_THRESHOLD = 0.5; public static final double LAUNCHER_START_POS = 0.8; public static final double SERVO_TOLERANCE = 0.01; - public static final double LIFT_DRIVE = 0.37; - private double liftTargetPosition = LIFT_DRIVE; + public static final double LIFT_DRIVE = 0.069; // 0.78 is the highest it can mechanically go right now + private double liftTargetPosition = LIFT_DRIVE; // was 0.37 before moving servos for larger range // stack positions (top 2 o 5 and next 2 of 3 ) // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin @@ -85,17 +83,17 @@ public class Mutation extends LinearOpMode { public static final double SCORE_TWO_SHOULDER = SCORE_ONE_SHOULDER; public static final double SCORE_TWO_WRIST = SCORE_ONE_WRIST; public static final double SCORE_TWO_ELBOW = SCORE_ONE_ELBOW; - public static final double SCORE_TWO_LIFT = 0.57; + public static final double SCORE_TWO_LIFT = 0.26; // score position three button map (gamepad2.a) public static final double SCORE_THREE_SHOULDER = SCORE_ONE_SHOULDER; public static final double SCORE_THREE_WRIST = SCORE_ONE_WRIST; public static final double SCORE_THREE_ELBOW = SCORE_ONE_ELBOW; - public static final double SCORE_THREE_LIFT = 0.68; + public static final double SCORE_THREE_LIFT = 0.37; // score position four button map (gamepad2.x) public static final double SCORE_FOUR_SHOULDER = SCORE_ONE_SHOULDER; public static final double SCORE_FOUR_WRIST = SCORE_ONE_WRIST; public static final double SCORE_FOUR_ELBOW = SCORE_ONE_ELBOW; - public static final double SCORE_FOUR_LIFT = 0.75; + public static final double SCORE_FOUR_LIFT = .44; // score position five button map (gamepad2.left_bumper && gamepad2.y) public static final double SCORE_FIVE_SHOULDER = 0.92611111; public static final double SCORE_FIVE_WRIST = 1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java index 0c6974e704..70a2470dfc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -34,8 +34,6 @@ public class OdoMec extends LinearOpMode { private Servo rightFinger; private Servo rightLift; private Servo leftLift; - double LiftLeftOffset = -.05; - double LiftHeight; private double servoposition = 0.0; private double servodelta = 0.02; private double servodelaytime = 0.03; @@ -58,7 +56,7 @@ public class OdoMec extends LinearOpMode { private static final double TRIGGER_THRESHOLD = 0.5; private static final double LAUNCHER_START_POS = 0.8; private static final double SERVO_TOLERANCE = 0.01; - private static final double LIFT_DRIVE = 0.37; + private static final double LIFT_DRIVE = 0.06; private double liftTargetPosition = LIFT_DRIVE; // stack positions (top 2 o 5 and next 2 of 3 ) From 44a97083f0bfa52fbbb8f5f51d9310468a2c2a86 Mon Sep 17 00:00:00 2001 From: russmc Date: Tue, 12 Dec 2023 10:25:28 -0700 Subject: [PATCH 117/154] went through and checked the code, reorganized, functions to be with states, added new servo movement code and updated positions to include acc & vel --- .../firstinspires/ftc/teamcode/Mutation.java | 532 +++++++++++------- 1 file changed, 313 insertions(+), 219 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 238ce51287..5309f6c950 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -37,6 +37,13 @@ public class Mutation extends LinearOpMode { private double servodelta = 0.02; private double servodelaytime = 0.03; + private static final double LOW_ACC = 7.25; + private static final double LOW_VEL = 8.5; + private static final double MED_ACC = 10.25; + private static final double MED_VEL = 11.5; + private static final double HIGH_ACC = 11.25; + private static final double HIGH_VEL = 13.5; + // servo values public static final double SHOULDER_DRIVE = 0.425; // 0.425 @@ -137,67 +144,65 @@ public class Mutation extends LinearOpMode { private RevTouchSensor leftLower; - // state machines enumeration - // intake + // state machines - states enumeration, positions, handle sequences, functions + // INTAKE private enum intakeState { IDLE, + MOVING_LIFT, MOVING_SHOULDER, MOVING_WRIST, MOVING_ELBOW, COMPLETED } + private Mutation.intakeState currentIntakeState = Mutation.intakeState.IDLE; private Mutation.IntakePosition activeIntakePosition = null; - // drive - private enum driveState { - IDLE, - MOVING_LIFT, - MOVING_SHOULDER, - MOVING_ELBOW, - MOVING_WRIST, - COMPLETED - } - private Mutation.driveState currentDriveState = Mutation.driveState.IDLE; - // score - private enum scoreState { - IDLE, - MOVING_SHOULDER, - MOVING_WRIST, - MOVING_ELBOW, - MOVING_LIFT, - COMPLETED - } - private Mutation.scoreState currentScoreState = Mutation.scoreState.IDLE; - private Mutation.ScorePosition activeScorePosition = null; + static class IntakePosition { + double liftPosition; + double shoulderPosition; + double wristPosition; + double elbowPosition; + double accelerationMax; + double velocityMax; + + public IntakePosition(double liftPos, double shoulderPos, double wristPos, double elbowPos, double accMax, double velMax) { + this.liftPosition = liftPos; + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + this.accelerationMax = accMax; + this.velocityMax = velMax; + } + } - // state machine sequences to provide cascading actions with a single input (button press) - // intake private void handleIntakeSequence(Mutation.IntakePosition intakePos) { switch (currentIntakeState) { case MOVING_SHOULDER: // Move the shoulder to intake position - moveServoGradually(shoulder, intakePos.shoulderPosition + .05); - shoulder.setPosition(intakePos.shoulderPosition); - if (isServoAtPosition(shoulder, intakePos.shoulderPosition)) { + moveServoWithTrapezoidalVelocity(shoulder, intakePos.shoulderPosition, intakePos.accelerationMax, intakePos.velocityMax); + if (isServoAtPosition(shoulder, intakePos.shoulderPosition, SERVO_TOLERANCE)) { currentIntakeState = Mutation.intakeState.MOVING_WRIST; } break; case MOVING_WRIST: // Move the wrist to intake position - wrist.setPosition(intakePos.wristPosition); - if (isServoAtPosition(wrist, intakePos.wristPosition)) { - currentIntakeState = intakeState.MOVING_ELBOW; + moveServoWithTrapezoidalVelocity(wrist, intakePos.wristPosition, intakePos.accelerationMax, intakePos.velocityMax); + if (isServoAtPosition(wrist, intakePos.wristPosition, SERVO_TOLERANCE)) { + currentIntakeState = Mutation.intakeState.MOVING_ELBOW; } break; case MOVING_ELBOW: // Move the elbow to intake position so it don't slap the floor - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - moveServoGradually(elbow, intakePos.elbowPosition); - if (isServoAtPosition(elbow, intakePos.elbowPosition)) { - currentIntakeState = intakeState.COMPLETED; + moveServoWithTrapezoidalVelocity(elbow, intakePos.elbowPosition, intakePos.accelerationMax, intakePos.velocityMax); + if (isServoAtPosition(elbow, intakePos.elbowPosition, SERVO_TOLERANCE)) { + // Check if the elbow is 70% down and open the claws if it is + if (elbow.getPosition() > (intakePos.elbowPosition - 0.4)) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + currentIntakeState = Mutation.intakeState.COMPLETED; } break; case COMPLETED: @@ -210,39 +215,116 @@ private void handleIntakeSequence(Mutation.IntakePosition intakePos) { } } - // drive - private void handleDriveSequence() { + private void intakeFunction() { + // intake + // claw intake from floor + //TODO: add saftey + if (gamepad1.left_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + activeIntakePosition = new Mutation.IntakePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE, MED_ACC, MED_VEL); + currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; + } + // claw intake the top 2 from a stack of 5 + if (gamepad1.b && currentIntakeState == Mutation.intakeState.IDLE) { + activeIntakePosition = new Mutation.IntakePosition(LIFT_DRIVE, SHOULDER_TOP_TWO, WRIST_TOP_TWO, ELBOW_TOP_TWO, MED_ACC, MED_VEL); + currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; + } + // claw intake the next 2 from a stack of 3 + if (gamepad1.a && currentIntakeState == Mutation.intakeState.IDLE) { + activeIntakePosition = new Mutation.IntakePosition(LIFT_DRIVE, SHOULDER_NEXT_TWO, WRIST_NEXT_TWO, ELBOW_NEXT_TWO, MED_ACC, MED_VEL); + currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; + } + if (activeIntakePosition != null) { + handleIntakeSequence(activeIntakePosition); + } + if (currentIntakeState == Mutation.intakeState.COMPLETED) { + currentIntakeState = Mutation.intakeState.IDLE; + activeIntakePosition = null; // Reset the active position + } + + + // dropping on the backboard for scoring + if (gamepad2.dpad_up && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_left && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + } else if (gamepad2.dpad_right && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_down && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + + // grabbing off the floor or stack + if (gamepad2.right_bumper && gamepad2.dpad_up && (currentScoreState == Mutation.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentScoreState == Mutation.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentScoreState == Mutation.scoreState.IDLE)) { + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentScoreState == Mutation.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + } + + // DRIVE + private enum driveState { + IDLE, + MOVING_LIFT, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + COMPLETED + } + + private Mutation.driveState currentDriveState = Mutation.driveState.IDLE; + private Mutation.DrivePosition activeDrivePosition = null; + + + static class DrivePosition { + double liftPosition; + double shoulderPosition; + double wristPosition; + double elbowPosition; + double accelerationMax; + double velocityMax; + + public DrivePosition(double liftPos, double shoulderPos, double wristPos, double elbowPos, double accMax, double velMax) { + this.liftPosition = liftPos; + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + this.accelerationMax = accMax; + this.velocityMax = velMax; + } + } + private void handleDriveSequence(Mutation.DrivePosition drivePos) { switch (currentDriveState) { - case MOVING_SHOULDER: - // Move the shoulder to drive position - //moveServoGradually(shoulder, SHOULDER_DRIVE + .05); - //shoulder.setPosition(SHOULDER_DRIVE); - moveServoGradually(shoulder, SHOULDER_DRIVE); - if (isServoAtPosition(shoulder, SHOULDER_DRIVE)) { - currentDriveState = driveState.MOVING_LIFT; - } - break; case MOVING_LIFT: - rightLift.setPosition(LIFT_DRIVE); - leftLift.setPosition(LIFT_DRIVE); - if (isServoAtPosition(rightLift, LIFT_DRIVE) && isServoAtPosition(leftLift, LIFT_DRIVE)) { - currentDriveState = driveState.MOVING_ELBOW; + setLiftPosition(LIFT_DRIVE); + break; + case MOVING_SHOULDER: + // Move the shoulder to intake position + moveServoWithTrapezoidalVelocity(shoulder, drivePos.shoulderPosition, drivePos.accelerationMax, drivePos.velocityMax); + if (isServoAtPosition(shoulder, drivePos.shoulderPosition, SERVO_TOLERANCE)) { + currentDriveState = Mutation.driveState.MOVING_ELBOW; } break; - case MOVING_ELBOW: - // Move the elbow to drive position + // Move the elbow to intake position so it don't slap the floor leftFinger.setPosition(LEFT_FINGER_GRIP); rightFinger.setPosition(RIGHT_FINGER_GRIP); - elbow.setPosition(ELBOW_DRIVE); - if (isServoAtPosition(elbow, ELBOW_DRIVE)) { + moveServoWithTrapezoidalVelocity(elbow, drivePos.elbowPosition, drivePos.accelerationMax, drivePos.velocityMax); + if (isServoAtPosition(elbow, drivePos.elbowPosition, SERVO_TOLERANCE)) { currentDriveState = Mutation.driveState.MOVING_WRIST; } break; case MOVING_WRIST: - // Move the wrist to drive position - wrist.setPosition(WRIST_DRIVE); - if (isServoAtPosition(wrist, WRIST_DRIVE)) { + // Move the wrist to intake position + moveServoWithTrapezoidalVelocity(wrist, drivePos.wristPosition, drivePos.accelerationMax, drivePos.velocityMax); + if (isServoAtPosition(wrist, drivePos.wristPosition, SERVO_TOLERANCE)) { currentDriveState = Mutation.driveState.COMPLETED; } break; @@ -255,192 +337,222 @@ private void handleDriveSequence() { currentDriveState = Mutation.driveState.IDLE; } } - private void driveCode() { - // mecanum drive - double forward = gamepad1.left_stick_y; - double strafe = -gamepad1.left_stick_x; - double turn = -gamepad1.right_stick_x; - - double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); - double rightFrontPower = (forward - strafe - turn) / denominator; - double leftFrontPower = (forward + strafe + turn) / denominator; - double rightBackPower = (forward + strafe - turn) / denominator; - double leftBackPower = (forward - strafe + turn) / denominator; + private void drivingFunction() { + // Check if the right bumper is pressed and the drive state is IDLE + if (gamepad1.right_bumper && currentDriveState == Mutation.driveState.IDLE) { + activeDrivePosition = new Mutation.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_DRIVE, ELBOW_DRIVE, MED_ACC, MED_VEL); + currentDriveState = Mutation.driveState.MOVING_SHOULDER; + } - if (gamepad1.left_bumper) { - rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); - leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); - rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); - leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); - } else { - rightFrontPower = Range.clip(rightFrontPower, -1, 1); - leftFrontPower = Range.clip(leftFrontPower, -1, 1); - rightBackPower = Range.clip(rightBackPower, -1, 1); - leftBackPower = Range.clip(leftBackPower, -1, 1); + if (activeDrivePosition != null) { + handleDriveSequence(activeDrivePosition); } - rightFront.setPower(rightFrontPower); - leftFront.setPower(leftFrontPower); - rightBack.setPower(rightBackPower); - leftBack.setPower(leftBackPower); + if (currentDriveState == Mutation.driveState.COMPLETED) { + currentDriveState = Mutation.driveState.IDLE; + activeDrivePosition = null; // Reset the active position + } + } + // SCORE + private enum scoreState { + IDLE, + MOVING_LIFT, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + COMPLETED } - // score - private void handleScorePosSequence(Mutation.ScorePosition scorePos) { + private Mutation.scoreState currentScoreState = Mutation.scoreState.IDLE; + private Mutation.ScorePosition activeScorePosition = null; + + + static class ScorePosition { + double liftPosition; + double shoulderPosition; + double wristPosition; + double elbowPosition; + double accelerationMax; + double velocityMax; + + public ScorePosition(double liftPos, double shoulderPos, double wristPos, double elbowPos, double accMax, double velMax) { + this.liftPosition = liftPos; + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + this.accelerationMax = accMax; + this.velocityMax = velMax; + } + } + private void handleScoreSequence(Mutation.ScorePosition scorePos) { switch (currentScoreState) { + case MOVING_SHOULDER: - // Move the shoulder to the specified position - moveServoGradually(shoulder, scorePos.shoulderPosition - .05); - shoulder.setPosition(scorePos.shoulderPosition); - if (isServoAtPosition(shoulder, scorePos.shoulderPosition)) { - currentScoreState = Mutation.scoreState.MOVING_WRIST; - } - break; - case MOVING_WRIST: - // Move the wrist to the specified position - wrist.setPosition(scorePos.wristPosition); - if (isServoAtPosition(wrist, scorePos.wristPosition)) { + // Move the shoulder to intake position + moveServoWithTrapezoidalVelocity(shoulder, scorePos.shoulderPosition, scorePos.accelerationMax, scorePos.velocityMax); + if (isServoAtPosition(shoulder, scorePos.shoulderPosition, SERVO_TOLERANCE)) { currentScoreState = Mutation.scoreState.MOVING_ELBOW; } break; case MOVING_ELBOW: - // Move the elbow to the specified position + // Move the elbow to intake position so it don't slap the floor leftFinger.setPosition(LEFT_FINGER_GRIP); rightFinger.setPosition(RIGHT_FINGER_GRIP); - elbow.setPosition(scorePos.elbowPosition); - if (isServoAtPosition(elbow, scorePos.elbowPosition)) { + moveServoWithTrapezoidalVelocity(elbow, scorePos.elbowPosition, scorePos.accelerationMax, scorePos.velocityMax); + if (isServoAtPosition(elbow, scorePos.elbowPosition, SERVO_TOLERANCE)) { + currentScoreState = Mutation.scoreState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to intake position + moveServoWithTrapezoidalVelocity(wrist, scorePos.wristPosition, scorePos.accelerationMax, scorePos.velocityMax); + if (isServoAtPosition(wrist, scorePos.wristPosition, SERVO_TOLERANCE)) { currentScoreState = Mutation.scoreState.MOVING_LIFT; } break; case MOVING_LIFT: setLiftPosition(scorePos.liftPosition); - if (isServoAtPosition(leftLift, scorePos.liftPosition) || isServoAtPosition(rightLift, scorePos.liftPosition)) { + if (isServoAtPosition(leftLift, scorePos.liftPosition, SERVO_TOLERANCE) && isServoAtPosition(rightLift, scorePos.liftPosition, SERVO_TOLERANCE)) { currentScoreState = Mutation.scoreState.COMPLETED; } break; case COMPLETED: // Sequence complete, reset the state or perform additional actions - currentScoreState = Mutation.scoreState.IDLE; - activeScorePosition = null; break; } - + // Check to reset the state to IDLE outside the switch if (currentScoreState == Mutation.scoreState.COMPLETED) { currentScoreState = Mutation.scoreState.IDLE; } } - // classes to support state machine actions - static class IntakePosition { - double shoulderPosition; - double wristPosition; - double elbowPosition; - - public IntakePosition(double shoulderPos, double wristPos, double elbowPos) { - this.shoulderPosition = shoulderPos; - this.wristPosition = wristPos; - this.elbowPosition = elbowPos; - } - } - - static class ScorePosition { - double shoulderPosition; - double wristPosition; - double elbowPosition; - double liftPosition; - - public ScorePosition(double shoulderPos, double wristPos, double elbowPos, double liftPos) { - this.shoulderPosition = shoulderPos; - this.wristPosition = wristPos; - this.elbowPosition = elbowPos; - this.liftPosition = liftPos; - } - } - - // functions to support state machine actions - private boolean isServoAtPosition(Servo servo, double position) { - return Math.abs(servo.getPosition() - position) < SERVO_TOLERANCE; - } - private void scoringFunction() { // scoring // score position one if (gamepad2.y && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, SCORE_ONE_LIFT); + activeScorePosition = new Mutation.ScorePosition(SCORE_ONE_SHOULDER, SCORE_ONE_ELBOW, SCORE_ONE_WRIST, SCORE_ONE_LIFT, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position two if (gamepad2.b && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, SCORE_TWO_LIFT); + activeScorePosition = new Mutation.ScorePosition(SCORE_TWO_SHOULDER, SCORE_TWO_ELBOW, SCORE_TWO_WRIST, SCORE_TWO_LIFT, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position three if (gamepad2.a && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_THREE_SHOULDER, SCORE_THREE_WRIST, SCORE_THREE_ELBOW, SCORE_THREE_LIFT); + activeScorePosition = new Mutation.ScorePosition(SCORE_THREE_SHOULDER, SCORE_THREE_ELBOW, SCORE_THREE_WRIST, SCORE_THREE_LIFT, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position four //TODO: gotta put !gamepad2.left_bumper above if (gamepad2.x && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, SCORE_FOUR_LIFT); + activeScorePosition = new Mutation.ScorePosition(SCORE_FOUR_SHOULDER, SCORE_FOUR_ELBOW, SCORE_FOUR_WRIST, SCORE_FOUR_LIFT, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position five if ((gamepad2.left_bumper && gamepad2.y) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_FIVE_SHOULDER, SCORE_FIVE_WRIST, SCORE_FIVE_ELBOW, SCORE_FIVE_LIFT); + activeScorePosition = new Mutation.ScorePosition(SCORE_FIVE_SHOULDER, SCORE_FIVE_ELBOW, SCORE_FIVE_WRIST, SCORE_FIVE_LIFT, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position six if ((gamepad2.left_bumper && gamepad2.b) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_SIX_SHOULDER, SCORE_SIX_WRIST, SCORE_SIX_ELBOW, SCORE_SIX_LIFT); + activeScorePosition = new Mutation.ScorePosition(SCORE_SIX_SHOULDER, SCORE_SIX_ELBOW, SCORE_SIX_WRIST, SCORE_SIX_LIFT, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position seven if ((gamepad2.left_bumper && gamepad2.a) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_SEVEN_SHOULDER, SCORE_SEVEN_WRIST, SCORE_SEVEN_ELBOW, SCORE_SEVEN_LIFT); + activeScorePosition = new Mutation.ScorePosition(SCORE_SEVEN_SHOULDER, SCORE_SEVEN_ELBOW, SCORE_SEVEN_WRIST, SCORE_SEVEN_LIFT, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position eight if ((gamepad2.left_bumper && gamepad2.x) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_EIGHT_SHOULDER, SCORE_EIGHT_WRIST, SCORE_EIGHT_ELBOW, SCORE_EIGHT_LIFT); + activeScorePosition = new Mutation.ScorePosition(SCORE_EIGHT_SHOULDER, SCORE_EIGHT_ELBOW, SCORE_EIGHT_WRIST, SCORE_EIGHT_LIFT, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position nine if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.y) && !gamepad2.left_bumper && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_NINE_SHOULDER, SCORE_NINE_WRIST, SCORE_NINE_ELBOW, SCORE_NINE_LIFT); + activeScorePosition = new Mutation.ScorePosition(SCORE_NINE_SHOULDER, SCORE_NINE_ELBOW, SCORE_NINE_WRIST, SCORE_NINE_LIFT, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position ten if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.b) && !gamepad2.left_bumper && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_TEN_SHOULDER, SCORE_TEN_WRIST, SCORE_TEN_ELBOW, SCORE_TEN_LIFT); + activeScorePosition = new Mutation.ScorePosition(SCORE_TEN_SHOULDER, SCORE_TEN_ELBOW, SCORE_TEN_WRIST, SCORE_TEN_LIFT, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position eleven if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.a) && !gamepad2.left_bumper && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_ELBOW, SCORE_ELEVEN_LIFT); + activeScorePosition = new Mutation.ScorePosition(SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_ELBOW, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_LIFT, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } if (activeScorePosition != null) { - handleScorePosSequence(activeScorePosition); + handleScoreSequence(activeScorePosition); } if (currentScoreState == Mutation.scoreState.COMPLETED) { currentScoreState = Mutation.scoreState.IDLE; activeScorePosition = null; // Reset the active position } } + + // functions to support state machine actions using servos + + // check servo positions with some tolerance + private boolean isServoAtPosition(Servo servo, double targetPosition, double tolerance) { + double currentPosition = servo.getPosition(); + double normalizedTarget = Range.clip(targetPosition, 0.0, 1.0); // Ensure target is within valid range + double normalizedCurrent = Range.clip(currentPosition, 0.0, 1.0); // Ensure current position is within valid range + + return Math.abs(normalizedCurrent - normalizedTarget) < tolerance; + } + + // move servo with ramping and soft-start created from the math and methods explained here: https://www.instructables.com/Servo-Ramping-and-Soft-Start/ + private void moveServoWithTrapezoidalVelocity(Servo servo, double targetPosition, double amax, double vmax) { + double currentPosition = servo.getPosition(); + double currentVelocity = 0.0; // Initial velocity is zero + double p0 = 0.0; // Distance needed to come to rest + + ElapsedTime timer = new ElapsedTime(); + timer.reset(); + + while (Math.abs(currentPosition - targetPosition) > SERVO_TOLERANCE && opModeIsActive()) { + // Calculate the elapsed time since the last update + double elapsedTime = timer.seconds(); + timer.reset(); + + // Calculate the distance needed to come to rest given the current velocity + p0 = 2 * currentVelocity + Math.abs(currentVelocity) * currentVelocity / (2 * amax); + + // Calculate the desired velocity based on the position error + double velocityError = targetPosition - currentPosition - p0; + double sign = Math.signum(velocityError); + double desiredVelocity = currentVelocity + sign * amax; + desiredVelocity = Range.clip(desiredVelocity, -vmax, vmax); // Constrain to vmax + + // Update the servo position based on the desired velocity and elapsed time + currentPosition += desiredVelocity * elapsedTime; + currentPosition = Range.clip(currentPosition, 0, 1); // Ensure the position is within valid range + servo.setPosition(currentPosition); + + // Update the current velocity for the next iteration + currentVelocity = desiredVelocity; + + telemetry.addData("Servo Position", currentPosition); + telemetry.update(); + } + } + // moves servo gradually private void moveServoGradually(Servo servo, double targetPosition) { double currentPosition = servo.getPosition(); @@ -461,7 +573,48 @@ private void moveServoGradually(Servo servo, double targetPosition) { } } - private void hangCode() { + // lift (has been adjusted mechanically to use the same height) + public Servo setLiftPosition(double targetPosition) { + // Ensure the target position is within the valid range + targetPosition = Math.max(0.0, Math.min(targetPosition, 1.0)); + + // Set the servo positions + leftLift.setPosition(targetPosition); + rightLift.setPosition(targetPosition); + return null; + } + private void driveCode() { + // mecanum drive + double forward = gamepad1.left_stick_y; + double strafe = -gamepad1.left_stick_x; + double turn = -gamepad1.right_stick_x; + + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); + + double rightFrontPower = (forward - strafe - turn) / denominator; + double leftFrontPower = (forward + strafe + turn) / denominator; + double rightBackPower = (forward + strafe - turn) / denominator; + double leftBackPower = (forward - strafe + turn) / denominator; + + if (gamepad1.left_bumper) { + rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); + leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); + rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); + leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); + } else { + rightFrontPower = Range.clip(rightFrontPower, -1, 1); + leftFrontPower = Range.clip(leftFrontPower, -1, 1); + rightBackPower = Range.clip(rightBackPower, -1, 1); + leftBackPower = Range.clip(leftBackPower, -1, 1); + } + + rightFront.setPower(rightFrontPower); + leftFront.setPower(leftFrontPower); + rightBack.setPower(rightBackPower); + leftBack.setPower(leftBackPower); + } + + private void hangCode() { // hanging if (gamepad1.right_trigger > TRIGGER_THRESHOLD){ if (!leftUpper.isPressed()) { @@ -489,59 +642,7 @@ private void hangCode() { } } - private void intakeFunction() { - // intake - // claw intake from floor - //TODO: add saftey - if (gamepad1.left_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - activeIntakePosition = new Mutation.IntakePosition(SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE); - currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; - } - // claw intake the top 2 from a stack of 5 - if (gamepad1.b && currentIntakeState == Mutation.intakeState.IDLE) { - activeIntakePosition = new Mutation.IntakePosition(SHOULDER_TOP_TWO, WRIST_TOP_TWO, ELBOW_TOP_TWO); - currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; - } - // claw intake the next 2 from a stack of 3 - if (gamepad1.a && currentIntakeState == Mutation.intakeState.IDLE) { - activeIntakePosition = new Mutation.IntakePosition(SHOULDER_NEXT_TWO, WRIST_NEXT_TWO, ELBOW_NEXT_TWO); - currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; - } - if (activeIntakePosition != null) { - handleIntakeSequence(activeIntakePosition); - } - if (currentIntakeState == Mutation.intakeState.COMPLETED) { - currentIntakeState = Mutation.intakeState.IDLE; - activeIntakePosition = null; // Reset the active position - } - - // dropping on the backboard for scoring - if (gamepad2.dpad_up && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_DROP); - rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_left && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_DROP); - } else if (gamepad2.dpad_right && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_down && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - } - - // grabbing off the floor or stack - if (gamepad2.right_bumper && gamepad2.dpad_up && (currentScoreState == Mutation.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_GRIP); - rightFinger.setPosition(RIGHT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentScoreState == Mutation.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentScoreState == Mutation.scoreState.IDLE)) { - rightFinger.setPosition(RIGHT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentScoreState == Mutation.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - } - } private void emergencyStop() { if (gamepad1.y) { leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); @@ -551,16 +652,6 @@ private void emergencyStop() { } } - // lift (has been adjusted mechanically to use the same height) - public void setLiftPosition(double targetPosition) { - // Ensure the target position is within the valid range - targetPosition = Math.max(0.0, Math.min(targetPosition, 1.0)); - - // Set the servo positions - leftLift.setPosition(targetPosition); - rightLift.setPosition(targetPosition); - } - // launcher private void airplane() { if (gamepad2.back) { @@ -641,19 +732,14 @@ public void runOpMode() throws InterruptedException { while(opModeIsActive()){ - // claw drive position - //TODO: add safety, this is drive around pos - if (gamepad1.right_bumper && currentDriveState == Mutation.driveState.IDLE) { - currentDriveState = driveState.MOVING_SHOULDER; - } - handleDriveSequence(); - - // telemetry telemetry.addData("Status", "Run " + runtime.toString()); telemetry.addData("Intake", currentIntakeState); telemetry.addData("Drive", currentDriveState); telemetry.addData("Score", currentScoreState); + telemetry.addData("Shoulder Position", shoulder.getPosition()); + telemetry.addData("Wrist Position", wrist.getPosition()); + telemetry.addData("Elbow Position", elbow.getPosition()); //telemetry.addData("Left Lower", leftLower.isPressed() ? "Pressed" : "Not Pressed"); //telemetry.addData("Left Upper", leftUpper.isPressed() ? "Pressed" : "Not Pressed"); @@ -662,9 +748,17 @@ public void runOpMode() throws InterruptedException { // launcher airplane(); + // mecanum drive driveCode(); - scoringFunction(); + // hang + hangCode(); + // intake positions intakeFunction(); + // driving pos + drivingFunction(); + // scoring positions + scoringFunction(); + // emergency stop slides emergencyStop(); telemetry.update(); From a05171d182ed6d38ca62997ee06d1881d195b255 Mon Sep 17 00:00:00 2001 From: digitalbreadllc Date: Tue, 12 Dec 2023 16:11:19 -0700 Subject: [PATCH 118/154] updates for positions to mutation teleop --- .../firstinspires/ftc/teamcode/ArmWork.java | 2 +- .../firstinspires/ftc/teamcode/Mutation.java | 101 ++++++++++-------- 2 files changed, 58 insertions(+), 45 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmWork.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmWork.java index 4730cc0529..7d0fe9abd0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmWork.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmWork.java @@ -39,7 +39,7 @@ public class ArmWork extends LinearOpMode{ private static final double TRIGGER_THRESHOLD = 0.5; private static final double LAUNCHER_START_POS = 0.8; private static final double SERVO_TOLERANCE = 0.01; - private static final double LIFT_DRIVE = 0.069; + private static final double LIFT_DRIVE = 0.10; private double liftTargetPosition = LIFT_DRIVE; // score position one button map (gamepad2.y) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 5309f6c950..a952a11061 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -47,29 +47,29 @@ public class Mutation extends LinearOpMode { // servo values public static final double SHOULDER_DRIVE = 0.425; // 0.425 - public static final double SHOULDER_INT = 0.43; // 0.455 - public static final double ELBOW_DRIVE= 0.5; - public static final double ELBOW_INTAKE = 0.8; - public static final double WRIST_INT = 0.55; // int for auto & testing 0.55; // int for DRIVE 0.265 - public static final double WRIST_DRIVE = 0.82; // int for auto & testing 0.55; // int for DRIVE 0.265 - public static final double WRIST_INTAKE = 0.545; - public static final double LEFT_FINGER_GRIP = 0.74; + public static final double SHOULDER_INT = 0.43; + public static final double ELBOW_DRIVE= 0.47; + public static final double ELBOW_INTAKE = 0.78; + public static final double WRIST_TUCK = 0.29; + public static final double WRIST_DRIVE = 0.82; + public static final double WRIST_INTAKE = 0.57; + public static final double LEFT_FINGER_GRIP = 0.72; public static final double LEFT_FINGER_DROP = 0.5; - public static final double LEFT_FINGER_INTAKE = 0.34; + public static final double LEFT_FINGER_INTAKE = 0.3; public static final double RIGHT_FINGER_GRIP = .27; public static final double RIGHT_FINGER_DROP = .5; public static final double RIGHT_FINGER_INTAKE = 0.64; public static final double TRIGGER_THRESHOLD = 0.5; public static final double LAUNCHER_START_POS = 0.8; public static final double SERVO_TOLERANCE = 0.01; - public static final double LIFT_DRIVE = 0.069; // 0.78 is the highest it can mechanically go right now + public static final double LIFT_DRIVE = 0.10; // 0.78 is the highest it can mechanically go right now private double liftTargetPosition = LIFT_DRIVE; // was 0.37 before moving servos for larger range // stack positions (top 2 o 5 and next 2 of 3 ) // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin // intake two off a stack of five public static final double SHOULDER_TOP_TWO = 0.425; - public static final double WRIST_TOP_TWO = 0.606; + public static final double WRIST_TOP_TWO = 0.59; public static final double ELBOW_TOP_TWO = 0.74; // intake two off a stack of three @@ -152,6 +152,7 @@ private enum intakeState { MOVING_SHOULDER, MOVING_WRIST, MOVING_ELBOW, + MOVING_CLAWS, COMPLETED } @@ -188,20 +189,27 @@ private void handleIntakeSequence(Mutation.IntakePosition intakePos) { break; case MOVING_WRIST: // Move the wrist to intake position - moveServoWithTrapezoidalVelocity(wrist, intakePos.wristPosition, intakePos.accelerationMax, intakePos.velocityMax); + //moveServoGradually(wrist, intakePos.wristPosition); + wrist.setPosition(intakePos.wristPosition); + //moveServoWithTrapezoidalVelocity(wrist, intakePos.wristPosition, intakePos.accelerationMax, intakePos.velocityMax); if (isServoAtPosition(wrist, intakePos.wristPosition, SERVO_TOLERANCE)) { currentIntakeState = Mutation.intakeState.MOVING_ELBOW; } break; case MOVING_ELBOW: // Move the elbow to intake position so it don't slap the floor - moveServoWithTrapezoidalVelocity(elbow, intakePos.elbowPosition, intakePos.accelerationMax, intakePos.velocityMax); + moveServoGradually(elbow, intakePos.elbowPosition); + // moveServoWithTrapezoidalVelocity(elbow, intakePos.elbowPosition, intakePos.accelerationMax, intakePos.velocityMax); if (isServoAtPosition(elbow, intakePos.elbowPosition, SERVO_TOLERANCE)) { // Check if the elbow is 70% down and open the claws if it is - if (elbow.getPosition() > (intakePos.elbowPosition - 0.4)) { - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - } + currentIntakeState = Mutation.intakeState.MOVING_CLAWS; + } + break; + case MOVING_CLAWS: + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + if (isServoAtPosition(leftFinger, LEFT_FINGER_INTAKE, SERVO_TOLERANCE) || isServoAtPosition(rightFinger, RIGHT_FINGER_INTAKE, SERVO_TOLERANCE)) { + // Check if the elbow is 70% down and open the claws if it is currentIntakeState = Mutation.intakeState.COMPLETED; } break; @@ -241,32 +249,6 @@ private void intakeFunction() { activeIntakePosition = null; // Reset the active position } - - // dropping on the backboard for scoring - if (gamepad2.dpad_up && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_DROP); - rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_left && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_DROP); - } else if (gamepad2.dpad_right && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - rightFinger.setPosition(RIGHT_FINGER_DROP); - } else if (gamepad2.dpad_down && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - } - - // grabbing off the floor or stack - if (gamepad2.right_bumper && gamepad2.dpad_up && (currentScoreState == Mutation.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_GRIP); - rightFinger.setPosition(RIGHT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentScoreState == Mutation.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentScoreState == Mutation.scoreState.IDLE)) { - rightFinger.setPosition(RIGHT_FINGER_GRIP); - } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentScoreState == Mutation.scoreState.IDLE)) { - leftFinger.setPosition(LEFT_FINGER_INTAKE); - rightFinger.setPosition(RIGHT_FINGER_INTAKE); - } } // DRIVE @@ -341,7 +323,7 @@ private void handleDriveSequence(Mutation.DrivePosition drivePos) { private void drivingFunction() { // Check if the right bumper is pressed and the drive state is IDLE if (gamepad1.right_bumper && currentDriveState == Mutation.driveState.IDLE) { - activeDrivePosition = new Mutation.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_DRIVE, ELBOW_DRIVE, MED_ACC, MED_VEL); + activeDrivePosition = new Mutation.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_TUCK, ELBOW_DRIVE, HIGH_ACC, HIGH_VEL); currentDriveState = Mutation.driveState.MOVING_SHOULDER; } @@ -652,6 +634,35 @@ private void emergencyStop() { } } + // operating the claws to grab and drop + private void grapdropFunction() { + // dropping on the backboard for scoring + if (gamepad2.dpad_up && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_left && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + } else if (gamepad2.dpad_right && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_down && !gamepad2.right_bumper && currentIntakeState == Mutation.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + + // grabbing off the floor or stack + if (gamepad2.right_bumper && gamepad2.dpad_up && (currentScoreState == Mutation.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentScoreState == Mutation.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentScoreState == Mutation.scoreState.IDLE)) { + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentScoreState == Mutation.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + } + // launcher private void airplane() { if (gamepad2.back) { @@ -720,7 +731,7 @@ public void runOpMode() throws InterruptedException { setLiftPosition(LIFT_DRIVE); shoulder.setPosition(SHOULDER_INT); elbow.setPosition(ELBOW_DRIVE); - wrist.setPosition(WRIST_INTAKE); + wrist.setPosition(WRIST_TUCK); launcherstartPos(); telemetry.addData("Status", "OdoMec is ready to run!"); @@ -752,6 +763,8 @@ public void runOpMode() throws InterruptedException { driveCode(); // hang hangCode(); + // claws + grapdropFunction(); // intake positions intakeFunction(); // driving pos From ad6f0f998f8c84003d87aa4bc9be61f78c8a0c2f Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 12 Dec 2023 17:39:20 -0700 Subject: [PATCH 119/154] Blue left auto fully works --- .../ftc/teamcode/EnigmaAuto.java | 46 +++++++++++-------- 1 file changed, 28 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index fea0915805..6ae249cf91 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -147,6 +147,16 @@ private void moveServoGradually(Servo servo, double targetPosition) { } } + private Servo setLiftPosition(double targetPosition) { + // Ensure the target position is within the valid range + targetPosition = Math.max(0.0, Math.min(targetPosition, 1.0)); + + // Set the servo positions + leftLift.setPosition(targetPosition); + rightLift.setPosition(targetPosition); + return null; + } + @Override public void runOpMode() throws InterruptedException { wrist = hardwareMap.get(Servo.class, "wrist"); @@ -161,15 +171,15 @@ public void runOpMode() throws InterruptedException { shoulder.setDirection(Servo.Direction.REVERSE); wrist.setDirection(Servo.Direction.REVERSE); + //init pos - leftLift.setPosition(0.37); - rightLift.setPosition(0.37); - shoulder.setPosition(0.43); - wrist.setPosition(0.575); - elbow.setPosition(0.5); - sleep(1000); - leftFinger.setPosition(0.74); - rightFinger.setPosition(0.27); + setLiftPosition(Mutation.LIFT_DRIVE); + shoulder.setPosition(Mutation.SHOULDER_DRIVE); + wrist.setPosition(Mutation.WRIST_INTAKE); + elbow.setPosition(Mutation.ELBOW_DRIVE); + sleep(2500); + leftFinger.setPosition(Mutation.LEFT_FINGER_GRIP); + rightFinger.setPosition(Mutation.RIGHT_FINGER_GRIP); // Vision OpenCV / Color Detection WebcamName webcamName = hardwareMap.get(WebcamName.class, "Webcam 1"); @@ -263,17 +273,17 @@ public void runAutonoumousMode() { case LEFT: dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); - dropYellowPixelPose = new Pose2d(23, 36, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(19, 36, Math.toRadians(-90)); break; case MIDDLE: dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(30, 36, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(26, 36, Math.toRadians(-90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); - dropYellowPixelPose = new Pose2d(37, 36, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(31, 36, Math.toRadians(-90)); break; } midwayPose1 = new Pose2d(14, 13, Math.toRadians(-45)); @@ -378,7 +388,7 @@ public void runAutonoumousMode() { sleep(10); } sleep(1000); - rightFinger.setPosition(0.64); + rightFinger.setPosition(Mutation.RIGHT_FINGER_DROP); sleep(500); elbow.setPosition(ELBOW_DRIVE); sleep(500); @@ -433,7 +443,7 @@ public void runAutonoumousMode() { sleep(7); } sleep(300); - leftFinger.setPosition(0.5); + leftFinger.setPosition(Mutation.LEFT_FINGER_DROP); sleep(300); for(int s = 0; s<200; s++) { moveServoGradually(shoulder, Mutation.SHOULDER_DRIVE); @@ -468,11 +478,11 @@ public void selectStartingPosition() { telemetry.addData("Initializing ENIGMA Autonomous Team# a", TEAM_NAME, " ", TEAM_NUMBER); telemetry.addData("---------------------------------------",""); - telemetry.addData("Select Starting Position using XYAB on Logitech (or ▢ΔOX on Playstayion) on gamepad 1:",""); - telemetry.addData(" Blue Left ", "(X / ▢)"); - telemetry.addData(" Blue Right ", "(Y / Δ)"); - telemetry.addData(" Red Left ", "(B / O)"); - telemetry.addData(" Red Right ", "(A / X)"); + telemetry.addData("Select Starting Position using XYAB:",""); + telemetry.addData(" Blue Left ", "(X)"); + telemetry.addData(" Blue Right ", "(Y)"); + telemetry.addData(" Red Left ", "(B)"); + telemetry.addData(" Red Right ", "(A)"); if(gamepad1.x){ startPosition = START_POSITION.BLUE_LEFT; break; From bd83d62cb8857a6d03abff1f1b80663a71a92181 Mon Sep 17 00:00:00 2001 From: Gabriel Date: Tue, 12 Dec 2023 17:46:02 -0700 Subject: [PATCH 120/154] updating Mutation --- .../java/org/firstinspires/ftc/teamcode/EnigmaAuto.java | 1 - .../java/org/firstinspires/ftc/teamcode/Mutation.java | 8 ++++---- .../main/java/org/firstinspires/ftc/teamcode/OdoMec.java | 1 + 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 6ae249cf91..285306e4cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -511,7 +511,6 @@ public void safeWaitSeconds(double time) { while (!isStopRequested() && timer.time() < time) { } } - class teamElementPipeline extends OpenCvPipeline{ Mat YCbCr = new Mat(); Mat leftCrop; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index a952a11061..8e44dc6d47 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -414,26 +414,26 @@ private void scoringFunction() { // score position one if (gamepad2.y && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_ONE_SHOULDER, SCORE_ONE_ELBOW, SCORE_ONE_WRIST, SCORE_ONE_LIFT, MED_ACC, MED_VEL); + activeScorePosition = new Mutation.ScorePosition(SCORE_ONE_LIFT, SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position two if (gamepad2.b && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_TWO_SHOULDER, SCORE_TWO_ELBOW, SCORE_TWO_WRIST, SCORE_TWO_LIFT, MED_ACC, MED_VEL); + activeScorePosition = new Mutation.ScorePosition(SCORE_TWO_LIFT, SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position three if (gamepad2.a && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_THREE_SHOULDER, SCORE_THREE_ELBOW, SCORE_THREE_WRIST, SCORE_THREE_LIFT, MED_ACC, MED_VEL); + activeScorePosition = new Mutation.ScorePosition(SCORE_THREE_LIFT, SCORE_THREE_SHOULDER, SCORE_THREE_WRIST, SCORE_THREE_ELBOW, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position four //TODO: gotta put !gamepad2.left_bumper above if (gamepad2.x && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_FOUR_SHOULDER, SCORE_FOUR_ELBOW, SCORE_FOUR_WRIST, SCORE_FOUR_LIFT, MED_ACC, MED_VEL); + activeScorePosition = new Mutation.ScorePosition(SCORE_FOUR_LIFT, SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position five diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java index 70a2470dfc..044fb74db5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -21,6 +21,7 @@ public class OdoMec extends LinearOpMode { private DcMotor rightFront; //front right 0 private DcMotor leftFront; //front left 2 private DcMotor rightBack; //rear right 1 + private DcMotor leftBack; //rear left 3 private DcMotor leftHang; private DcMotor rightHang; From 8aa3a76046c1cc1a9a55e604f035f870326e4bc7 Mon Sep 17 00:00:00 2001 From: Gabriel Date: Tue, 12 Dec 2023 17:58:00 -0700 Subject: [PATCH 121/154] updating Mutation --- .../org/firstinspires/ftc/teamcode/Mutation.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 8e44dc6d47..bb3dd85c26 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -439,43 +439,43 @@ private void scoringFunction() { // score position five if ((gamepad2.left_bumper && gamepad2.y) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_FIVE_SHOULDER, SCORE_FIVE_ELBOW, SCORE_FIVE_WRIST, SCORE_FIVE_LIFT, MED_ACC, MED_VEL); + activeScorePosition = new Mutation.ScorePosition(SCORE_FIVE_LIFT, SCORE_FIVE_SHOULDER, SCORE_FIVE_WRIST, SCORE_FIVE_ELBOW, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position six if ((gamepad2.left_bumper && gamepad2.b) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_SIX_SHOULDER, SCORE_SIX_ELBOW, SCORE_SIX_WRIST, SCORE_SIX_LIFT, MED_ACC, MED_VEL); + activeScorePosition = new Mutation.ScorePosition(SCORE_SIX_LIFT, SCORE_SIX_SHOULDER, SCORE_SIX_WRIST, SCORE_SIX_ELBOW, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position seven if ((gamepad2.left_bumper && gamepad2.a) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_SEVEN_SHOULDER, SCORE_SEVEN_ELBOW, SCORE_SEVEN_WRIST, SCORE_SEVEN_LIFT, MED_ACC, MED_VEL); + activeScorePosition = new Mutation.ScorePosition(SCORE_SEVEN_LIFT, SCORE_SEVEN_SHOULDER, SCORE_SEVEN_WRIST, SCORE_SEVEN_ELBOW, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position eight if ((gamepad2.left_bumper && gamepad2.x) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_EIGHT_SHOULDER, SCORE_EIGHT_ELBOW, SCORE_EIGHT_WRIST, SCORE_EIGHT_LIFT, MED_ACC, MED_VEL); + activeScorePosition = new Mutation.ScorePosition(SCORE_EIGHT_LIFT, SCORE_EIGHT_SHOULDER, SCORE_EIGHT_WRIST, SCORE_EIGHT_ELBOW, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position nine if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.y) && !gamepad2.left_bumper && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_NINE_SHOULDER, SCORE_NINE_ELBOW, SCORE_NINE_WRIST, SCORE_NINE_LIFT, MED_ACC, MED_VEL); + activeScorePosition = new Mutation.ScorePosition(SCORE_NINE_LIFT, SCORE_NINE_SHOULDER, SCORE_NINE_WRIST, SCORE_NINE_ELBOW, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position ten if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.b) && !gamepad2.left_bumper && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_TEN_SHOULDER, SCORE_TEN_ELBOW, SCORE_TEN_WRIST, SCORE_TEN_LIFT, MED_ACC, MED_VEL); + activeScorePosition = new Mutation.ScorePosition(SCORE_TEN_LIFT, SCORE_TEN_SHOULDER, SCORE_TEN_WRIST, SCORE_TEN_ELBOW, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position eleven if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.a) && !gamepad2.left_bumper && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_ELBOW, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_LIFT, MED_ACC, MED_VEL); + activeScorePosition = new Mutation.ScorePosition(SCORE_ELEVEN_LIFT, SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_ELBOW, MED_ACC, MED_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } if (activeScorePosition != null) { From abff6fcf99b8e9d09174d4955dac1f595ce2479c Mon Sep 17 00:00:00 2001 From: Gabriel Date: Tue, 12 Dec 2023 17:59:29 -0700 Subject: [PATCH 122/154] updating Mutation --- .../src/main/java/org/firstinspires/ftc/teamcode/Mutation.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index bb3dd85c26..8e3bf9d965 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -323,7 +323,7 @@ private void handleDriveSequence(Mutation.DrivePosition drivePos) { private void drivingFunction() { // Check if the right bumper is pressed and the drive state is IDLE if (gamepad1.right_bumper && currentDriveState == Mutation.driveState.IDLE) { - activeDrivePosition = new Mutation.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_TUCK, ELBOW_DRIVE, HIGH_ACC, HIGH_VEL); + activeDrivePosition = new Mutation.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_TUCK, ELBOW_DRIVE, MED_ACC, MED_VEL); currentDriveState = Mutation.driveState.MOVING_SHOULDER; } From 5af066e5ac200074914ac5e40013882d7a90ceb5 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 12 Dec 2023 18:04:43 -0700 Subject: [PATCH 123/154] dirve funchion no go --- .../src/main/java/org/firstinspires/ftc/teamcode/Mutation.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 8e3bf9d965..f81ac9c592 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -324,7 +324,7 @@ private void drivingFunction() { // Check if the right bumper is pressed and the drive state is IDLE if (gamepad1.right_bumper && currentDriveState == Mutation.driveState.IDLE) { activeDrivePosition = new Mutation.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_TUCK, ELBOW_DRIVE, MED_ACC, MED_VEL); - currentDriveState = Mutation.driveState.MOVING_SHOULDER; + currentDriveState = driveState.MOVING_LIFT; } if (activeDrivePosition != null) { From aecddabbdb12963426ae9943692bf83e6684c834 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 12 Dec 2023 18:08:08 -0700 Subject: [PATCH 124/154] Drive function can now go from any value w/ lift to drive pos. WOOOOOOOO --- .../src/main/java/org/firstinspires/ftc/teamcode/Mutation.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index f81ac9c592..291bf582c9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -286,6 +286,7 @@ private void handleDriveSequence(Mutation.DrivePosition drivePos) { switch (currentDriveState) { case MOVING_LIFT: setLiftPosition(LIFT_DRIVE); + currentDriveState = Mutation.driveState.MOVING_SHOULDER; break; case MOVING_SHOULDER: // Move the shoulder to intake position @@ -324,7 +325,7 @@ private void drivingFunction() { // Check if the right bumper is pressed and the drive state is IDLE if (gamepad1.right_bumper && currentDriveState == Mutation.driveState.IDLE) { activeDrivePosition = new Mutation.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_TUCK, ELBOW_DRIVE, MED_ACC, MED_VEL); - currentDriveState = driveState.MOVING_LIFT; + currentDriveState = Mutation.driveState.MOVING_LIFT; } if (activeDrivePosition != null) { From 2e483c2fc44164ee0ed0d3b8a18860f5ccd6a059 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 12 Dec 2023 18:22:00 -0700 Subject: [PATCH 125/154] Ermmm tele op good now yee haw --- .../firstinspires/ftc/teamcode/Mutation.java | 58 +++++++++---------- 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 291bf582c9..9ba375a3c2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -52,7 +52,7 @@ public class Mutation extends LinearOpMode { public static final double ELBOW_INTAKE = 0.78; public static final double WRIST_TUCK = 0.29; public static final double WRIST_DRIVE = 0.82; - public static final double WRIST_INTAKE = 0.57; + public static final double WRIST_INTAKE = 0.545; public static final double LEFT_FINGER_GRIP = 0.72; public static final double LEFT_FINGER_DROP = 0.5; public static final double LEFT_FINGER_INTAKE = 0.3; @@ -102,40 +102,40 @@ public class Mutation extends LinearOpMode { public static final double SCORE_FOUR_ELBOW = SCORE_ONE_ELBOW; public static final double SCORE_FOUR_LIFT = .44; // score position five button map (gamepad2.left_bumper && gamepad2.y) - public static final double SCORE_FIVE_SHOULDER = 0.92611111; - public static final double SCORE_FIVE_WRIST = 1; - public static final double SCORE_FIVE_ELBOW = 0.73; - public static final double SCORE_FIVE_LIFT = 0.59; + public static final double SCORE_FIVE_SHOULDER = SCORE_ONE_SHOULDER; + public static final double SCORE_FIVE_WRIST = SCORE_ONE_WRIST; + public static final double SCORE_FIVE_ELBOW = SCORE_ONE_ELBOW; + public static final double SCORE_FIVE_LIFT = 0.55; // score position six button map (gamepad2.left_bumper && gamepad2.b) - private static final double SCORE_SIX_SHOULDER = 0.925; - private static final double SCORE_SIX_WRIST = 1; - private static final double SCORE_SIX_ELBOW = 0.75; - private static final double SCORE_SIX_LIFT = .71; + private static final double SCORE_SIX_SHOULDER = 0.905; + private static final double SCORE_SIX_WRIST = 0.85; + private static final double SCORE_SIX_ELBOW = 0.84; + private static final double SCORE_SIX_LIFT = .23; // score position seven button map (gamepad2.left_bumper && gamepad2.a) - private static final double SCORE_SEVEN_SHOULDER = 0.91; - private static final double SCORE_SEVEN_WRIST = 1; - private static final double SCORE_SEVEN_ELBOW = 0.72; - private static final double SCORE_SEVEN_LIFT = 0.8; + private static final double SCORE_SEVEN_SHOULDER = SCORE_SIX_SHOULDER; + private static final double SCORE_SEVEN_WRIST = SCORE_SIX_WRIST; + private static final double SCORE_SEVEN_ELBOW = SCORE_SIX_ELBOW; + private static final double SCORE_SEVEN_LIFT = 0.34; // score position eight button map (gamepad2.left_bumper && gamepad2.x) - private static final double SCORE_EIGHT_SHOULDER = 0.94; - private static final double SCORE_EIGHT_WRIST = 0.93; - private static final double SCORE_EIGHT_ELBOW = 0.8; - private static final double SCORE_EIGHT_LIFT = 0.8; + private static final double SCORE_EIGHT_SHOULDER = SCORE_SIX_SHOULDER; + private static final double SCORE_EIGHT_WRIST = SCORE_SIX_WRIST; + private static final double SCORE_EIGHT_ELBOW = SCORE_SIX_ELBOW; + private static final double SCORE_EIGHT_LIFT = 0.45; // score position nine button map (gamepad2.left_trigger && gamepad2.y) - private static final double SCORE_NINE_SHOULDER = 0.93 ; - private static final double SCORE_NINE_WRIST = 0.93; - private static final double SCORE_NINE_ELBOW = 0.79; - private static final double SCORE_NINE_LIFT = 0.86; + private static final double SCORE_NINE_SHOULDER = SCORE_SIX_SHOULDER; + private static final double SCORE_NINE_WRIST = SCORE_SIX_WRIST; + private static final double SCORE_NINE_ELBOW = SCORE_SIX_ELBOW; + private static final double SCORE_NINE_LIFT = 0.45; // score position ten button map (gamepad2.left_trigger && gamepad2.b) - private static final double SCORE_TEN_SHOULDER = 0.9; - private static final double SCORE_TEN_WRIST = 0.93; - private static final double SCORE_TEN_ELBOW = 0.76; - private static final double SCORE_TEN_LIFT = 0.98; + private static final double SCORE_TEN_SHOULDER = SCORE_SIX_SHOULDER; + private static final double SCORE_TEN_WRIST = SCORE_SIX_WRIST; + private static final double SCORE_TEN_ELBOW = SCORE_SIX_ELBOW; + private static final double SCORE_TEN_LIFT = 0.45; // score position eleven button map (gamepad2.left_trigger && gamepad2.a) - private static final double SCORE_ELEVEN_SHOULDER = 0.9; - private static final double SCORE_ELEVEN_WRIST = 0.93; - private static final double SCORE_ELEVEN_ELBOW = 0.76; - private static final double SCORE_ELEVEN_LIFT = 0.98; + private static final double SCORE_ELEVEN_SHOULDER = SCORE_SIX_SHOULDER; + private static final double SCORE_ELEVEN_WRIST = SCORE_SIX_WRIST; + private static final double SCORE_ELEVEN_ELBOW = SCORE_SIX_ELBOW; + private static final double SCORE_ELEVEN_LIFT = 0.45; // sensors private RevTouchSensor rightUpper; From 8571eb075f1e1acac8450b45a44b752f1c221631 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 12 Dec 2023 19:20:50 -0700 Subject: [PATCH 126/154] Auto works 6/12 times Blue left 3/3 Blue right 3/3 Red left 0/3 Red right 0/3 --- .../firstinspires/ftc/teamcode/EnigmaAuto.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 285306e4cf..f6fa65f826 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -321,17 +321,17 @@ public void runAutonoumousMode() { case LEFT: dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); - dropYellowPixelPose = new Pose2d(27, 86, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(19, 86, Math.toRadians(-90)); break; case MIDDLE: dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(34, 86, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(26.25, 86, Math.toRadians(-90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); - dropYellowPixelPose = new Pose2d(43, 86, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(33.5, 86, Math.toRadians(-90)); break; } midwayPose1 = new Pose2d(8, -8, Math.toRadians(0)); @@ -387,11 +387,11 @@ public void runAutonoumousMode() { moveServoGradually(elbow, ELBOW_INTAKE); sleep(10); } - sleep(1000); + sleep(200); rightFinger.setPosition(Mutation.RIGHT_FINGER_DROP); - sleep(500); + sleep(200); elbow.setPosition(ELBOW_DRIVE); - sleep(500); + wrist.setPosition(Mutation.WRIST_TUCK); //Move robot to midwayPose1 Actions.runBlocking( @@ -442,9 +442,9 @@ public void runAutonoumousMode() { moveServoGradually(shoulder, SCORE_ONE_SHOULDER); sleep(7); } - sleep(300); + sleep(200); leftFinger.setPosition(Mutation.LEFT_FINGER_DROP); - sleep(300); + sleep(100); for(int s = 0; s<200; s++) { moveServoGradually(shoulder, Mutation.SHOULDER_DRIVE); sleep(7); From 2290109e37c27a29adb1e35ca9cfe1b5566eb675 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 13 Dec 2023 15:55:14 -0700 Subject: [PATCH 127/154] Auto works 9/12 times Blue left 3/3 Blue right 3/3 Red left 0/3 Red right 2/3 --- .../java/org/firstinspires/ftc/teamcode/EnigmaAuto.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index f6fa65f826..38f7e6d916 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -297,17 +297,17 @@ public void runAutonoumousMode() { case LEFT: dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); - dropYellowPixelPose = new Pose2d(21, -36, Math.toRadians(90)); + dropYellowPixelPose = new Pose2d(34, -36, Math.toRadians(90)); break; case MIDDLE: dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(29, -36, Math.toRadians(90)); + dropYellowPixelPose = new Pose2d(28.25, -36, Math.toRadians(90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); - dropYellowPixelPose = new Pose2d(37, -36, Math.toRadians(90)); + dropYellowPixelPose = new Pose2d(20, -36, Math.toRadians(90)); break; } midwayPose1 = new Pose2d(14, -13, Math.toRadians(45)); From 7abc352a7509979a518f43355f94d884c39075f3 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 13 Dec 2023 16:36:30 -0700 Subject: [PATCH 128/154] Auto works 9/12 times Blue left 3/3 Blue right 3/3 Red left 0/3 Red right 2/3 --- .../main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 38f7e6d916..64a53f0d36 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -348,7 +348,7 @@ public void runAutonoumousMode() { case LEFT: dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); - dropYellowPixelPose = new Pose2d(37, -86, Math.toRadians(90)); + dropYellowPixelPose = new Pose2d(34, -86, Math.toRadians(90)); break; case MIDDLE: dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up @@ -358,7 +358,7 @@ public void runAutonoumousMode() { case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); - dropYellowPixelPose = new Pose2d(21, -86, Math.toRadians(90)); + dropYellowPixelPose = new Pose2d(23, -86, Math.toRadians(90)); break; } midwayPose1 = new Pose2d(8, 8, Math.toRadians(0)); From 64e205025225d830e7a8a8613db6e84585a22309 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 13 Dec 2023 16:38:28 -0700 Subject: [PATCH 129/154] Auto works 9/12 times Blue left 3/3 Blue right 3/3 Red left 0/3 Red right 2/3 --- .../src/main/java/org/firstinspires/ftc/teamcode/Mutation.java | 2 +- .../org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 9ba375a3c2..a2cf82be4b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -52,7 +52,7 @@ public class Mutation extends LinearOpMode { public static final double ELBOW_INTAKE = 0.78; public static final double WRIST_TUCK = 0.29; public static final double WRIST_DRIVE = 0.82; - public static final double WRIST_INTAKE = 0.545; + public static final double WRIST_INTAKE = 0.555; public static final double LEFT_FINGER_GRIP = 0.72; public static final double LEFT_FINGER_DROP = 0.5; public static final double LEFT_FINGER_INTAKE = 0.3; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java index 658f790339..6706b80e5c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java @@ -46,7 +46,7 @@ private static OpModeMeta metaForClass(Class cls) { .build(); } - @OpModeRegistrar + //@OpModeRegistrar public static void register(OpModeManager manager) { if (DISABLED) return; From c377f57b54d1aa21dd59f079395f876cb4bfbd78 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 13 Dec 2023 17:19:52 -0700 Subject: [PATCH 130/154] Auto works 9/12 times Blue left 3/3 Blue right 3/3 Red left 0/3 Red right 2/3 --- .../firstinspires/ftc/teamcode/ArmWork.java | 2 +- .../firstinspires/ftc/teamcode/Mutation.java | 50 ++++++++++--------- .../ftc/teamcode/MutationOld.java | 2 +- .../firstinspires/ftc/teamcode/OdoMec.java | 2 +- 4 files changed, 29 insertions(+), 27 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmWork.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmWork.java index 7d0fe9abd0..11d7d6ea6a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmWork.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmWork.java @@ -4,7 +4,7 @@ import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; -@TeleOp +//@TeleOp public class ArmWork extends LinearOpMode{ // Declare vars // timers diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index a2cf82be4b..95acc63739 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -107,35 +107,35 @@ public class Mutation extends LinearOpMode { public static final double SCORE_FIVE_ELBOW = SCORE_ONE_ELBOW; public static final double SCORE_FIVE_LIFT = 0.55; // score position six button map (gamepad2.left_bumper && gamepad2.b) - private static final double SCORE_SIX_SHOULDER = 0.905; - private static final double SCORE_SIX_WRIST = 0.85; - private static final double SCORE_SIX_ELBOW = 0.84; - private static final double SCORE_SIX_LIFT = .23; + private static final double SCORE_SIX_SHOULDER = SCORE_ONE_SHOULDER; + private static final double SCORE_SIX_WRIST = SCORE_ONE_WRIST; + private static final double SCORE_SIX_ELBOW = SCORE_ONE_ELBOW; + private static final double SCORE_SIX_LIFT = .66; // score position seven button map (gamepad2.left_bumper && gamepad2.a) - private static final double SCORE_SEVEN_SHOULDER = SCORE_SIX_SHOULDER; - private static final double SCORE_SEVEN_WRIST = SCORE_SIX_WRIST; - private static final double SCORE_SEVEN_ELBOW = SCORE_SIX_ELBOW; - private static final double SCORE_SEVEN_LIFT = 0.34; + private static final double SCORE_SEVEN_SHOULDER = SCORE_ONE_SHOULDER; + private static final double SCORE_SEVEN_WRIST = 0.305; + private static final double SCORE_SEVEN_ELBOW = 0.6; + private static final double SCORE_SEVEN_LIFT = .66; // score position eight button map (gamepad2.left_bumper && gamepad2.x) - private static final double SCORE_EIGHT_SHOULDER = SCORE_SIX_SHOULDER; - private static final double SCORE_EIGHT_WRIST = SCORE_SIX_WRIST; - private static final double SCORE_EIGHT_ELBOW = SCORE_SIX_ELBOW; - private static final double SCORE_EIGHT_LIFT = 0.45; + private static final double SCORE_EIGHT_SHOULDER = SCORE_ONE_SHOULDER; + private static final double SCORE_EIGHT_WRIST = 0.235; + private static final double SCORE_EIGHT_ELBOW = 0.67; + private static final double SCORE_EIGHT_LIFT = .66; // score position nine button map (gamepad2.left_trigger && gamepad2.y) - private static final double SCORE_NINE_SHOULDER = SCORE_SIX_SHOULDER; - private static final double SCORE_NINE_WRIST = SCORE_SIX_WRIST; - private static final double SCORE_NINE_ELBOW = SCORE_SIX_ELBOW; - private static final double SCORE_NINE_LIFT = 0.45; + private static final double SCORE_NINE_SHOULDER = SCORE_ONE_SHOULDER; + private static final double SCORE_NINE_WRIST = 0.174; + private static final double SCORE_NINE_ELBOW = 0.72; + private static final double SCORE_NINE_LIFT = .66; // score position ten button map (gamepad2.left_trigger && gamepad2.b) - private static final double SCORE_TEN_SHOULDER = SCORE_SIX_SHOULDER; - private static final double SCORE_TEN_WRIST = SCORE_SIX_WRIST; - private static final double SCORE_TEN_ELBOW = SCORE_SIX_ELBOW; - private static final double SCORE_TEN_LIFT = 0.45; + private static final double SCORE_TEN_SHOULDER = SCORE_ONE_SHOULDER; + private static final double SCORE_TEN_WRIST = SCORE_ONE_WRIST; + private static final double SCORE_TEN_ELBOW = SCORE_ONE_ELBOW; + private static final double SCORE_TEN_LIFT = .66; // score position eleven button map (gamepad2.left_trigger && gamepad2.a) - private static final double SCORE_ELEVEN_SHOULDER = SCORE_SIX_SHOULDER; - private static final double SCORE_ELEVEN_WRIST = SCORE_SIX_WRIST; - private static final double SCORE_ELEVEN_ELBOW = SCORE_SIX_ELBOW; - private static final double SCORE_ELEVEN_LIFT = 0.45; + private static final double SCORE_ELEVEN_SHOULDER = SCORE_ONE_SHOULDER; + private static final double SCORE_ELEVEN_WRIST = SCORE_ONE_WRIST; + private static final double SCORE_ELEVEN_ELBOW = SCORE_ONE_ELBOW; + private static final double SCORE_ELEVEN_LIFT = .66; // sensors private RevTouchSensor rightUpper; @@ -752,6 +752,8 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Shoulder Position", shoulder.getPosition()); telemetry.addData("Wrist Position", wrist.getPosition()); telemetry.addData("Elbow Position", elbow.getPosition()); + telemetry.addData("Lift Left", leftLift.getPosition()); + telemetry.addData("Lift Right", rightLift.getPosition()); //telemetry.addData("Left Lower", leftLower.isPressed() ? "Pressed" : "Not Pressed"); //telemetry.addData("Left Upper", leftUpper.isPressed() ? "Pressed" : "Not Pressed"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MutationOld.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MutationOld.java index 6e8ff36946..1951c6af97 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MutationOld.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MutationOld.java @@ -16,7 +16,7 @@ import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; -@TeleOp +//@TeleOp public class MutationOld extends LinearOpMode { // Declare vars private RevTouchSensor rightUpper; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java index 044fb74db5..b38f2bdea7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdoMec.java @@ -9,7 +9,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; -@TeleOp +//@TeleOp public class OdoMec extends LinearOpMode { // adding for push test // Declare vars From 55566aab854daedfc742238d914db26a21337a99 Mon Sep 17 00:00:00 2001 From: Gabriel Date: Wed, 13 Dec 2023 17:44:46 -0700 Subject: [PATCH 131/154] new thread for isolating drive code --- .../firstinspires/ftc/teamcode/Mutation.java | 38 ++++++++++++++++--- 1 file changed, 33 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index f81ac9c592..40e2fdcce0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -136,7 +136,7 @@ public class Mutation extends LinearOpMode { private static final double SCORE_ELEVEN_WRIST = 0.93; private static final double SCORE_ELEVEN_ELBOW = 0.76; private static final double SCORE_ELEVEN_LIFT = 0.98; - + // sensors private RevTouchSensor rightUpper; private RevTouchSensor leftUpper; @@ -324,7 +324,7 @@ private void drivingFunction() { // Check if the right bumper is pressed and the drive state is IDLE if (gamepad1.right_bumper && currentDriveState == Mutation.driveState.IDLE) { activeDrivePosition = new Mutation.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_TUCK, ELBOW_DRIVE, MED_ACC, MED_VEL); - currentDriveState = driveState.MOVING_LIFT; + currentDriveState = Mutation.driveState.MOVING_LIFT; } if (activeDrivePosition != null) { @@ -565,6 +565,22 @@ public Servo setLiftPosition(double targetPosition) { rightLift.setPosition(targetPosition); return null; } + + private class MecanumDriveRunnable implements Runnable { + public volatile boolean running = true; + + @Override + public void run() { + while (running && opModeIsActive()) { + driveCode(); + try { + Thread.sleep(10); // Small delay to prevent high CPU usage + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + } + } + } private void driveCode() { // mecanum drive double forward = gamepad1.left_stick_y; @@ -737,6 +753,10 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Status", "OdoMec is ready to run!"); telemetry.addData("Initializing TeleOp",""); + MecanumDriveRunnable mecanumDriveRunnable = new MecanumDriveRunnable(); + Thread mecanumDriveThread = new Thread(mecanumDriveRunnable); + mecanumDriveThread.start(); + waitForStart(); runtime.reset(); @@ -759,9 +779,7 @@ public void runOpMode() throws InterruptedException { // launcher airplane(); - // mecanum drive - driveCode(); - // hang + // hang hangCode(); // claws grapdropFunction(); @@ -773,6 +791,16 @@ public void runOpMode() throws InterruptedException { scoringFunction(); // emergency stop slides emergencyStop(); + // mecanum drive + //driveCode(); + // Stop the mecanum drive thread after the op mode is over + mecanumDriveRunnable.running = false; + mecanumDriveThread.interrupt(); + try { + mecanumDriveThread.join(); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } telemetry.update(); } From 8e19850d0b197b4063688555790876070fb60346 Mon Sep 17 00:00:00 2001 From: Gabriel Date: Wed, 13 Dec 2023 17:47:39 -0700 Subject: [PATCH 132/154] new thread for isolating drive code --- .../src/main/java/org/firstinspires/ftc/teamcode/Mutation.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 9350f818f6..e08dfa548d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -795,7 +795,7 @@ public void runOpMode() throws InterruptedException { // emergency stop slides emergencyStop(); // mecanum drive - //driveCode(); + //driveCode(); say what // Stop the mecanum drive thread after the op mode is over mecanumDriveRunnable.running = false; mecanumDriveThread.interrupt(); From d14b20f63af4c2060c0211375387690d21f9ef41 Mon Sep 17 00:00:00 2001 From: russmc Date: Thu, 14 Dec 2023 11:20:45 -0700 Subject: [PATCH 133/154] went through and checked the code, reorganized, functions to be with states, added new servo movement code and updated positions to include acc & vel --- .../firstinspires/ftc/teamcode/Mutation.java | 48 ++++++++++++------- 1 file changed, 31 insertions(+), 17 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index e08dfa548d..5cfcb0f25d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -530,8 +530,8 @@ private void moveServoWithTrapezoidalVelocity(Servo servo, double targetPosition // Update the current velocity for the next iteration currentVelocity = desiredVelocity; - telemetry.addData("Servo Position", currentPosition); - telemetry.update(); + // telemetry.addData("Servo Position", currentPosition); + // telemetry.update(); } } @@ -573,11 +573,13 @@ private class MecanumDriveRunnable implements Runnable { @Override public void run() { while (running && opModeIsActive()) { - driveCode(); try { - Thread.sleep(10); // Small delay to prevent high CPU usage + driveCode(); + Thread.sleep(10); } catch (InterruptedException e) { - Thread.currentThread().interrupt(); + running = false; // Set running to false to stop the loop + Thread.currentThread().interrupt(); // Preserve interrupt status + break; // Break the loop to stop the thread } } } @@ -756,15 +758,27 @@ public void runOpMode() throws InterruptedException { MecanumDriveRunnable mecanumDriveRunnable = new MecanumDriveRunnable(); Thread mecanumDriveThread = new Thread(mecanumDriveRunnable); - mecanumDriveThread.start(); + waitForStart(); runtime.reset(); + mecanumDriveThread.start(); while(opModeIsActive()){ // telemetry + telemetry.addData("Right Front Power", rightFront.getPower()); + telemetry.addData("Left Front Power", leftFront.getPower()); + telemetry.addData("Right Back Power", rightBack.getPower()); + telemetry.addData("Left Back Power", leftBack.getPower()); + telemetry.addData("Forward", gamepad1.left_stick_y); + telemetry.addData("Strafe", -gamepad1.left_stick_x); + telemetry.addData("Turn", -gamepad1.right_stick_x); + telemetry.addData("Mecanum Thread Running", mecanumDriveRunnable.running); + telemetry.addData("Loop Time", "Duration: " + runtime.milliseconds() + " ms"); + + /* telemetry.addData("Status", "Run " + runtime.toString()); telemetry.addData("Intake", currentIntakeState); telemetry.addData("Drive", currentDriveState); @@ -774,7 +788,7 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Elbow Position", elbow.getPosition()); telemetry.addData("Lift Left", leftLift.getPosition()); telemetry.addData("Lift Right", rightLift.getPosition()); - + */ //telemetry.addData("Left Lower", leftLower.isPressed() ? "Pressed" : "Not Pressed"); //telemetry.addData("Left Upper", leftUpper.isPressed() ? "Pressed" : "Not Pressed"); //telemetry.addData("Right Lower", rightLower.isPressed() ? "Pressed" : "Not Pressed"); @@ -782,7 +796,7 @@ public void runOpMode() throws InterruptedException { // launcher airplane(); - // hang + // hang hangCode(); // claws grapdropFunction(); @@ -796,17 +810,17 @@ public void runOpMode() throws InterruptedException { emergencyStop(); // mecanum drive //driveCode(); say what - // Stop the mecanum drive thread after the op mode is over - mecanumDriveRunnable.running = false; - mecanumDriveThread.interrupt(); - try { - mecanumDriveThread.join(); - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); - } - telemetry.update(); } + // Stop the mecanum drive thread after the op mode is over + mecanumDriveRunnable.running = false; + mecanumDriveThread.interrupt(); + try { + mecanumDriveThread.join(); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + } } \ No newline at end of file From 2646eae9bc33b7f0f2e6f9ae5a10e3396c8bcd58 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Thu, 14 Dec 2023 15:13:33 -0700 Subject: [PATCH 134/154] testing mecanum drive thread to avoid blocking code such as loops (while/for) and sleep() and left comments explaining --- .../main/java/org/firstinspires/ftc/teamcode/Mutation.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 5cfcb0f25d..c3507a9603 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -324,7 +324,7 @@ private void handleDriveSequence(Mutation.DrivePosition drivePos) { private void drivingFunction() { // Check if the right bumper is pressed and the drive state is IDLE if (gamepad1.right_bumper && currentDriveState == Mutation.driveState.IDLE) { - activeDrivePosition = new Mutation.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_TUCK, ELBOW_DRIVE, MED_ACC, MED_VEL); + activeDrivePosition = new Mutation.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_TUCK, ELBOW_DRIVE, HIGH_ACC, HIGH_VEL); currentDriveState = Mutation.driveState.MOVING_LIFT; } @@ -567,6 +567,7 @@ public Servo setLiftPosition(double targetPosition) { return null; } + // Mecanum thread creation private class MecanumDriveRunnable implements Runnable { public volatile boolean running = true; @@ -756,6 +757,7 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Status", "OdoMec is ready to run!"); telemetry.addData("Initializing TeleOp",""); + // Mecanum drive in a separate thread to avoid blocks in state machines like any loops (while/for) or sleep() MecanumDriveRunnable mecanumDriveRunnable = new MecanumDriveRunnable(); Thread mecanumDriveThread = new Thread(mecanumDriveRunnable); @@ -768,6 +770,7 @@ public void runOpMode() throws InterruptedException { while(opModeIsActive()){ // telemetry + /* telemetry.addData("Right Front Power", rightFront.getPower()); telemetry.addData("Left Front Power", leftFront.getPower()); telemetry.addData("Right Back Power", rightBack.getPower()); @@ -778,7 +781,7 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Mecanum Thread Running", mecanumDriveRunnable.running); telemetry.addData("Loop Time", "Duration: " + runtime.milliseconds() + " ms"); - /* + telemetry.addData("Status", "Run " + runtime.toString()); telemetry.addData("Intake", currentIntakeState); telemetry.addData("Drive", currentDriveState); From 999fb2f767f7faf0dc9931af6714ec5be97f3992 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Thu, 14 Dec 2023 17:28:23 -0700 Subject: [PATCH 135/154] SUPER --- .../org/firstinspires/ftc/teamcode/EnigmaAuto.java | 2 +- .../java/org/firstinspires/ftc/teamcode/Mutation.java | 11 +++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 64a53f0d36..4ce7f6d82e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -64,7 +64,7 @@ /** * ENIGMA Autonomous Example for only vision detection using openCv and park */ -@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "Beginnings") +@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "Mutation") public class EnigmaAuto extends LinearOpMode { public static String TEAM_NAME = "ENIGMA"; //TODO: Enter team Name diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index c3507a9603..15993ac29d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -44,6 +44,9 @@ public class Mutation extends LinearOpMode { private static final double HIGH_ACC = 11.25; private static final double HIGH_VEL = 13.5; + private static final double SUPER_ACC = 18.0; + private static final double SUPER_VEL = 20.0; + // servo values public static final double SHOULDER_DRIVE = 0.425; // 0.425 @@ -228,7 +231,7 @@ private void intakeFunction() { // claw intake from floor //TODO: add saftey if (gamepad1.left_bumper && currentIntakeState == Mutation.intakeState.IDLE) { - activeIntakePosition = new Mutation.IntakePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE, MED_ACC, MED_VEL); + activeIntakePosition = new Mutation.IntakePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE, SUPER_ACC, SUPER_VEL); currentIntakeState = Mutation.intakeState.MOVING_SHOULDER; } // claw intake the top 2 from a stack of 5 @@ -324,7 +327,7 @@ private void handleDriveSequence(Mutation.DrivePosition drivePos) { private void drivingFunction() { // Check if the right bumper is pressed and the drive state is IDLE if (gamepad1.right_bumper && currentDriveState == Mutation.driveState.IDLE) { - activeDrivePosition = new Mutation.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_TUCK, ELBOW_DRIVE, HIGH_ACC, HIGH_VEL); + activeDrivePosition = new Mutation.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_TUCK, ELBOW_DRIVE, SUPER_ACC, SUPER_VEL); currentDriveState = Mutation.driveState.MOVING_LIFT; } @@ -415,13 +418,13 @@ private void scoringFunction() { // score position one if (gamepad2.y && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_ONE_LIFT, SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, MED_ACC, MED_VEL); + activeScorePosition = new Mutation.ScorePosition(SCORE_ONE_LIFT, SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, SUPER_ACC, SUPER_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position two if (gamepad2.b && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Mutation.scoreState.IDLE)) { // Assign a new ScorePosition inside the if block - activeScorePosition = new Mutation.ScorePosition(SCORE_TWO_LIFT, SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, MED_ACC, MED_VEL); + activeScorePosition = new Mutation.ScorePosition(SCORE_TWO_LIFT, SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, SUPER_ACC, SUPER_VEL); currentScoreState = Mutation.scoreState.MOVING_SHOULDER; } // score position three From 748aeee2e2e73f7b0b2c77e6dc56425e343ad873 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Thu, 14 Dec 2023 18:52:53 -0700 Subject: [PATCH 136/154] 166 is our best. Ready for comp! --- .../main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java | 2 +- .../main/java/org/firstinspires/ftc/teamcode/Mutation.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 4ce7f6d82e..d85a3154eb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -302,7 +302,7 @@ public void runAutonoumousMode() { case MIDDLE: dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(28.25, -36, Math.toRadians(90)); + dropYellowPixelPose = new Pose2d(27, -36, Math.toRadians(90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 15993ac29d..88fef6fa8c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -44,8 +44,8 @@ public class Mutation extends LinearOpMode { private static final double HIGH_ACC = 11.25; private static final double HIGH_VEL = 13.5; - private static final double SUPER_ACC = 18.0; - private static final double SUPER_VEL = 20.0; + private static final double SUPER_ACC = 20.0; + private static final double SUPER_VEL = 22.0; // servo values From 2041b812a10c5e1bf80fb0f41c063d53c000fe70 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 30 Dec 2023 10:50:51 -0700 Subject: [PATCH 137/154] State prep --- .../ftc/teamcode/EnigmaAuto2QualBackup.java | 590 +++++++++++++ .../firstinspires/ftc/teamcode/Evolution.java | 833 ++++++++++++++++++ .../firstinspires/ftc/teamcode/Mutation.java | 4 +- 3 files changed, 1426 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto2QualBackup.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto2QualBackup.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto2QualBackup.java new file mode 100644 index 0000000000..5feef7e0df --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto2QualBackup.java @@ -0,0 +1,590 @@ +/* Copyright (c) 2019 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. + */ +//do he auto and get a 2+2 +package org.firstinspires.ftc.teamcode; + +import static com.qualcomm.robotcore.util.ElapsedTime.Resolution.SECONDS; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.hardware.rev.RevTouchSensor; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvInternalCamera; +import org.openftc.easyopencv.OpenCvPipeline; + +/** + * ENIGMA Autonomous Example for only vision detection using openCv and park + */ +@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "Mutation") +public class EnigmaAuto2QualBackup extends LinearOpMode { + + public static String TEAM_NAME = "ENIGMA"; //TODO: Enter team Name + public static int TEAM_NUMBER = 16265; //TODO: Enter team Number + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + private ElapsedTime runtime = new ElapsedTime(); + private ElapsedTime servoTimer = new ElapsedTime(); + + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; + private Servo rightLift; + private Servo leftLift; + private Servo shoulder; + private Servo wrist; + private Servo elbow; + private Servo leftFinger; + private Servo rightFinger; + + double LiftLeftOffset = .04; + double LiftHeight; + + private static final double ELBOW_DRIVE= Mutation.ELBOW_DRIVE; + private static final double ELBOW_INTAKE = Mutation.ELBOW_INTAKE; + private static final double WRIST_INTAKE = Mutation.WRIST_INTAKE; + //private static final double SHOULDER_DRIVE = 0.425; // 0.425 + private static final double SCORE_ONE_SHOULDER = Mutation.SCORE_ONE_SHOULDER; + private static final double SCORE_ONE_WRIST = Mutation.SCORE_ONE_WRIST; + private static final double SCORE_ONE_ELBOW = Mutation.SCORE_ONE_ELBOW; + + + //Define and declare Robot Starting Locations + public enum START_POSITION{ + BLUE_LEFT, + BLUE_RIGHT, + RED_LEFT, + RED_RIGHT + } + public static START_POSITION startPosition; + + /* + OpenCV / Color Detection + */ + OpenCvCamera webcam1 = null; + + public enum IDENTIFIED_SPIKE_MARK_LOCATION { + LEFT, + MIDDLE, + RIGHT + } + public static IDENTIFIED_SPIKE_MARK_LOCATION identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; + public static double leftavgfinoutput = 0; + public static double centeravgfinoutput = 0; + public static double rightavgfinoutput = 0; + + private double servoposition = 0.0; + private double servodelta = 0.02; + private double servodelaytime = 0.03; + + private void moveServoGradually(Servo servo, double targetPosition) { + double currentPosition = servo.getPosition(); + + // Check if enough time has passed since the last update + if (servoTimer.seconds() > servodelaytime) { + // Determine the direction of movement + double direction = targetPosition > currentPosition ? servodelta : -servodelta; + + // Calculate the new position + servoposition = currentPosition + direction; + servoposition = Range.clip(servoposition, 0, 1); // Ensure the position is within valid range + + // Update the servo position + servo.setPosition(servoposition); + + // Reset the timer + servoTimer.reset(); + } + } + + private Servo setLiftPosition(double targetPosition) { + // Ensure the target position is within the valid range + targetPosition = Math.max(0.0, Math.min(targetPosition, 1.0)); + + // Set the servo positions + leftLift.setPosition(targetPosition); + rightLift.setPosition(targetPosition); + return null; + } + + @Override + public void runOpMode() throws InterruptedException { + wrist = hardwareMap.get(Servo.class, "wrist"); + + shoulder = hardwareMap.get(Servo.class, "shoulder"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + elbow = hardwareMap.get(Servo.class, "elbow"); + leftFinger = hardwareMap.get(Servo.class, "lFinger"); + rightFinger = hardwareMap.get(Servo.class, "rFinger"); + + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + + + //init pos + setLiftPosition(Mutation.LIFT_DRIVE); + shoulder.setPosition(Mutation.SHOULDER_DRIVE); + wrist.setPosition(Mutation.WRIST_INTAKE); + elbow.setPosition(Mutation.ELBOW_DRIVE); + sleep(2500); + leftFinger.setPosition(Mutation.LEFT_FINGER_GRIP); + rightFinger.setPosition(Mutation.RIGHT_FINGER_GRIP); + + // Vision OpenCV / Color Detection + WebcamName webcamName = hardwareMap.get(WebcamName.class, "Webcam 1"); + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + webcam1 = OpenCvCameraFactory.getInstance().createInternalCamera(OpenCvInternalCamera.CameraDirection.BACK, cameraMonitorViewId); + //webcam1 = OpenCvCameraFactory.getInstance().createWebcam(webcamName, cameraMonitorViewId); + + webcam1.setPipeline(new teamElementPipeline()); + + webcam1.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + webcam1.startStreaming(1280, 720, OpenCvCameraRotation.SENSOR_NATIVE); + } + + @Override + public void onError(int errorCode) { + + } + }); + + + telemetry.setMsTransmissionInterval(50); + + //Key Pay inputs to selecting Starting Position of robot + selectStartingPosition(); + telemetry.addData("Selected Starting Position", startPosition); + + //Activate Camera Vision that uses TensorFlow for pixel detection + //initTfod(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch Play to start OpMode"); + telemetry.update(); + //waitForStart(); + + while (!isStopRequested() && !opModeIsActive()) { + telemetry.addData("Selected Starting Position", startPosition); + + //Run Vuforia Tensor Flow and keep watching for the identifier in the Signal Cone. + // runTfodTensorFlow(); + telemetry.addData("Vision identified Parking Location", identifiedSpikeMarkLocation); + telemetry.addData("leftavfin", leftavgfinoutput); + telemetry.addData("centeravfin", centeravgfinoutput); + telemetry.addData("rightavfin", rightavgfinoutput); + telemetry.update(); + } + + //Game Play Button is pressed + if (opModeIsActive() && !isStopRequested()) { + //Build parking trajectory based on last detected target by vision + runAutonoumousMode(); + } + } // end runOpMode() + private void setLiftHeight(double inputLiftHeight) { + if (inputLiftHeight < 0.42) { + inputLiftHeight = 0.42; + } + if (inputLiftHeight > 1) { + inputLiftHeight = 1; + } + LiftHeight = inputLiftHeight; + leftLift.setPosition(LiftLeftOffset + LiftHeight); + rightLift.setPosition(LiftHeight); + } + public void runAutonoumousMode() { + //Initialize Pose2d as desired + Pose2d initPose = new Pose2d(0, 0, 0); // Starting Pose + Pose2d moveBeyondTrussPose = new Pose2d(0,0,0); + Pose2d dropPurplePixelPosePush = new Pose2d(0, 0, 0); + //Pose2d dropPurplePixelPosePos = new Pose2d(0, 0, 0); + Pose2d dropPurplePixelPose = new Pose2d(0, 0, 0); + Pose2d midwayPose1 = new Pose2d(0,0,0); + Pose2d midwayPose1a = new Pose2d(0,0,0); + Pose2d intakeStack = new Pose2d(0,0,0); + Pose2d midwayPose2 = new Pose2d(0,0,0); + Pose2d dropYellowPixelPose = new Pose2d(0, 0, 0); + Pose2d parkPose = new Pose2d(0,0, 0); + double waitSecondsBeforeDrop = 0; + MecanumDrive drive = new MecanumDrive(hardwareMap, initPose); + + initPose = new Pose2d(0, 0, Math.toRadians(0)); //Starting pose + moveBeyondTrussPose = new Pose2d(15,0,0); + + //TODO: edit crap here + switch (startPosition) { + case BLUE_LEFT: + drive = new MecanumDrive(hardwareMap, initPose); + switch(identifiedSpikeMarkLocation){ + case LEFT: + dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); + dropYellowPixelPose = new Pose2d(19, 36, Math.toRadians(-90)); + break; + case MIDDLE: + dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(26, 36, Math.toRadians(-90)); + break; + case RIGHT: + dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); + dropYellowPixelPose = new Pose2d(31, 36, Math.toRadians(-90)); + break; + } + midwayPose1 = new Pose2d(14, 13, Math.toRadians(-45)); + waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + parkPose = new Pose2d(8, 30, Math.toRadians(-90)); + break; + + case RED_RIGHT: + drive = new MecanumDrive(hardwareMap, initPose); + switch(identifiedSpikeMarkLocation){ + case LEFT: + dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); + dropYellowPixelPose = new Pose2d(34, -36, Math.toRadians(90)); + break; + case MIDDLE: + dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(27, -36, Math.toRadians(90)); + break; + case RIGHT: + dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); + dropYellowPixelPose = new Pose2d(20, -36, Math.toRadians(90)); + break; + } + midwayPose1 = new Pose2d(14, -13, Math.toRadians(45)); + waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + parkPose = new Pose2d(8, -30, Math.toRadians(90)); + break; + + case BLUE_RIGHT: + drive = new MecanumDrive(hardwareMap, initPose); + switch(identifiedSpikeMarkLocation){ + case LEFT: + dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); + dropYellowPixelPose = new Pose2d(19, 86, Math.toRadians(-90)); + break; + case MIDDLE: + dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(26.25, 86, Math.toRadians(-90)); + break; + case RIGHT: + dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); + dropYellowPixelPose = new Pose2d(33.5, 86, Math.toRadians(-90)); + break; + } + midwayPose1 = new Pose2d(8, -8, Math.toRadians(0)); + midwayPose1a = new Pose2d(18, -21, Math.toRadians(-90)); + intakeStack = new Pose2d(52, -19,Math.toRadians(-90)); + midwayPose2 = new Pose2d(52, 62, Math.toRadians(-90)); + waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + parkPose = new Pose2d(50, 84, Math.toRadians(-90)); + break; + + case RED_LEFT: + drive = new MecanumDrive(hardwareMap, initPose); + switch(identifiedSpikeMarkLocation){ + case LEFT: + dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); + dropYellowPixelPose = new Pose2d(34, -86, Math.toRadians(90)); + break; + case MIDDLE: + dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up + dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); + dropYellowPixelPose = new Pose2d(29, -86, Math.toRadians(90)); + break; + case RIGHT: + dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); + dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); + dropYellowPixelPose = new Pose2d(23, -86, Math.toRadians(90)); + break; + } + midwayPose1 = new Pose2d(8, 8, Math.toRadians(0)); + midwayPose1a = new Pose2d(18, 21, Math.toRadians(90)); + intakeStack = new Pose2d(52, 19,Math.toRadians(90)); + midwayPose2 = new Pose2d(52, -62, Math.toRadians(90)); + waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + parkPose = new Pose2d(50, -84, Math.toRadians(90)); + break; + } + + //Move robot to dropPurplePixel based on identified Spike Mark Location + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(moveBeyondTrussPose.position, moveBeyondTrussPose.heading) + .strafeToLinearHeading(dropPurplePixelPosePush.position, dropPurplePixelPosePush.heading) + .strafeToLinearHeading(dropPurplePixelPose.position, dropPurplePixelPose.heading) + //.strafeToLinearHeading(dropPurplePixelPosePos.position, dropPurplePixelPosePos.heading) + .build()); + + //TODO : Code to drop Purple Pixel on Spike Mark + safeWaitSeconds(1); + shoulder.setPosition(Mutation.SHOULDER_DRIVE); + wrist.setPosition(Mutation.WRIST_INTAKE); + for(int c = 0; c<40; c++) { + moveServoGradually(elbow, ELBOW_INTAKE); + sleep(10); + } + sleep(200); + rightFinger.setPosition(Mutation.RIGHT_FINGER_DROP); + sleep(200); + elbow.setPosition(ELBOW_DRIVE); + wrist.setPosition(Mutation.WRIST_TUCK); + + //Move robot to midwayPose1 + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(midwayPose1.position, midwayPose1.heading) + .build()); + + //For Blue Right and Red Left, intake pixel from stack + if (startPosition == START_POSITION.BLUE_RIGHT || + startPosition == START_POSITION.RED_LEFT) { + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(midwayPose1a.position, midwayPose1a.heading) + .strafeToLinearHeading(intakeStack.position, intakeStack.heading) + .build()); + + //TODO : Code to intake pixel from stack + safeWaitSeconds(1); + + //Move robot to midwayPose2 and to dropYellowPixelPose + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(midwayPose2.position, midwayPose2.heading) + .build()); + } + + safeWaitSeconds(waitSecondsBeforeDrop); + + //Move robot to midwayPose2 and to dropYellowPixelPose + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .setReversed(true) + .splineToLinearHeading(dropYellowPixelPose,0) + .build()); + + + //TODO : Code to drop Pixel on Backdrop + safeWaitSeconds(1); + for(int w = 0; w<40; w++) { + moveServoGradually(wrist, SCORE_ONE_WRIST); + sleep(10); + } + for(int e = 0; e<20; e++) { + moveServoGradually(elbow, SCORE_ONE_ELBOW); + sleep(10); + } + for(int s = 0; s<200; s++) { + moveServoGradually(shoulder, SCORE_ONE_SHOULDER); + sleep(7); + } + sleep(200); + leftFinger.setPosition(Mutation.LEFT_FINGER_DROP); + sleep(100); + for(int s = 0; s<200; s++) { + moveServoGradually(shoulder, Mutation.SHOULDER_DRIVE); + sleep(7); + } + for(int w = 0; w<40; w++) { + moveServoGradually(wrist, WRIST_INTAKE); + sleep(10); + } + for(int e = 0; e<20; e++) { + moveServoGradually(elbow, ELBOW_DRIVE); + sleep(10); + } + + + + //Move robot to park in Backstage + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(parkPose.position, parkPose.heading) + //.splineToLinearHeading(parkPose,0) + .build()); + } + + + //Method to select starting position using X, Y, A, B buttons on gamepad + public void selectStartingPosition() { + telemetry.setAutoClear(true); + telemetry.clearAll(); + //******select start pose***** + while(!isStopRequested()){ + telemetry.addData("Initializing ENIGMA Autonomous Team# a", + TEAM_NAME, " ", TEAM_NUMBER); + telemetry.addData("---------------------------------------",""); + telemetry.addData("Select Starting Position using XYAB:",""); + telemetry.addData(" Blue Left ", "(X)"); + telemetry.addData(" Blue Right ", "(Y)"); + telemetry.addData(" Red Left ", "(B)"); + telemetry.addData(" Red Right ", "(A)"); + if(gamepad1.x){ + startPosition = START_POSITION.BLUE_LEFT; + break; + } + if(gamepad1.y){ + startPosition = START_POSITION.BLUE_RIGHT; + break; + } + if(gamepad1.b){ + startPosition = START_POSITION.RED_LEFT; + break; + } + if(gamepad1.a){ + startPosition = START_POSITION.RED_RIGHT; + break; + } + telemetry.update(); + } + telemetry.clearAll(); + } + + //method to wait safely with stop button working if needed. Use this instead of sleep + public void safeWaitSeconds(double time) { + ElapsedTime timer = new ElapsedTime(SECONDS); + timer.reset(); + while (!isStopRequested() && timer.time() < time) { + } + } +class teamElementPipeline extends OpenCvPipeline{ + Mat YCbCr = new Mat(); + Mat leftCrop; + Mat centerCrop; + Mat rightCrop; + double leftavgfin; + double centeravgfin; + double rightavgfin; + Mat outPut = new Mat(); + Scalar rectColor = new Scalar(255.0, 0.0, 0.0); + + public Mat processFrame(Mat input){ + Imgproc.cvtColor(input,YCbCr,Imgproc.COLOR_RGB2YCrCb); + telemetry.addLine("pipeline running"); + /* OG Config + Rect leftRect = new Rect(1,1,212, 479); + Rect centerRect = new Rect(213,1,212, 479); + Rect rightRect = new Rect(426,1,213, 479); + + Rect leftRect = new Rect(1,300,200, 400); + Rect centerRect = new Rect(400,300,500, 200); + Rect rightRect = new Rect(1000,300,260, 400); +*/ + // testing tighter frames + + Rect leftRect = new Rect(1,380,125, 200); + Rect centerRect = new Rect(500,300,200, 200); + Rect rightRect = new Rect(1100,380,150, 200); + + + + + input.copyTo(outPut); + + + Imgproc.rectangle(outPut, leftRect, rectColor, 2); + Imgproc.rectangle(outPut, centerRect, rectColor, 2); + Imgproc.rectangle(outPut, rightRect, rectColor, 2); + + leftCrop = YCbCr.submat(leftRect); + centerCrop = YCbCr.submat(centerRect); + rightCrop = YCbCr.submat(rightRect); + + if (startPosition == START_POSITION.BLUE_LEFT || startPosition == START_POSITION.BLUE_RIGHT) { + Core.extractChannel(leftCrop, leftCrop, 0); + Core.extractChannel(centerCrop, centerCrop, 0); + Core.extractChannel(rightCrop, rightCrop, 0); // blue is 0, green is 1, red is 2 + } else if (startPosition == START_POSITION.RED_LEFT || startPosition == START_POSITION.RED_RIGHT) { + Core.extractChannel(leftCrop, leftCrop, 2); + Core.extractChannel(centerCrop, centerCrop, 2); + Core.extractChannel(rightCrop, rightCrop, 2); // blue is 0, green is 1, red is 2 + } else { + Core.extractChannel(leftCrop, leftCrop, 0); + Core.extractChannel(centerCrop, centerCrop, 0); + Core.extractChannel(rightCrop, rightCrop, 0); // blue is 0, green is 1, red is 2 + } + + Scalar leftavg = Core.mean(leftCrop); + Scalar centeravg = Core.mean(centerCrop); + Scalar rightavg = Core.mean(rightCrop); + + leftavgfin = leftavg.val[0]; + centeravgfin = centeravg.val[0]; + rightavgfin = rightavg.val[0]; + + leftavgfinoutput = leftavgfin; + centeravgfinoutput = centeravgfin; + rightavgfinoutput = rightavgfin; + + if (leftavgfin < centeravgfin && leftavgfin < rightavgfin) { + //telemetry.addLine("Left"); + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.LEFT; + } else if (centeravgfin < leftavgfin && centeravgfin < rightavgfin) { + //telemetry.addLine("Center"); + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.MIDDLE; + } else if (rightavgfin < leftavgfin && rightavgfin < centeravgfin) { + //telemetry.addLine("Right"); + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.RIGHT; + } else { + //telemetry.addLine("Failed to detect position"); + identifiedSpikeMarkLocation = IDENTIFIED_SPIKE_MARK_LOCATION.RIGHT; + } + return (outPut); + } +} +} // end class diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java new file mode 100644 index 0000000000..422ecbc485 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java @@ -0,0 +1,833 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.rev.RevTouchSensor; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +//@TeleOp +public class Evolution extends LinearOpMode { + + // Declare vars + // timers + private ElapsedTime runtime = new ElapsedTime(); + private ElapsedTime servoTimer = new ElapsedTime(); + + // motors + private DcMotor rightFront; //front right 0 + private DcMotor leftFront; //front left 2 + private DcMotor rightBack; //rear right 1 + private DcMotor leftBack; //rear left 3 + private DcMotor leftHang; + private DcMotor rightHang; + + // servos + private Servo launcher; + private Servo shoulder; + private Servo wrist; + private Servo elbow; + private Servo leftFinger; + private Servo rightFinger; + private Servo rightLift; + private Servo leftLift; + private double servoposition = 0.0; + private double servodelta = 0.02; + private double servodelaytime = 0.03; + + private static final double LOW_ACC = 7.25; + private static final double LOW_VEL = 8.5; + private static final double MED_ACC = 10.25; + private static final double MED_VEL = 11.5; + private static final double HIGH_ACC = 11.25; + private static final double HIGH_VEL = 13.5; + + private static final double SUPER_ACC = 20.0; + private static final double SUPER_VEL = 22.0; + + + // servo values + public static final double AIR_PLANE_INT = 0.1; + public static final double AIR_PLANE_SHOOT = 0.8; + public static final double SHOULDER_DRIVE = 0.425; // 0.425 + public static final double SHOULDER_INT = 0.43; + public static final double ELBOW_DRIVE= 0.47; + public static final double ELBOW_INTAKE = 0.78; + public static final double WRIST_TUCK = 0.29; + public static final double WRIST_DRIVE = 0.82; + public static final double WRIST_INTAKE = 0.555; + public static final double LEFT_FINGER_GRIP = 0.72; + public static final double LEFT_FINGER_DROP = 0.5; + public static final double LEFT_FINGER_INTAKE = 0.3; + public static final double RIGHT_FINGER_GRIP = .27; + public static final double RIGHT_FINGER_DROP = .5; + public static final double RIGHT_FINGER_INTAKE = 0.64; + public static final double TRIGGER_THRESHOLD = 0.5; + public static final double LAUNCHER_START_POS = 0.8; + public static final double SERVO_TOLERANCE = 0.01; + public static final double LIFT_DRIVE = 0.10; // 0.78 is the highest it can mechanically go right now + private double liftTargetPosition = LIFT_DRIVE; // was 0.37 before moving servos for larger range + + // stack positions (top 2 o 5 and next 2 of 3 ) + // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin + // intake two off a stack of five + public static final double SHOULDER_TOP_TWO = 0.425; + public static final double WRIST_TOP_TWO = 0.59; + public static final double ELBOW_TOP_TWO = 0.74; + + // intake two off a stack of three + public static final double SHOULDER_NEXT_TWO = 0.425; + public static final double WRIST_NEXT_TWO = 0.565; + public static final double ELBOW_NEXT_TWO = 0.76; + + + //=^-^= + // score positions (11 rows on the board) + // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin + // score position one button map (gamepad2.y) + public static final double SCORE_ONE_SHOULDER = 0.915; + public static final double SCORE_ONE_WRIST = 0.365; + public static final double SCORE_ONE_ELBOW = 0.57; + public static final double SCORE_ONE_LIFT = LIFT_DRIVE; + // score position two button map (gamepad2.b) + public static final double SCORE_TWO_SHOULDER = SCORE_ONE_SHOULDER; + public static final double SCORE_TWO_WRIST = SCORE_ONE_WRIST; + public static final double SCORE_TWO_ELBOW = SCORE_ONE_ELBOW; + public static final double SCORE_TWO_LIFT = 0.26; + // score position three button map (gamepad2.a) + public static final double SCORE_THREE_SHOULDER = SCORE_ONE_SHOULDER; + public static final double SCORE_THREE_WRIST = SCORE_ONE_WRIST; + public static final double SCORE_THREE_ELBOW = SCORE_ONE_ELBOW; + public static final double SCORE_THREE_LIFT = 0.37; + // score position four button map (gamepad2.x) + public static final double SCORE_FOUR_SHOULDER = SCORE_ONE_SHOULDER; + public static final double SCORE_FOUR_WRIST = SCORE_ONE_WRIST; + public static final double SCORE_FOUR_ELBOW = SCORE_ONE_ELBOW; + public static final double SCORE_FOUR_LIFT = .44; + // score position five button map (gamepad2.left_bumper && gamepad2.y) + public static final double SCORE_FIVE_SHOULDER = SCORE_ONE_SHOULDER; + public static final double SCORE_FIVE_WRIST = SCORE_ONE_WRIST; + public static final double SCORE_FIVE_ELBOW = SCORE_ONE_ELBOW; + public static final double SCORE_FIVE_LIFT = 0.55; + // score position six button map (gamepad2.left_bumper && gamepad2.b) + private static final double SCORE_SIX_SHOULDER = SCORE_ONE_SHOULDER; + private static final double SCORE_SIX_WRIST = SCORE_ONE_WRIST; + private static final double SCORE_SIX_ELBOW = SCORE_ONE_ELBOW; + private static final double SCORE_SIX_LIFT = .66; + // score position seven button map (gamepad2.left_bumper && gamepad2.a) + private static final double SCORE_SEVEN_SHOULDER = SCORE_ONE_SHOULDER; + private static final double SCORE_SEVEN_WRIST = 0.305; + private static final double SCORE_SEVEN_ELBOW = 0.6; + private static final double SCORE_SEVEN_LIFT = .66; + // score position eight button map (gamepad2.left_bumper && gamepad2.x) + private static final double SCORE_EIGHT_SHOULDER = SCORE_ONE_SHOULDER; + private static final double SCORE_EIGHT_WRIST = 0.235; + private static final double SCORE_EIGHT_ELBOW = 0.67; + private static final double SCORE_EIGHT_LIFT = .66; + // score position nine button map (gamepad2.left_trigger && gamepad2.y) + private static final double SCORE_NINE_SHOULDER = SCORE_ONE_SHOULDER; + private static final double SCORE_NINE_WRIST = 0.174; + private static final double SCORE_NINE_ELBOW = 0.72; + private static final double SCORE_NINE_LIFT = .66; + // score position ten button map (gamepad2.left_trigger && gamepad2.b) + private static final double SCORE_TEN_SHOULDER = SCORE_ONE_SHOULDER; + private static final double SCORE_TEN_WRIST = SCORE_ONE_WRIST; + private static final double SCORE_TEN_ELBOW = SCORE_ONE_ELBOW; + private static final double SCORE_TEN_LIFT = .66; + // score position eleven button map (gamepad2.left_trigger && gamepad2.a) + private static final double SCORE_ELEVEN_SHOULDER = SCORE_ONE_SHOULDER; + private static final double SCORE_ELEVEN_WRIST = SCORE_ONE_WRIST; + private static final double SCORE_ELEVEN_ELBOW = SCORE_ONE_ELBOW; + private static final double SCORE_ELEVEN_LIFT = .66; + + // sensors + private RevTouchSensor rightUpper; + private RevTouchSensor leftUpper; + private RevTouchSensor rightLower; + private RevTouchSensor leftLower; + + + // state machines - states enumeration, positions, handle sequences, functions + // INTAKE + private enum intakeState { + IDLE, + MOVING_LIFT, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + MOVING_CLAWS, + COMPLETED + } + + private Evolution.intakeState currentIntakeState = Evolution.intakeState.IDLE; + private Evolution.IntakePosition activeIntakePosition = null; + + + static class IntakePosition { + double liftPosition; + double shoulderPosition; + double wristPosition; + double elbowPosition; + double accelerationMax; + double velocityMax; + + public IntakePosition(double liftPos, double shoulderPos, double wristPos, double elbowPos, double accMax, double velMax) { + this.liftPosition = liftPos; + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + this.accelerationMax = accMax; + this.velocityMax = velMax; + } + } + + private void handleIntakeSequence(Evolution.IntakePosition intakePos) { + switch (currentIntakeState) { + case MOVING_SHOULDER: + // Move the shoulder to intake position + moveServoWithTrapezoidalVelocity(shoulder, intakePos.shoulderPosition, intakePos.accelerationMax, intakePos.velocityMax); + if (isServoAtPosition(shoulder, intakePos.shoulderPosition, SERVO_TOLERANCE)) { + currentIntakeState = Evolution.intakeState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to intake position + //moveServoGradually(wrist, intakePos.wristPosition); + wrist.setPosition(intakePos.wristPosition); + //moveServoWithTrapezoidalVelocity(wrist, intakePos.wristPosition, intakePos.accelerationMax, intakePos.velocityMax); + if (isServoAtPosition(wrist, intakePos.wristPosition, SERVO_TOLERANCE)) { + currentIntakeState = Evolution.intakeState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: + // Move the elbow to intake position so it don't slap the floor + moveServoGradually(elbow, intakePos.elbowPosition); + // moveServoWithTrapezoidalVelocity(elbow, intakePos.elbowPosition, intakePos.accelerationMax, intakePos.velocityMax); + if (isServoAtPosition(elbow, intakePos.elbowPosition, SERVO_TOLERANCE)) { + // Check if the elbow is 70% down and open the claws if it is + currentIntakeState = Evolution.intakeState.MOVING_CLAWS; + } + break; + case MOVING_CLAWS: + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + if (isServoAtPosition(leftFinger, LEFT_FINGER_INTAKE, SERVO_TOLERANCE) || isServoAtPosition(rightFinger, RIGHT_FINGER_INTAKE, SERVO_TOLERANCE)) { + // Check if the elbow is 70% down and open the claws if it is + currentIntakeState = Evolution.intakeState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; + } + // Check to reset the state to IDLE outside the switch + if (currentIntakeState == Evolution.intakeState.COMPLETED) { + currentIntakeState = Evolution.intakeState.IDLE; + } + } + + private void intakeFunction() { + // intake + // claw intake from floor + //TODO: add saftey + if (gamepad1.left_bumper && currentIntakeState == Evolution.intakeState.IDLE) { + activeIntakePosition = new Evolution.IntakePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_INTAKE, ELBOW_INTAKE, SUPER_ACC, SUPER_VEL); + currentIntakeState = Evolution.intakeState.MOVING_SHOULDER; + } + // claw intake the top 2 from a stack of 5 + if (gamepad1.b && currentIntakeState == Evolution.intakeState.IDLE) { + activeIntakePosition = new Evolution.IntakePosition(LIFT_DRIVE, SHOULDER_TOP_TWO, WRIST_TOP_TWO, ELBOW_TOP_TWO, MED_ACC, MED_VEL); + currentIntakeState = Evolution.intakeState.MOVING_SHOULDER; + } + // claw intake the next 2 from a stack of 3 + if (gamepad1.a && currentIntakeState == Evolution.intakeState.IDLE) { + activeIntakePosition = new Evolution.IntakePosition(LIFT_DRIVE, SHOULDER_NEXT_TWO, WRIST_NEXT_TWO, ELBOW_NEXT_TWO, MED_ACC, MED_VEL); + currentIntakeState = Evolution.intakeState.MOVING_SHOULDER; + } + if (activeIntakePosition != null) { + handleIntakeSequence(activeIntakePosition); + } + if (currentIntakeState == Evolution.intakeState.COMPLETED) { + currentIntakeState = Evolution.intakeState.IDLE; + activeIntakePosition = null; // Reset the active position + } + + } + + // DRIVE + private enum driveState { + IDLE, + MOVING_LIFT, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + COMPLETED + } + + private Evolution.driveState currentDriveState = Evolution.driveState.IDLE; + private Evolution.DrivePosition activeDrivePosition = null; + + + static class DrivePosition { + double liftPosition; + double shoulderPosition; + double wristPosition; + double elbowPosition; + double accelerationMax; + double velocityMax; + + public DrivePosition(double liftPos, double shoulderPos, double wristPos, double elbowPos, double accMax, double velMax) { + this.liftPosition = liftPos; + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + this.accelerationMax = accMax; + this.velocityMax = velMax; + } + } + private void handleDriveSequence(Evolution.DrivePosition drivePos) { + switch (currentDriveState) { + case MOVING_LIFT: + setLiftPosition(LIFT_DRIVE); + currentDriveState = Evolution.driveState.MOVING_SHOULDER; + break; + case MOVING_SHOULDER: + // Move the shoulder to intake position + moveServoWithTrapezoidalVelocity(shoulder, drivePos.shoulderPosition, drivePos.accelerationMax, drivePos.velocityMax); + if (isServoAtPosition(shoulder, drivePos.shoulderPosition, SERVO_TOLERANCE)) { + currentDriveState = Evolution.driveState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: + // Move the elbow to intake position so it don't slap the floor + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + moveServoWithTrapezoidalVelocity(elbow, drivePos.elbowPosition, drivePos.accelerationMax, drivePos.velocityMax); + if (isServoAtPosition(elbow, drivePos.elbowPosition, SERVO_TOLERANCE)) { + currentDriveState = Evolution.driveState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to intake position + moveServoWithTrapezoidalVelocity(wrist, drivePos.wristPosition, drivePos.accelerationMax, drivePos.velocityMax); + if (isServoAtPosition(wrist, drivePos.wristPosition, SERVO_TOLERANCE)) { + currentDriveState = Evolution.driveState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; + } + // Check to reset the state to IDLE outside the switch + if (currentDriveState == Evolution.driveState.COMPLETED) { + currentDriveState = Evolution.driveState.IDLE; + } + } + + private void drivingFunction() { + // Check if the right bumper is pressed and the drive state is IDLE + if (gamepad1.right_bumper && currentDriveState == Evolution.driveState.IDLE) { + activeDrivePosition = new Evolution.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_TUCK, ELBOW_DRIVE, SUPER_ACC, SUPER_VEL); + currentDriveState = Evolution.driveState.MOVING_LIFT; + } + + if (activeDrivePosition != null) { + handleDriveSequence(activeDrivePosition); + } + + if (currentDriveState == Evolution.driveState.COMPLETED) { + currentDriveState = Evolution.driveState.IDLE; + activeDrivePosition = null; // Reset the active position + } + } + // SCORE + private enum scoreState { + IDLE, + MOVING_LIFT, + MOVING_SHOULDER, + MOVING_WRIST, + MOVING_ELBOW, + COMPLETED + } + + private Evolution.scoreState currentScoreState = Evolution.scoreState.IDLE; + private Evolution.ScorePosition activeScorePosition = null; + + + static class ScorePosition { + double liftPosition; + double shoulderPosition; + double wristPosition; + double elbowPosition; + double accelerationMax; + double velocityMax; + + public ScorePosition(double liftPos, double shoulderPos, double wristPos, double elbowPos, double accMax, double velMax) { + this.liftPosition = liftPos; + this.shoulderPosition = shoulderPos; + this.wristPosition = wristPos; + this.elbowPosition = elbowPos; + this.accelerationMax = accMax; + this.velocityMax = velMax; + } + } + private void handleScoreSequence(Evolution.ScorePosition scorePos) { + switch (currentScoreState) { + + case MOVING_SHOULDER: + // Move the shoulder to intake position + moveServoWithTrapezoidalVelocity(shoulder, scorePos.shoulderPosition, scorePos.accelerationMax, scorePos.velocityMax); + if (isServoAtPosition(shoulder, scorePos.shoulderPosition, SERVO_TOLERANCE)) { + currentScoreState = Evolution.scoreState.MOVING_ELBOW; + } + break; + case MOVING_ELBOW: + // Move the elbow to intake position so it don't slap the floor + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + moveServoWithTrapezoidalVelocity(elbow, scorePos.elbowPosition, scorePos.accelerationMax, scorePos.velocityMax); + if (isServoAtPosition(elbow, scorePos.elbowPosition, SERVO_TOLERANCE)) { + currentScoreState = Evolution.scoreState.MOVING_WRIST; + } + break; + case MOVING_WRIST: + // Move the wrist to intake position + moveServoWithTrapezoidalVelocity(wrist, scorePos.wristPosition, scorePos.accelerationMax, scorePos.velocityMax); + if (isServoAtPosition(wrist, scorePos.wristPosition, SERVO_TOLERANCE)) { + currentScoreState = Evolution.scoreState.MOVING_LIFT; + } + break; + case MOVING_LIFT: + setLiftPosition(scorePos.liftPosition); + if (isServoAtPosition(leftLift, scorePos.liftPosition, SERVO_TOLERANCE) && isServoAtPosition(rightLift, scorePos.liftPosition, SERVO_TOLERANCE)) { + currentScoreState = Evolution.scoreState.COMPLETED; + } + break; + case COMPLETED: + // Sequence complete, reset the state or perform additional actions + break; + } + // Check to reset the state to IDLE outside the switch + if (currentScoreState == Evolution.scoreState.COMPLETED) { + currentScoreState = Evolution.scoreState.IDLE; + } + } + + private void scoringFunction() { + // scoring + // score position one + if (gamepad2.y && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Evolution.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Evolution.ScorePosition(SCORE_ONE_LIFT, SCORE_ONE_SHOULDER, SCORE_ONE_WRIST, SCORE_ONE_ELBOW, SUPER_ACC, SUPER_VEL); + currentScoreState = Evolution.scoreState.MOVING_SHOULDER; + } + // score position two + if (gamepad2.b && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Evolution.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Evolution.ScorePosition(SCORE_TWO_LIFT, SCORE_TWO_SHOULDER, SCORE_TWO_WRIST, SCORE_TWO_ELBOW, SUPER_ACC, SUPER_VEL); + currentScoreState = Evolution.scoreState.MOVING_SHOULDER; + } + // score position three + if (gamepad2.a && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Evolution.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Evolution.ScorePosition(SCORE_THREE_LIFT, SCORE_THREE_SHOULDER, SCORE_THREE_WRIST, SCORE_THREE_ELBOW, MED_ACC, MED_VEL); + currentScoreState = Evolution.scoreState.MOVING_SHOULDER; + } + // score position four + //TODO: gotta put !gamepad2.left_bumper above + if (gamepad2.x && !gamepad2.left_bumper && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Evolution.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Evolution.ScorePosition(SCORE_FOUR_LIFT, SCORE_FOUR_SHOULDER, SCORE_FOUR_WRIST, SCORE_FOUR_ELBOW, MED_ACC, MED_VEL); + currentScoreState = Evolution.scoreState.MOVING_SHOULDER; + } + // score position five + if ((gamepad2.left_bumper && gamepad2.y) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Evolution.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Evolution.ScorePosition(SCORE_FIVE_LIFT, SCORE_FIVE_SHOULDER, SCORE_FIVE_WRIST, SCORE_FIVE_ELBOW, MED_ACC, MED_VEL); + currentScoreState = Evolution.scoreState.MOVING_SHOULDER; + } + // score position six + if ((gamepad2.left_bumper && gamepad2.b) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Evolution.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Evolution.ScorePosition(SCORE_SIX_LIFT, SCORE_SIX_SHOULDER, SCORE_SIX_WRIST, SCORE_SIX_ELBOW, MED_ACC, MED_VEL); + currentScoreState = Evolution.scoreState.MOVING_SHOULDER; + } + // score position seven + if ((gamepad2.left_bumper && gamepad2.a) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Evolution.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Evolution.ScorePosition(SCORE_SEVEN_LIFT, SCORE_SEVEN_SHOULDER, SCORE_SEVEN_WRIST, SCORE_SEVEN_ELBOW, MED_ACC, MED_VEL); + currentScoreState = Evolution.scoreState.MOVING_SHOULDER; + } + // score position eight + if ((gamepad2.left_bumper && gamepad2.x) && (gamepad2.left_trigger < TRIGGER_THRESHOLD) && (currentScoreState == Evolution.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Evolution.ScorePosition(SCORE_EIGHT_LIFT, SCORE_EIGHT_SHOULDER, SCORE_EIGHT_WRIST, SCORE_EIGHT_ELBOW, MED_ACC, MED_VEL); + currentScoreState = Evolution.scoreState.MOVING_SHOULDER; + } + // score position nine + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.y) && !gamepad2.left_bumper && (currentScoreState == Evolution.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Evolution.ScorePosition(SCORE_NINE_LIFT, SCORE_NINE_SHOULDER, SCORE_NINE_WRIST, SCORE_NINE_ELBOW, MED_ACC, MED_VEL); + currentScoreState = Evolution.scoreState.MOVING_SHOULDER; + } + // score position ten + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.b) && !gamepad2.left_bumper && (currentScoreState == Evolution.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Evolution.ScorePosition(SCORE_TEN_LIFT, SCORE_TEN_SHOULDER, SCORE_TEN_WRIST, SCORE_TEN_ELBOW, MED_ACC, MED_VEL); + currentScoreState = Evolution.scoreState.MOVING_SHOULDER; + } + // score position eleven + if (((gamepad2.left_trigger > TRIGGER_THRESHOLD) && gamepad2.a) && !gamepad2.left_bumper && (currentScoreState == Evolution.scoreState.IDLE)) { + // Assign a new ScorePosition inside the if block + activeScorePosition = new Evolution.ScorePosition(SCORE_ELEVEN_LIFT, SCORE_ELEVEN_SHOULDER, SCORE_ELEVEN_WRIST, SCORE_ELEVEN_ELBOW, MED_ACC, MED_VEL); + currentScoreState = Evolution.scoreState.MOVING_SHOULDER; + } + if (activeScorePosition != null) { + handleScoreSequence(activeScorePosition); + } + if (currentScoreState == Evolution.scoreState.COMPLETED) { + currentScoreState = Evolution.scoreState.IDLE; + activeScorePosition = null; // Reset the active position + } + } + + // functions to support state machine actions using servos + + // check servo positions with some tolerance + private boolean isServoAtPosition(Servo servo, double targetPosition, double tolerance) { + double currentPosition = servo.getPosition(); + double normalizedTarget = Range.clip(targetPosition, 0.0, 1.0); // Ensure target is within valid range + double normalizedCurrent = Range.clip(currentPosition, 0.0, 1.0); // Ensure current position is within valid range + + return Math.abs(normalizedCurrent - normalizedTarget) < tolerance; + } + + // move servo with ramping and soft-start created from the math and methods explained here: https://www.instructables.com/Servo-Ramping-and-Soft-Start/ + private void moveServoWithTrapezoidalVelocity(Servo servo, double targetPosition, double amax, double vmax) { + double currentPosition = servo.getPosition(); + double currentVelocity = 0.0; // Initial velocity is zero + double p0 = 0.0; // Distance needed to come to rest + + ElapsedTime timer = new ElapsedTime(); + timer.reset(); + + while (Math.abs(currentPosition - targetPosition) > SERVO_TOLERANCE && opModeIsActive()) { + // Calculate the elapsed time since the last update + double elapsedTime = timer.seconds(); + timer.reset(); + + // Calculate the distance needed to come to rest given the current velocity + p0 = 2 * currentVelocity + Math.abs(currentVelocity) * currentVelocity / (2 * amax); + + // Calculate the desired velocity based on the position error + double velocityError = targetPosition - currentPosition - p0; + double sign = Math.signum(velocityError); + double desiredVelocity = currentVelocity + sign * amax; + desiredVelocity = Range.clip(desiredVelocity, -vmax, vmax); // Constrain to vmax + + // Update the servo position based on the desired velocity and elapsed time + currentPosition += desiredVelocity * elapsedTime; + currentPosition = Range.clip(currentPosition, 0, 1); // Ensure the position is within valid range + servo.setPosition(currentPosition); + + // Update the current velocity for the next iteration + currentVelocity = desiredVelocity; + + // telemetry.addData("Servo Position", currentPosition); + // telemetry.update(); + } + } + + // moves servo gradually + private void moveServoGradually(Servo servo, double targetPosition) { + double currentPosition = servo.getPosition(); + + // Check if enough time has passed since the last update + if (servoTimer.seconds() > servodelaytime) { + // Determine the direction of movement + double direction = targetPosition > currentPosition ? servodelta : -servodelta; + + // Calculate the new position + servoposition = currentPosition + direction; + servoposition = Range.clip(servoposition, 0, 1); // Ensure the position is within valid range + + // Update the servo position + servo.setPosition(servoposition); + + // Reset the timer + servoTimer.reset(); + } + } + + // lift (has been adjusted mechanically to use the same height) + public Servo setLiftPosition(double targetPosition) { + // Ensure the target position is within the valid range + targetPosition = Math.max(0.0, Math.min(targetPosition, 1.0)); + + // Set the servo positions + leftLift.setPosition(targetPosition); + rightLift.setPosition(targetPosition); + return null; + } + + // Mecanum thread creation + private class MecanumDriveRunnable implements Runnable { + public volatile boolean running = true; + + @Override + public void run() { + while (running && opModeIsActive()) { + try { + driveCode(); + Thread.sleep(10); + } catch (InterruptedException e) { + running = false; // Set running to false to stop the loop + Thread.currentThread().interrupt(); // Preserve interrupt status + break; // Break the loop to stop the thread + } + } + } + } + private void driveCode() { + // mecanum drive + double forward = gamepad1.left_stick_y; + double strafe = -gamepad1.left_stick_x; + double turn = -gamepad1.right_stick_x; + + double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(turn), 1); + + double rightFrontPower = (forward - strafe - turn) / denominator; + double leftFrontPower = (forward + strafe + turn) / denominator; + double rightBackPower = (forward + strafe - turn) / denominator; + double leftBackPower = (forward - strafe + turn) / denominator; + + if (gamepad1.left_bumper) { + rightFrontPower = Range.clip(rightFrontPower, -0.4, 0.4); + leftFrontPower = Range.clip(leftFrontPower, -0.4, 0.4); + rightBackPower = Range.clip(rightBackPower, -0.4, 0.4); + leftBackPower = Range.clip(leftBackPower, -0.4, 0.4); + } else { + rightFrontPower = Range.clip(rightFrontPower, -1, 1); + leftFrontPower = Range.clip(leftFrontPower, -1, 1); + rightBackPower = Range.clip(rightBackPower, -1, 1); + leftBackPower = Range.clip(leftBackPower, -1, 1); + } + + rightFront.setPower(rightFrontPower); + leftFront.setPower(leftFrontPower); + rightBack.setPower(rightBackPower); + leftBack.setPower(leftBackPower); + } + + private void hangCode() { + // hanging + if (gamepad1.right_trigger > TRIGGER_THRESHOLD){ + if (!leftUpper.isPressed()) { + leftHang.setDirection(DcMotor.Direction.FORWARD); + leftHang.setPower(.9); + } + if (!rightUpper.isPressed()) { + rightHang.setDirection(DcMotor.Direction.FORWARD); + rightHang.setPower(.9); + } + } else if (gamepad1.left_trigger > TRIGGER_THRESHOLD){ + if (!leftLower.isPressed()) { + leftHang.setDirection(DcMotor.Direction.REVERSE); + leftHang.setPower(.9); + } + if (!rightLower.isPressed()) { + rightHang.setDirection(DcMotor.Direction.REVERSE); + rightHang.setPower(.9); + } + } else { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setPower(0); + } + } + + + private void emergencyStop() { + if (gamepad1.y) { + leftHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightHang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftHang.setPower(0); + rightHang.setPower(0); + } + } + + // operating the claws to grab and drop + private void grapdropFunction() { + // dropping on the backboard for scoring + if (gamepad2.dpad_up && !gamepad2.right_bumper && currentIntakeState == Evolution.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_left && !gamepad2.right_bumper && currentIntakeState == Evolution.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_DROP); + } else if (gamepad2.dpad_right && !gamepad2.right_bumper && currentIntakeState == Evolution.intakeState.IDLE) { + rightFinger.setPosition(RIGHT_FINGER_DROP); + } else if (gamepad2.dpad_down && !gamepad2.right_bumper && currentIntakeState == Evolution.intakeState.IDLE) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + + // grabbing off the floor or stack + if (gamepad2.right_bumper && gamepad2.dpad_up && (currentScoreState == Evolution.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_left && (currentScoreState == Evolution.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_right && (currentScoreState == Evolution.scoreState.IDLE)) { + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } else if (gamepad2.right_bumper && gamepad2.dpad_down && (currentScoreState == Evolution.scoreState.IDLE)) { + leftFinger.setPosition(LEFT_FINGER_INTAKE); + rightFinger.setPosition(RIGHT_FINGER_INTAKE); + } + } + + // launcher + private void airplane() { + if (gamepad2.back) { + launcher.setPosition(0.1); + } else { + launcher.setPosition(0.8); + } + } + private void launcherstartPos() { + launcher.setPosition(LAUNCHER_START_POS); + } + + @Override + // NOT loop \/ - Or int of vars + public void runOpMode() throws InterruptedException { + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + // hardware mappings and parameters + + // motors + rightFront = hardwareMap.get(DcMotor.class, "rightFront"); + leftFront = hardwareMap.get(DcMotor.class, "leftFront"); + rightBack = hardwareMap.get(DcMotor.class, "rightBack"); + leftBack = hardwareMap.get(DcMotor.class, "leftBack"); + + leftHang = hardwareMap.get(DcMotor.class, "leftHang"); + rightHang = hardwareMap.get(DcMotor.class, "rightHang"); + + rightFront.setDirection(DcMotor.Direction.REVERSE); + leftFront.setDirection(DcMotor.Direction.FORWARD); + rightBack.setDirection(DcMotor.Direction.REVERSE); + leftBack.setDirection(DcMotor.Direction.FORWARD); + + // motor modes + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // servos + shoulder = hardwareMap.get(Servo.class, "shoulder"); + wrist = hardwareMap.get(Servo.class, "wrist"); + elbow = hardwareMap.get(Servo.class, "elbow"); + leftFinger = hardwareMap.get(Servo.class, "lFinger"); + rightFinger = hardwareMap.get(Servo.class, "rFinger"); + launcher = hardwareMap.get(Servo.class, "launcher"); + rightLift = hardwareMap.get(Servo.class, "rightLift"); + leftLift = hardwareMap.get(Servo.class, "leftLift"); + + // servo modes + shoulder.setDirection(Servo.Direction.REVERSE); + wrist.setDirection(Servo.Direction.REVERSE); + launcher.setDirection(Servo.Direction.REVERSE); + + // sensors + leftUpper = hardwareMap.get(RevTouchSensor.class, "leftUpper"); + rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); + leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); + rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); + + // init positions + leftFinger.setPosition(LEFT_FINGER_GRIP); + rightFinger.setPosition(RIGHT_FINGER_GRIP); + setLiftPosition(LIFT_DRIVE); + shoulder.setPosition(SHOULDER_INT); + elbow.setPosition(ELBOW_DRIVE); + wrist.setPosition(WRIST_TUCK); + launcherstartPos(); + + telemetry.addData("Status", "OdoMec is ready to run!"); + telemetry.addData("Initializing TeleOp",""); + + // Mecanum drive in a separate thread to avoid blocks in state machines like any loops (while/for) or sleep() + MecanumDriveRunnable mecanumDriveRunnable = new MecanumDriveRunnable(); + Thread mecanumDriveThread = new Thread(mecanumDriveRunnable); + + + waitForStart(); + runtime.reset(); + + mecanumDriveThread.start(); + + while(opModeIsActive()){ + + // telemetry + /* + telemetry.addData("Right Front Power", rightFront.getPower()); + telemetry.addData("Left Front Power", leftFront.getPower()); + telemetry.addData("Right Back Power", rightBack.getPower()); + telemetry.addData("Left Back Power", leftBack.getPower()); + telemetry.addData("Forward", gamepad1.left_stick_y); + telemetry.addData("Strafe", -gamepad1.left_stick_x); + telemetry.addData("Turn", -gamepad1.right_stick_x); + telemetry.addData("Mecanum Thread Running", mecanumDriveRunnable.running); + telemetry.addData("Loop Time", "Duration: " + runtime.milliseconds() + " ms"); + + + telemetry.addData("Status", "Run " + runtime.toString()); + telemetry.addData("Intake", currentIntakeState); + telemetry.addData("Drive", currentDriveState); + telemetry.addData("Score", currentScoreState); + telemetry.addData("Shoulder Position", shoulder.getPosition()); + telemetry.addData("Wrist Position", wrist.getPosition()); + telemetry.addData("Elbow Position", elbow.getPosition()); + telemetry.addData("Lift Left", leftLift.getPosition()); + telemetry.addData("Lift Right", rightLift.getPosition()); + */ + //telemetry.addData("Left Lower", leftLower.isPressed() ? "Pressed" : "Not Pressed"); + //telemetry.addData("Left Upper", leftUpper.isPressed() ? "Pressed" : "Not Pressed"); + //telemetry.addData("Right Lower", rightLower.isPressed() ? "Pressed" : "Not Pressed"); + //telemetry.addData("Right Upper", rightUpper.isPressed() ? "Pressed" : "Not Pressed"); + + // launcher + airplane(); + // hang + hangCode(); + // claws + grapdropFunction(); + // intake positions + intakeFunction(); + // driving pos + drivingFunction(); + // scoring positions + scoringFunction(); + // emergency stop slides + emergencyStop(); + // mecanum drive + //driveCode(); say what + telemetry.update(); + } + + // Stop the mecanum drive thread after the op mode is over + mecanumDriveRunnable.running = false; + mecanumDriveThread.interrupt(); + try { + mecanumDriveThread.join(); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java index 88fef6fa8c..1288aa9041 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mutation.java @@ -8,7 +8,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; -@TeleOp +//@TeleOp public class Mutation extends LinearOpMode { // Declare vars @@ -49,6 +49,8 @@ public class Mutation extends LinearOpMode { // servo values + public static final double AIR_PLANE_INT = 0.1; + public static final double AIR_PLANE_SHOOT = 0.8; public static final double SHOULDER_DRIVE = 0.425; // 0.425 public static final double SHOULDER_INT = 0.43; public static final double ELBOW_DRIVE= 0.47; From 631fe1d34b04546a5a3bb51165bf40216bfcc78b Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 30 Dec 2023 12:47:59 -0700 Subject: [PATCH 138/154] Testing 2+1 --- .../ftc/teamcode/EnigmaAuto.java | 90 +++++++++++++------ .../ftc/teamcode/EnigmaAuto2QualBackup.java | 2 +- .../firstinspires/ftc/teamcode/Evolution.java | 3 +- .../ftc/teamcode/tuning/LocalizationTest.java | 2 + 4 files changed, 67 insertions(+), 30 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index d85a3154eb..51a614062b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -64,7 +64,7 @@ /** * ENIGMA Autonomous Example for only vision detection using openCv and park */ -@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "Mutation") +@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "Evolution") public class EnigmaAuto extends LinearOpMode { public static String TEAM_NAME = "ENIGMA"; //TODO: Enter team Name @@ -90,13 +90,19 @@ public class EnigmaAuto extends LinearOpMode { double LiftLeftOffset = .04; double LiftHeight; - private static final double ELBOW_DRIVE= Mutation.ELBOW_DRIVE; - private static final double ELBOW_INTAKE = Mutation.ELBOW_INTAKE; - private static final double WRIST_INTAKE = Mutation.WRIST_INTAKE; + private static final double ELBOW_DRIVE= Evolution.ELBOW_DRIVE; + private static final double ELBOW_INTAKE = Evolution.ELBOW_INTAKE; + private static final double WRIST_INTAKE = Evolution.WRIST_INTAKE; //private static final double SHOULDER_DRIVE = 0.425; // 0.425 - private static final double SCORE_ONE_SHOULDER = Mutation.SCORE_ONE_SHOULDER; - private static final double SCORE_ONE_WRIST = Mutation.SCORE_ONE_WRIST; - private static final double SCORE_ONE_ELBOW = Mutation.SCORE_ONE_ELBOW; + private static final double SCORE_ONE_SHOULDER = Evolution.SCORE_ONE_SHOULDER; + private static final double SCORE_ONE_WRIST = Evolution.SCORE_ONE_WRIST; + private static final double SCORE_ONE_ELBOW = Evolution.SCORE_ONE_ELBOW; + + public static final double SHOULDER_TOP_TWO = Evolution.SHOULDER_TOP_TWO; + public static final double WRIST_TOP_TWO = Evolution.WRIST_TOP_TWO; + public static final double ELBOW_TOP_TWO = Evolution.ELBOW_TOP_TWO; + + private static final double PIXEL_STACK_FINGER_GRAB = 0.6; //Define and declare Robot Starting Locations @@ -173,13 +179,13 @@ public void runOpMode() throws InterruptedException { //init pos - setLiftPosition(Mutation.LIFT_DRIVE); - shoulder.setPosition(Mutation.SHOULDER_DRIVE); - wrist.setPosition(Mutation.WRIST_INTAKE); - elbow.setPosition(Mutation.ELBOW_DRIVE); + setLiftPosition(Evolution.LIFT_DRIVE); + shoulder.setPosition(Evolution.SHOULDER_DRIVE); + wrist.setPosition(Evolution.WRIST_INTAKE); + elbow.setPosition(Evolution.ELBOW_DRIVE); sleep(2500); - leftFinger.setPosition(Mutation.LEFT_FINGER_GRIP); - rightFinger.setPosition(Mutation.RIGHT_FINGER_GRIP); + leftFinger.setPosition(Evolution.LEFT_FINGER_GRIP); + rightFinger.setPosition(Evolution.RIGHT_FINGER_GRIP); // Vision OpenCV / Color Detection WebcamName webcamName = hardwareMap.get(WebcamName.class, "Webcam 1"); @@ -256,6 +262,7 @@ public void runAutonoumousMode() { Pose2d midwayPose1 = new Pose2d(0,0,0); Pose2d midwayPose1a = new Pose2d(0,0,0); Pose2d intakeStack = new Pose2d(0,0,0); + Pose2d intakeStack2 = new Pose2d(0,0,0); Pose2d midwayPose2 = new Pose2d(0,0,0); Pose2d dropYellowPixelPose = new Pose2d(0, 0, 0); Pose2d parkPose = new Pose2d(0,0, 0); @@ -287,7 +294,8 @@ public void runAutonoumousMode() { break; } midwayPose1 = new Pose2d(14, 13, Math.toRadians(-45)); - waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + waitSecondsBeforeDrop = 0; //TODO: Adjust time to wait for alliance partner to move from board + //used to be 2 ^ this goes for all of them parkPose = new Pose2d(8, 30, Math.toRadians(-90)); break; @@ -311,7 +319,7 @@ public void runAutonoumousMode() { break; } midwayPose1 = new Pose2d(14, -13, Math.toRadians(45)); - waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + waitSecondsBeforeDrop = 0; //TODO: Adjust time to wait for alliance partner to move from board parkPose = new Pose2d(8, -30, Math.toRadians(90)); break; @@ -336,9 +344,10 @@ public void runAutonoumousMode() { } midwayPose1 = new Pose2d(8, -8, Math.toRadians(0)); midwayPose1a = new Pose2d(18, -21, Math.toRadians(-90)); - intakeStack = new Pose2d(52, -19,Math.toRadians(-90)); + intakeStack = new Pose2d(53.25, -15,Math.toRadians(-90)); + intakeStack2 = new Pose2d(53.25, -19.25,Math.toRadians(-90)); midwayPose2 = new Pose2d(52, 62, Math.toRadians(-90)); - waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + waitSecondsBeforeDrop = 0; //TODO: Adjust time to wait for alliance partner to move from board parkPose = new Pose2d(50, 84, Math.toRadians(-90)); break; @@ -365,7 +374,7 @@ public void runAutonoumousMode() { midwayPose1a = new Pose2d(18, 21, Math.toRadians(90)); intakeStack = new Pose2d(52, 19,Math.toRadians(90)); midwayPose2 = new Pose2d(52, -62, Math.toRadians(90)); - waitSecondsBeforeDrop = 2; //TODO: Adjust time to wait for alliance partner to move from board + waitSecondsBeforeDrop = 0; //TODO: Adjust time to wait for alliance partner to move from board parkPose = new Pose2d(50, -84, Math.toRadians(90)); break; } @@ -380,18 +389,18 @@ public void runAutonoumousMode() { .build()); //TODO : Code to drop Purple Pixel on Spike Mark - safeWaitSeconds(1); - shoulder.setPosition(Mutation.SHOULDER_DRIVE); - wrist.setPosition(Mutation.WRIST_INTAKE); + safeWaitSeconds(0); + shoulder.setPosition(Evolution.SHOULDER_DRIVE); + wrist.setPosition(Evolution.WRIST_INTAKE); for(int c = 0; c<40; c++) { moveServoGradually(elbow, ELBOW_INTAKE); sleep(10); } sleep(200); - rightFinger.setPosition(Mutation.RIGHT_FINGER_DROP); + rightFinger.setPosition(Evolution.RIGHT_FINGER_DROP); sleep(200); elbow.setPosition(ELBOW_DRIVE); - wrist.setPosition(Mutation.WRIST_TUCK); + wrist.setPosition(Evolution.WRIST_TUCK); //Move robot to midwayPose1 Actions.runBlocking( @@ -410,6 +419,31 @@ public void runAutonoumousMode() { //TODO : Code to intake pixel from stack safeWaitSeconds(1); + shoulder.setPosition(SHOULDER_TOP_TWO); + wrist.setPosition(WRIST_TOP_TWO); + elbow.setPosition(ELBOW_TOP_TWO); + for(int e = 0; e<40; e++) { + moveServoGradually(elbow, ELBOW_TOP_TWO); + sleep(10); + } + for(int w = 0; w<40; w++) { + moveServoGradually(wrist, WRIST_TOP_TWO); + sleep(10); + } + for(int s = 0; s<40; s++) { + moveServoGradually(shoulder, SHOULDER_TOP_TWO); + sleep(10); + } + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(intakeStack2.position, intakeStack2.heading) + .build()); + sleep(300); + rightFinger.setPosition(PIXEL_STACK_FINGER_GRAB); + sleep(500); + elbow.setPosition(ELBOW_DRIVE); + wrist.setPosition(Evolution.WRIST_TUCK); + sleep(500); //Move robot to midwayPose2 and to dropYellowPixelPose Actions.runBlocking( @@ -429,7 +463,7 @@ public void runAutonoumousMode() { //TODO : Code to drop Pixel on Backdrop - safeWaitSeconds(1); + safeWaitSeconds(0); for(int w = 0; w<40; w++) { moveServoGradually(wrist, SCORE_ONE_WRIST); sleep(10); @@ -442,11 +476,11 @@ public void runAutonoumousMode() { moveServoGradually(shoulder, SCORE_ONE_SHOULDER); sleep(7); } - sleep(200); - leftFinger.setPosition(Mutation.LEFT_FINGER_DROP); - sleep(100); + sleep(50); + leftFinger.setPosition(Evolution.LEFT_FINGER_DROP); + sleep(50); for(int s = 0; s<200; s++) { - moveServoGradually(shoulder, Mutation.SHOULDER_DRIVE); + moveServoGradually(shoulder, Evolution.SHOULDER_DRIVE); sleep(7); } for(int w = 0; w<40; w++) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto2QualBackup.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto2QualBackup.java index 5feef7e0df..ad11c8065a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto2QualBackup.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto2QualBackup.java @@ -55,7 +55,7 @@ /** * ENIGMA Autonomous Example for only vision detection using openCv and park */ -@Autonomous(name = "ENIGMA Autonomous Mode", group = "00-Autonomous", preselectTeleOp = "Mutation") +@Autonomous(name = "ENIGMA Autonomous Mode - Backup", group = "00-Autonomous", preselectTeleOp = "Mutation") public class EnigmaAuto2QualBackup extends LinearOpMode { public static String TEAM_NAME = "ENIGMA"; //TODO: Enter team Name diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java index 422ecbc485..bc3d99476f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java @@ -2,12 +2,13 @@ import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; -//@TeleOp +@TeleOp public class Evolution extends LinearOpMode { // Declare vars diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java index a5b5bbc5f9..22b8eba7ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java @@ -5,10 +5,12 @@ import com.acmerobotics.roadrunner.Twist2d; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.MecanumDrive; import org.firstinspires.ftc.teamcode.TankDrive; +@TeleOp public class LocalizationTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { From 5ca01b2e0c6f6fa88e6429e874eccba051f7f79b Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 30 Dec 2023 15:19:10 -0700 Subject: [PATCH 139/154] 2+1 working --- .../ftc/teamcode/EnigmaAuto.java | 104 ++++++++++++++---- 1 file changed, 82 insertions(+), 22 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 51a614062b..f04e0744ce 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -104,6 +104,10 @@ public class EnigmaAuto extends LinearOpMode { private static final double PIXEL_STACK_FINGER_GRAB = 0.6; + private static final double SHOULDER_TOP_ONE = 0.425; // SHOULDER_TOP_TWO; //0.4455 + private static final double WRIST_TOP_ONE = 0.575; // WRIST_TOP_TWO; //0.585 + private static final double ELBOW_TOP_ONE = 0.74; // ELBOW_TOP_TWO; //0.73 + //Define and declare Robot Starting Locations public enum START_POSITION{ @@ -272,7 +276,7 @@ public void runAutonoumousMode() { initPose = new Pose2d(0, 0, Math.toRadians(0)); //Starting pose moveBeyondTrussPose = new Pose2d(15,0,0); - //TODO: edit crap here + //TODO: edit here switch (startPosition) { case BLUE_LEFT: drive = new MecanumDrive(hardwareMap, initPose); @@ -390,13 +394,21 @@ public void runAutonoumousMode() { //TODO : Code to drop Purple Pixel on Spike Mark safeWaitSeconds(0); - shoulder.setPosition(Evolution.SHOULDER_DRIVE); - wrist.setPosition(Evolution.WRIST_INTAKE); + for(int c = 0; c<40; c++) { - moveServoGradually(elbow, ELBOW_INTAKE); - sleep(10); + moveServoGradually(shoulder, Evolution.SHOULDER_DRIVE); + sleep(15); } - sleep(200); + for(int c = 0; c<40; c++) { + moveServoGradually(wrist, Evolution.WRIST_INTAKE); + sleep(15); + } + for(int c = 0; c<40; c++) { + moveServoGradually(elbow, Evolution.ELBOW_INTAKE); + sleep(15); + } + + //sleep(200); rightFinger.setPosition(Evolution.RIGHT_FINGER_DROP); sleep(200); elbow.setPosition(ELBOW_DRIVE); @@ -418,32 +430,31 @@ public void runAutonoumousMode() { .build()); //TODO : Code to intake pixel from stack - safeWaitSeconds(1); - shoulder.setPosition(SHOULDER_TOP_TWO); - wrist.setPosition(WRIST_TOP_TWO); - elbow.setPosition(ELBOW_TOP_TWO); - for(int e = 0; e<40; e++) { - moveServoGradually(elbow, ELBOW_TOP_TWO); - sleep(10); + //safeWaitSeconds(1); + + rightFinger.setPosition(PIXEL_STACK_FINGER_GRAB); + for(int e = 0; e<50; e++) { + moveServoGradually(elbow, ELBOW_TOP_ONE); + sleep(15); } - for(int w = 0; w<40; w++) { - moveServoGradually(wrist, WRIST_TOP_TWO); - sleep(10); + for(int w = 0; w<50; w++) { + moveServoGradually(wrist, WRIST_TOP_ONE); + sleep(15); } - for(int s = 0; s<40; s++) { - moveServoGradually(shoulder, SHOULDER_TOP_TWO); - sleep(10); + for(int s = 0; s<50; s++) { + moveServoGradually(shoulder, SHOULDER_TOP_ONE); + sleep(15); } Actions.runBlocking( drive.actionBuilder(drive.pose) .strafeToLinearHeading(intakeStack2.position, intakeStack2.heading) .build()); sleep(300); - rightFinger.setPosition(PIXEL_STACK_FINGER_GRAB); - sleep(500); + rightFinger.setPosition(Evolution.RIGHT_FINGER_GRIP); + sleep(650); elbow.setPosition(ELBOW_DRIVE); wrist.setPosition(Evolution.WRIST_TUCK); - sleep(500); + sleep(100); //Move robot to midwayPose2 and to dropYellowPixelPose Actions.runBlocking( @@ -478,6 +489,8 @@ public void runAutonoumousMode() { } sleep(50); leftFinger.setPosition(Evolution.LEFT_FINGER_DROP); + sleep(400); + rightFinger.setPosition(Evolution.RIGHT_FINGER_DROP); sleep(50); for(int s = 0; s<200; s++) { moveServoGradually(shoulder, Evolution.SHOULDER_DRIVE); @@ -500,6 +513,53 @@ public void runAutonoumousMode() { .strafeToLinearHeading(parkPose.position, parkPose.heading) //.splineToLinearHeading(parkPose,0) .build()); + + // just copy paste it + + //For Blue Right and Red Left, intake pixel from stack + if (startPosition == START_POSITION.BLUE_RIGHT || + startPosition == START_POSITION.RED_LEFT) { + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(midwayPose1a.position, midwayPose1a.heading) + .strafeToLinearHeading(intakeStack.position, intakeStack.heading) + .build()); + + //TODO : Code to intake pixel from stack + //safeWaitSeconds(1); + + rightFinger.setPosition(PIXEL_STACK_FINGER_GRAB); + for(int e = 0; e<50; e++) { + moveServoGradually(elbow, ELBOW_TOP_ONE); + sleep(15); + } + for(int w = 0; w<50; w++) { + moveServoGradually(wrist, WRIST_TOP_ONE); + sleep(15); + } + for(int s = 0; s<50; s++) { + moveServoGradually(shoulder, SHOULDER_TOP_ONE); + sleep(15); + } + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(intakeStack2.position, intakeStack2.heading) + .build()); + sleep(300); + rightFinger.setPosition(Evolution.RIGHT_FINGER_GRIP); + sleep(650); + elbow.setPosition(ELBOW_DRIVE); + wrist.setPosition(Evolution.WRIST_TUCK); + sleep(100); + + //Move robot to midwayPose2 and to dropYellowPixelPose + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(midwayPose2.position, midwayPose2.heading) + .build()); + } + + } From abce31d10ba38c895b0d2c43a37ed61b2d6c3ba9 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 30 Dec 2023 15:40:14 -0700 Subject: [PATCH 140/154] Ending the day with a reliable 1+1 --- .../ftc/teamcode/EnigmaAuto.java | 46 ++++--------------- 1 file changed, 8 insertions(+), 38 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index f04e0744ce..99ede24d5d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -506,60 +506,30 @@ public void runAutonoumousMode() { } - +/* the original ending //Move robot to park in Backstage Actions.runBlocking( drive.actionBuilder(drive.pose) .strafeToLinearHeading(parkPose.position, parkPose.heading) //.splineToLinearHeading(parkPose,0) .build()); +*/ - // just copy paste it + for(int w = 0; w<40; w++) { + moveServoGradually(wrist, Evolution.WRIST_DRIVE); + sleep(10); + } - //For Blue Right and Red Left, intake pixel from stack + // GRAB MORE?! For Blue Right and Red Left, intake pixel from stack if (startPosition == START_POSITION.BLUE_RIGHT || startPosition == START_POSITION.RED_LEFT) { - Actions.runBlocking( - drive.actionBuilder(drive.pose) - .strafeToLinearHeading(midwayPose1a.position, midwayPose1a.heading) - .strafeToLinearHeading(intakeStack.position, intakeStack.heading) - .build()); - - //TODO : Code to intake pixel from stack - //safeWaitSeconds(1); - - rightFinger.setPosition(PIXEL_STACK_FINGER_GRAB); - for(int e = 0; e<50; e++) { - moveServoGradually(elbow, ELBOW_TOP_ONE); - sleep(15); - } - for(int w = 0; w<50; w++) { - moveServoGradually(wrist, WRIST_TOP_ONE); - sleep(15); - } - for(int s = 0; s<50; s++) { - moveServoGradually(shoulder, SHOULDER_TOP_ONE); - sleep(15); - } - Actions.runBlocking( - drive.actionBuilder(drive.pose) - .strafeToLinearHeading(intakeStack2.position, intakeStack2.heading) - .build()); - sleep(300); - rightFinger.setPosition(Evolution.RIGHT_FINGER_GRIP); - sleep(650); - elbow.setPosition(ELBOW_DRIVE); - wrist.setPosition(Evolution.WRIST_TUCK); - sleep(100); - - //Move robot to midwayPose2 and to dropYellowPixelPose Actions.runBlocking( drive.actionBuilder(drive.pose) .strafeToLinearHeading(midwayPose2.position, midwayPose2.heading) + .strafeToLinearHeading(intakeStack.position, intakeStack.heading) .build()); } - } From 382a094c5d377b5d194113a5d49b7bf1977cb23f Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 3 Jan 2024 17:01:44 -0700 Subject: [PATCH 141/154] Jett mostly got the code working (He can play fortnite now) --- .../ftc/teamcode/DistanceSensTest.java | 50 +++++++++++++++++++ .../ftc/teamcode/WorkingTeleOpFramework.java | 25 ++++++++++ 2 files changed, 75 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WorkingTeleOpFramework.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java new file mode 100644 index 0000000000..72de9ab1c5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import com.qualcomm.robotcore.hardware.DistanceSensor; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +@TeleOp +public class DistanceSensTest extends LinearOpMode { + + public static final double LEFT_FINGER_GRIP = 0.72; + public static final double RIGHT_FINGER_GRIP = .27; + Servo xyz; + double DistanceDetection = 10; + private Servo leftFinger; + private Servo rightFinger; + private DistanceSensor distanceL; + private DistanceSensor distanceR; + private void function() { + } + @Override + public void runOpMode() throws InterruptedException { + + distanceL = hardwareMap.get(DistanceSensor.class, "DistanceL"); + distanceR = hardwareMap.get(DistanceSensor.class, "DistanceR"); + leftFinger = hardwareMap.get(Servo.class, "leftFinger"); + rightFinger = hardwareMap.get(Servo.class, "rightFinger"); + + + + + + telemetry.update(); + waitForStart(); + + while(opModeIsActive()) { + if (distanceL.getDistance(DistanceUnit.CM) < DistanceDetection) { + leftFinger.setPosition(LEFT_FINGER_GRIP); + } + + if (distanceR.getDistance(DistanceUnit.CM) < DistanceDetection) { + rightFinger.setPosition(RIGHT_FINGER_GRIP); + } + + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WorkingTeleOpFramework.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WorkingTeleOpFramework.java new file mode 100644 index 0000000000..3d76024a20 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WorkingTeleOpFramework.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Servo; + +//@TeleOp +public class WorkingTeleOpFramework extends LinearOpMode { + Servo xyz; + + private void function() { + } + @Override + public void runOpMode() throws InterruptedException { + + xyz = hardwareMap.get(Servo.class, "xyz"); + + telemetry.update(); + waitForStart(); + + while(opModeIsActive()) { + + telemetry.update(); + } + } +} From 9e97a87cdea658fe955df5c77466e6b2ab85777b Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 3 Jan 2024 17:05:32 -0700 Subject: [PATCH 142/154] I cleaned up Jetts code :) --- .../ftc/teamcode/DistanceSensTest.java | 22 +++++-------------- 1 file changed, 6 insertions(+), 16 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java index 72de9ab1c5..20183f6e87 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java @@ -9,39 +9,29 @@ @TeleOp public class DistanceSensTest extends LinearOpMode { - - public static final double LEFT_FINGER_GRIP = 0.72; - public static final double RIGHT_FINGER_GRIP = .27; - Servo xyz; double DistanceDetection = 10; private Servo leftFinger; private Servo rightFinger; private DistanceSensor distanceL; private DistanceSensor distanceR; - private void function() { - } @Override public void runOpMode() throws InterruptedException { - + //Hardware Mapping ↓ distanceL = hardwareMap.get(DistanceSensor.class, "DistanceL"); distanceR = hardwareMap.get(DistanceSensor.class, "DistanceR"); - leftFinger = hardwareMap.get(Servo.class, "leftFinger"); - rightFinger = hardwareMap.get(Servo.class, "rightFinger"); - - - - + leftFinger = hardwareMap.get(Servo.class, "lFinger"); + rightFinger = hardwareMap.get(Servo.class, "rFinger"); telemetry.update(); waitForStart(); - + //CODE ↓ while(opModeIsActive()) { if (distanceL.getDistance(DistanceUnit.CM) < DistanceDetection) { - leftFinger.setPosition(LEFT_FINGER_GRIP); + leftFinger.setPosition(Evolution.LEFT_FINGER_GRIP); } if (distanceR.getDistance(DistanceUnit.CM) < DistanceDetection) { - rightFinger.setPosition(RIGHT_FINGER_GRIP); + rightFinger.setPosition(Evolution.RIGHT_FINGER_GRIP); } telemetry.update(); From e7fdafbdd47dc6fb0120a2d35ea33464739abfb6 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 3 Jan 2024 18:17:48 -0700 Subject: [PATCH 143/154] did sopm,e things --- .../java/org/firstinspires/ftc/teamcode/DistanceSensTest.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java index 20183f6e87..10dd14d896 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java @@ -17,8 +17,8 @@ public class DistanceSensTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { //Hardware Mapping ↓ - distanceL = hardwareMap.get(DistanceSensor.class, "DistanceL"); - distanceR = hardwareMap.get(DistanceSensor.class, "DistanceR"); + distanceL = hardwareMap.get(DistanceSensor.class, "ds1"); + distanceR = hardwareMap.get(DistanceSensor.class, "ds2"); leftFinger = hardwareMap.get(Servo.class, "lFinger"); rightFinger = hardwareMap.get(Servo.class, "rFinger"); From 23e26b85fb16fa99d5411070b6a82819df0f0aa4 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 3 Jan 2024 18:23:43 -0700 Subject: [PATCH 144/154] goodnight (a little ding ding ding) --- .../java/org/firstinspires/ftc/teamcode/DistanceSensTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java index 10dd14d896..ccbd88ae14 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java @@ -9,7 +9,7 @@ @TeleOp public class DistanceSensTest extends LinearOpMode { - double DistanceDetection = 10; + double DistanceDetection = 1; private Servo leftFinger; private Servo rightFinger; private DistanceSensor distanceL; From 82560fb5bf0e344d36df39dc6332e19dd095d679 Mon Sep 17 00:00:00 2001 From: Gabriel Date: Sat, 6 Jan 2024 15:18:03 -0700 Subject: [PATCH 145/154] start of bonus auto pixels --- .../ftc/teamcode/EnigmaAuto.java | 41 ++++++++++--------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 99ede24d5d..0b23fd46b0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -506,30 +506,31 @@ public void runAutonoumousMode() { } -/* the original ending - //Move robot to park in Backstage - Actions.runBlocking( - drive.actionBuilder(drive.pose) - .strafeToLinearHeading(parkPose.position, parkPose.heading) - //.splineToLinearHeading(parkPose,0) - .build()); -*/ - - for(int w = 0; w<40; w++) { - moveServoGradually(wrist, Evolution.WRIST_DRIVE); - sleep(10); - } - - // GRAB MORE?! For Blue Right and Red Left, intake pixel from stack - if (startPosition == START_POSITION.BLUE_RIGHT || - startPosition == START_POSITION.RED_LEFT) { + boolean go_for_bonus_white_pixels = true; + if(go_for_bonus_white_pixels == false) { + //Move robot to park in Backstage Actions.runBlocking( drive.actionBuilder(drive.pose) - .strafeToLinearHeading(midwayPose2.position, midwayPose2.heading) - .strafeToLinearHeading(intakeStack.position, intakeStack.heading) + .strafeToLinearHeading(parkPose.position, parkPose.heading) + //.splineToLinearHeading(parkPose,0) .build()); - } + } else { + // GRAB MORE?! For Blue Right and Red Left, intake pixel from stack + for (int w = 0; w < 40; w++) { + moveServoGradually(wrist, Evolution.WRIST_DRIVE); + sleep(10); + } + + if (startPosition == START_POSITION.BLUE_RIGHT || + startPosition == START_POSITION.RED_LEFT) { + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(midwayPose2.position, midwayPose2.heading) + .strafeToLinearHeading(intakeStack.position, intakeStack.heading) + .build()); + } + } } From 78f307cdc801b171302fae3afc2105369e35b296 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 6 Jan 2024 16:11:26 -0700 Subject: [PATCH 146/154] Logic troubles Intake & Drive Pos work --- .../ftc/teamcode/DistanceSensTest.java | 6 +- .../ftc/teamcode/EnigmaAuto.java | 4 +- .../firstinspires/ftc/teamcode/Evolution.java | 64 ++++++++++++++++--- 3 files changed, 59 insertions(+), 15 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java index ccbd88ae14..8de481890c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DistanceSensTest.java @@ -9,7 +9,7 @@ @TeleOp public class DistanceSensTest extends LinearOpMode { - double DistanceDetection = 1; + public double DistanceDetection = 4; private Servo leftFinger; private Servo rightFinger; private DistanceSensor distanceL; @@ -17,8 +17,8 @@ public class DistanceSensTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { //Hardware Mapping ↓ - distanceL = hardwareMap.get(DistanceSensor.class, "ds1"); - distanceR = hardwareMap.get(DistanceSensor.class, "ds2"); + distanceL = hardwareMap.get(DistanceSensor.class, "dsL"); + distanceR = hardwareMap.get(DistanceSensor.class, "dsR"); leftFinger = hardwareMap.get(Servo.class, "lFinger"); rightFinger = hardwareMap.get(Servo.class, "rFinger"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 0b23fd46b0..01a55fa6fd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -412,7 +412,7 @@ public void runAutonoumousMode() { rightFinger.setPosition(Evolution.RIGHT_FINGER_DROP); sleep(200); elbow.setPosition(ELBOW_DRIVE); - wrist.setPosition(Evolution.WRIST_TUCK); + wrist.setPosition(Evolution.WRIST_DRIVE); //Move robot to midwayPose1 Actions.runBlocking( @@ -453,7 +453,7 @@ public void runAutonoumousMode() { rightFinger.setPosition(Evolution.RIGHT_FINGER_GRIP); sleep(650); elbow.setPosition(ELBOW_DRIVE); - wrist.setPosition(Evolution.WRIST_TUCK); + wrist.setPosition(Evolution.WRIST_DRIVE); sleep(100); //Move robot to midwayPose2 and to dropYellowPixelPose diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java index bc3d99476f..435c4f32a6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java @@ -4,10 +4,13 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DistanceSensor; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + @TeleOp public class Evolution extends LinearOpMode { @@ -33,10 +36,17 @@ public class Evolution extends LinearOpMode { private Servo rightFinger; private Servo rightLift; private Servo leftLift; + private DistanceSensor distanceL; + private DistanceSensor distanceR; private double servoposition = 0.0; private double servodelta = 0.02; private double servodelaytime = 0.03; + private double DistanceDetection = 4; + private boolean tuckPreparing = false; + + ElapsedTime tuckTimer = new ElapsedTime(); + private static final double LOW_ACC = 7.25; private static final double LOW_VEL = 8.5; private static final double MED_ACC = 10.25; @@ -53,21 +63,20 @@ public class Evolution extends LinearOpMode { public static final double AIR_PLANE_SHOOT = 0.8; public static final double SHOULDER_DRIVE = 0.425; // 0.425 public static final double SHOULDER_INT = 0.43; - public static final double ELBOW_DRIVE= 0.47; - public static final double ELBOW_INTAKE = 0.78; - public static final double WRIST_TUCK = 0.29; - public static final double WRIST_DRIVE = 0.82; - public static final double WRIST_INTAKE = 0.555; + public static final double ELBOW_DRIVE= 0.53; + public static final double ELBOW_INTAKE = 0.83; + public static final double WRIST_DRIVE = 0.195; + public static final double WRIST_INTAKE = 0.465; public static final double LEFT_FINGER_GRIP = 0.72; public static final double LEFT_FINGER_DROP = 0.5; - public static final double LEFT_FINGER_INTAKE = 0.3; + public static final double LEFT_FINGER_INTAKE = 1; public static final double RIGHT_FINGER_GRIP = .27; public static final double RIGHT_FINGER_DROP = .5; - public static final double RIGHT_FINGER_INTAKE = 0.64; + public static final double RIGHT_FINGER_INTAKE = 0; public static final double TRIGGER_THRESHOLD = 0.5; public static final double LAUNCHER_START_POS = 0.8; public static final double SERVO_TOLERANCE = 0.01; - public static final double LIFT_DRIVE = 0.10; // 0.78 is the highest it can mechanically go right now + public static final double LIFT_DRIVE = 0.06; // 0.78 is the highest it can mechanically go right now private double liftTargetPosition = LIFT_DRIVE; // was 0.37 before moving servos for larger range // stack positions (top 2 o 5 and next 2 of 3 ) @@ -141,6 +150,8 @@ public class Evolution extends LinearOpMode { private static final double SCORE_ELEVEN_WRIST = SCORE_ONE_WRIST; private static final double SCORE_ELEVEN_ELBOW = SCORE_ONE_ELBOW; private static final double SCORE_ELEVEN_LIFT = .66; + + private boolean inIntakePos = false; // sensors private RevTouchSensor rightUpper; @@ -219,6 +230,7 @@ private void handleIntakeSequence(Evolution.IntakePosition intakePos) { } break; case COMPLETED: + inIntakePos = true; // Sequence complete, reset the state or perform additional actions break; } @@ -287,6 +299,32 @@ public DrivePosition(double liftPos, double shoulderPos, double wristPos, double this.velocityMax = velMax; } } + + private void autoClose() { + if (distanceL.getDistance(DistanceUnit.CM) < DistanceDetection) { + leftFinger.setPosition(Evolution.LEFT_FINGER_GRIP); + } + + if (distanceR.getDistance(DistanceUnit.CM) < DistanceDetection) { + rightFinger.setPosition(Evolution.RIGHT_FINGER_GRIP); + } + } + + private void autoTuck() { + if (distanceL.getDistance(DistanceUnit.CM) < DistanceDetection && distanceR.getDistance(DistanceUnit.CM) < DistanceDetection && inIntakePos == true && tuckPreparing == false) { + tuckPreparing = true; + tuckTimer.reset(); + } + + if (tuckTimer.seconds() > 1 && tuckPreparing == true) { + activeDrivePosition = new Evolution.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_DRIVE, ELBOW_DRIVE, SUPER_ACC, SUPER_VEL); + currentDriveState = Evolution.driveState.MOVING_LIFT; + inIntakePos = false; + tuckPreparing = false; + tuckTimer.reset(); + } + } + private void handleDriveSequence(Evolution.DrivePosition drivePos) { switch (currentDriveState) { case MOVING_LIFT: @@ -317,6 +355,7 @@ private void handleDriveSequence(Evolution.DrivePosition drivePos) { } break; case COMPLETED: + inIntakePos = false; // Sequence complete, reset the state or perform additional actions break; } @@ -329,7 +368,7 @@ private void handleDriveSequence(Evolution.DrivePosition drivePos) { private void drivingFunction() { // Check if the right bumper is pressed and the drive state is IDLE if (gamepad1.right_bumper && currentDriveState == Evolution.driveState.IDLE) { - activeDrivePosition = new Evolution.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_TUCK, ELBOW_DRIVE, SUPER_ACC, SUPER_VEL); + activeDrivePosition = new Evolution.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_DRIVE, ELBOW_DRIVE, SUPER_ACC, SUPER_VEL); currentDriveState = Evolution.driveState.MOVING_LIFT; } @@ -406,6 +445,7 @@ private void handleScoreSequence(Evolution.ScorePosition scorePos) { } break; case COMPLETED: + inIntakePos = false; // Sequence complete, reset the state or perform additional actions break; } @@ -749,6 +789,8 @@ public void runOpMode() throws InterruptedException { rightUpper = hardwareMap.get(RevTouchSensor.class, "rightUpper"); leftLower = hardwareMap.get(RevTouchSensor.class, "leftLower"); rightLower = hardwareMap.get(RevTouchSensor.class, "rightLower"); + distanceL = hardwareMap.get(DistanceSensor.class, "dsL"); + distanceR = hardwareMap.get(DistanceSensor.class, "dsR"); // init positions leftFinger.setPosition(LEFT_FINGER_GRIP); @@ -756,7 +798,7 @@ public void runOpMode() throws InterruptedException { setLiftPosition(LIFT_DRIVE); shoulder.setPosition(SHOULDER_INT); elbow.setPosition(ELBOW_DRIVE); - wrist.setPosition(WRIST_TUCK); + wrist.setPosition(WRIST_DRIVE); launcherstartPos(); telemetry.addData("Status", "OdoMec is ready to run!"); @@ -816,6 +858,8 @@ public void runOpMode() throws InterruptedException { scoringFunction(); // emergency stop slides emergencyStop(); + autoClose(); + autoTuck(); // mecanum drive //driveCode(); say what telemetry.update(); From a47f834abf2fc765336434b43ddd4422c953fdc5 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 6 Jan 2024 17:03:38 -0700 Subject: [PATCH 147/154] Goin to auto --- .../firstinspires/ftc/teamcode/Evolution.java | 79 ++++++++++--------- 1 file changed, 42 insertions(+), 37 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java index 435c4f32a6..5f2a767ed1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java @@ -96,45 +96,45 @@ public class Evolution extends LinearOpMode { // score positions (11 rows on the board) // TODO find positions with McMuffin (currently all set to drive) change to the actual double values like above from McMuffin // score position one button map (gamepad2.y) - public static final double SCORE_ONE_SHOULDER = 0.915; - public static final double SCORE_ONE_WRIST = 0.365; - public static final double SCORE_ONE_ELBOW = 0.57; + public static final double SCORE_ONE_SHOULDER = 0.935; + public static final double SCORE_ONE_WRIST = 0.385; + public static final double SCORE_ONE_ELBOW = 0.5; public static final double SCORE_ONE_LIFT = LIFT_DRIVE; // score position two button map (gamepad2.b) - public static final double SCORE_TWO_SHOULDER = SCORE_ONE_SHOULDER; - public static final double SCORE_TWO_WRIST = SCORE_ONE_WRIST; - public static final double SCORE_TWO_ELBOW = SCORE_ONE_ELBOW; - public static final double SCORE_TWO_LIFT = 0.26; + public static final double SCORE_TWO_SHOULDER = 0.935; + public static final double SCORE_TWO_WRIST = 0.385; + public static final double SCORE_TWO_ELBOW = 0.5; + public static final double SCORE_TWO_LIFT = 0.27; // score position three button map (gamepad2.a) - public static final double SCORE_THREE_SHOULDER = SCORE_ONE_SHOULDER; - public static final double SCORE_THREE_WRIST = SCORE_ONE_WRIST; - public static final double SCORE_THREE_ELBOW = SCORE_ONE_ELBOW; - public static final double SCORE_THREE_LIFT = 0.37; + public static final double SCORE_THREE_SHOULDER = 0.935; + public static final double SCORE_THREE_WRIST = 0.385; + public static final double SCORE_THREE_ELBOW = 0.5; + public static final double SCORE_THREE_LIFT = 0.39; // score position four button map (gamepad2.x) - public static final double SCORE_FOUR_SHOULDER = SCORE_ONE_SHOULDER; - public static final double SCORE_FOUR_WRIST = SCORE_ONE_WRIST; - public static final double SCORE_FOUR_ELBOW = SCORE_ONE_ELBOW; - public static final double SCORE_FOUR_LIFT = .44; + public static final double SCORE_FOUR_SHOULDER = 0.936; + public static final double SCORE_FOUR_WRIST = 0.385; + public static final double SCORE_FOUR_ELBOW = 0.5; + public static final double SCORE_FOUR_LIFT = 0.48; // score position five button map (gamepad2.left_bumper && gamepad2.y) - public static final double SCORE_FIVE_SHOULDER = SCORE_ONE_SHOULDER; - public static final double SCORE_FIVE_WRIST = SCORE_ONE_WRIST; - public static final double SCORE_FIVE_ELBOW = SCORE_ONE_ELBOW; + public static final double SCORE_FIVE_SHOULDER = 0.935; + public static final double SCORE_FIVE_WRIST = 0.385; + public static final double SCORE_FIVE_ELBOW = 0.5; public static final double SCORE_FIVE_LIFT = 0.55; // score position six button map (gamepad2.left_bumper && gamepad2.b) - private static final double SCORE_SIX_SHOULDER = SCORE_ONE_SHOULDER; - private static final double SCORE_SIX_WRIST = SCORE_ONE_WRIST; - private static final double SCORE_SIX_ELBOW = SCORE_ONE_ELBOW; - private static final double SCORE_SIX_LIFT = .66; + private static final double SCORE_SIX_SHOULDER = 0.895; + private static final double SCORE_SIX_WRIST = 0.225; + private static final double SCORE_SIX_ELBOW = 0.63; + private static final double SCORE_SIX_LIFT = .46; // score position seven button map (gamepad2.left_bumper && gamepad2.a) - private static final double SCORE_SEVEN_SHOULDER = SCORE_ONE_SHOULDER; - private static final double SCORE_SEVEN_WRIST = 0.305; - private static final double SCORE_SEVEN_ELBOW = 0.6; - private static final double SCORE_SEVEN_LIFT = .66; + private static final double SCORE_SEVEN_SHOULDER = 0.895; + private static final double SCORE_SEVEN_WRIST = 0.225; + private static final double SCORE_SEVEN_ELBOW = 0.63; + private static final double SCORE_SEVEN_LIFT = .57; // score position eight button map (gamepad2.left_bumper && gamepad2.x) - private static final double SCORE_EIGHT_SHOULDER = SCORE_ONE_SHOULDER; - private static final double SCORE_EIGHT_WRIST = 0.235; - private static final double SCORE_EIGHT_ELBOW = 0.67; - private static final double SCORE_EIGHT_LIFT = .66; + private static final double SCORE_EIGHT_SHOULDER = 0.895; + private static final double SCORE_EIGHT_WRIST = 0.16; + private static final double SCORE_EIGHT_ELBOW = 0.69; + private static final double SCORE_EIGHT_LIFT = 0.57; // score position nine button map (gamepad2.left_trigger && gamepad2.y) private static final double SCORE_NINE_SHOULDER = SCORE_ONE_SHOULDER; private static final double SCORE_NINE_WRIST = 0.174; @@ -151,6 +151,11 @@ public class Evolution extends LinearOpMode { private static final double SCORE_ELEVEN_ELBOW = SCORE_ONE_ELBOW; private static final double SCORE_ELEVEN_LIFT = .66; + public static final double SCORE_DOUBLE_SHOULDER = 0.916; + public static final double SCORE_DOUBLE_WRIST = 0.84; + public static final double SCORE_DOUBLE_ELBOW = 0.58; + public static final double SCORE_DOUBLE_LIFT = SCORE_ONE_LIFT; + private boolean inIntakePos = false; // sensors @@ -315,13 +320,13 @@ private void autoTuck() { tuckPreparing = true; tuckTimer.reset(); } - - if (tuckTimer.seconds() > 1 && tuckPreparing == true) { - activeDrivePosition = new Evolution.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_DRIVE, ELBOW_DRIVE, SUPER_ACC, SUPER_VEL); - currentDriveState = Evolution.driveState.MOVING_LIFT; - inIntakePos = false; + if (tuckTimer.seconds() > 1.0 && tuckPreparing == true) { + if (inIntakePos == true) { + activeDrivePosition = new Evolution.DrivePosition(LIFT_DRIVE, SHOULDER_DRIVE, WRIST_DRIVE, ELBOW_DRIVE, SUPER_ACC, SUPER_VEL); + currentDriveState = Evolution.driveState.MOVING_LIFT; + inIntakePos = false; + } tuckPreparing = false; - tuckTimer.reset(); } } @@ -859,7 +864,7 @@ public void runOpMode() throws InterruptedException { // emergency stop slides emergencyStop(); autoClose(); - autoTuck(); + //autoTuck(); // mecanum drive //driveCode(); say what telemetry.update(); From 0b0c1c5fef69c72c4d0cb0c570450b17e404a5df Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Sat, 6 Jan 2024 17:49:59 -0700 Subject: [PATCH 148/154] All tele op tuning done, need to fix auto tuck, need to tune top 2 --- .../ftc/teamcode/EnigmaAuto.java | 61 +++++++++++-------- .../firstinspires/ftc/teamcode/Evolution.java | 10 +-- 2 files changed, 40 insertions(+), 31 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 01a55fa6fd..2d2541fed3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -104,9 +104,9 @@ public class EnigmaAuto extends LinearOpMode { private static final double PIXEL_STACK_FINGER_GRAB = 0.6; - private static final double SHOULDER_TOP_ONE = 0.425; // SHOULDER_TOP_TWO; //0.4455 - private static final double WRIST_TOP_ONE = 0.575; // WRIST_TOP_TWO; //0.585 - private static final double ELBOW_TOP_ONE = 0.74; // ELBOW_TOP_TWO; //0.73 + private static final double SHOULDER_TOP_ONE = 0.31; // SHOULDER_TOP_TWO; //0.4455 + private static final double WRIST_TOP_ONE = 0.505; // WRIST_TOP_TWO; //0.585 + private static final double ELBOW_TOP_ONE = 0.77; // ELBOW_TOP_TWO; //0.73 //Define and declare Robot Starting Locations @@ -284,17 +284,17 @@ public void runAutonoumousMode() { case LEFT: dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); - dropYellowPixelPose = new Pose2d(19, 36, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(19, 38.5, Math.toRadians(-90)); break; case MIDDLE: dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(26, 36, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(26, 38.5, Math.toRadians(-90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); - dropYellowPixelPose = new Pose2d(31, 36, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(31, 38.5, Math.toRadians(-90)); break; } midwayPose1 = new Pose2d(14, 13, Math.toRadians(-45)); @@ -309,17 +309,17 @@ public void runAutonoumousMode() { case LEFT: dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); - dropYellowPixelPose = new Pose2d(34, -36, Math.toRadians(90)); + dropYellowPixelPose = new Pose2d(34, -38.5, Math.toRadians(90)); break; case MIDDLE: dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(27, -36, Math.toRadians(90)); + dropYellowPixelPose = new Pose2d(27, -38.5, Math.toRadians(90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); - dropYellowPixelPose = new Pose2d(20, -36, Math.toRadians(90)); + dropYellowPixelPose = new Pose2d(20, -38.5, Math.toRadians(90)); break; } midwayPose1 = new Pose2d(14, -13, Math.toRadians(45)); @@ -333,23 +333,23 @@ public void runAutonoumousMode() { case LEFT: dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); - dropYellowPixelPose = new Pose2d(19, 86, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(19, 87.5, Math.toRadians(-90)); break; case MIDDLE: dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(26.25, 86, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(26.25, 87.5, Math.toRadians(-90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); - dropYellowPixelPose = new Pose2d(33.5, 86, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(33.5, 87.5, Math.toRadians(-90)); break; } midwayPose1 = new Pose2d(8, -8, Math.toRadians(0)); midwayPose1a = new Pose2d(18, -21, Math.toRadians(-90)); intakeStack = new Pose2d(53.25, -15,Math.toRadians(-90)); - intakeStack2 = new Pose2d(53.25, -19.25,Math.toRadians(-90)); + intakeStack2 = new Pose2d(53.25, -17,Math.toRadians(-90)); midwayPose2 = new Pose2d(52, 62, Math.toRadians(-90)); waitSecondsBeforeDrop = 0; //TODO: Adjust time to wait for alliance partner to move from board parkPose = new Pose2d(50, 84, Math.toRadians(-90)); @@ -361,17 +361,17 @@ public void runAutonoumousMode() { case LEFT: dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); - dropYellowPixelPose = new Pose2d(34, -86, Math.toRadians(90)); + dropYellowPixelPose = new Pose2d(34, -87.5, Math.toRadians(90)); break; case MIDDLE: dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(29, -86, Math.toRadians(90)); + dropYellowPixelPose = new Pose2d(29, -87.5, Math.toRadians(90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); - dropYellowPixelPose = new Pose2d(23, -86, Math.toRadians(90)); + dropYellowPixelPose = new Pose2d(23, -87.5, Math.toRadians(90)); break; } midwayPose1 = new Pose2d(8, 8, Math.toRadians(0)); @@ -387,27 +387,36 @@ public void runAutonoumousMode() { Actions.runBlocking( drive.actionBuilder(drive.pose) .strafeToLinearHeading(moveBeyondTrussPose.position, moveBeyondTrussPose.heading) - .strafeToLinearHeading(dropPurplePixelPosePush.position, dropPurplePixelPosePush.heading) - .strafeToLinearHeading(dropPurplePixelPose.position, dropPurplePixelPose.heading) + //.strafeToLinearHeading(dropPurplePixelPosePush.position, dropPurplePixelPosePush.heading) + //.strafeToLinearHeading(dropPurplePixelPose.position, dropPurplePixelPose.heading) //.strafeToLinearHeading(dropPurplePixelPosePos.position, dropPurplePixelPosePos.heading) .build()); - //TODO : Code to drop Purple Pixel on Spike Mark - safeWaitSeconds(0); - for(int c = 0; c<40; c++) { moveServoGradually(shoulder, Evolution.SHOULDER_DRIVE); - sleep(15); + sleep(5); } for(int c = 0; c<40; c++) { moveServoGradually(wrist, Evolution.WRIST_INTAKE); - sleep(15); + sleep(5); } for(int c = 0; c<40; c++) { - moveServoGradually(elbow, Evolution.ELBOW_INTAKE); - sleep(15); + moveServoGradually(elbow, Evolution.ELBOW_TOP_TWO); + sleep(5); } + //Move robot to dropPurplePixel based on identified Spike Mark Location + Actions.runBlocking( + drive.actionBuilder(drive.pose) + //.strafeToLinearHeading(moveBeyondTrussPose.position, moveBeyondTrussPose.heading) + //.strafeToLinearHeading(dropPurplePixelPosePush.position, dropPurplePixelPosePush.heading) + .strafeToLinearHeading(dropPurplePixelPose.position, dropPurplePixelPose.heading) + //.strafeToLinearHeading(dropPurplePixelPosePos.position, dropPurplePixelPosePos.heading) + .build()); + + // Code to drop Purple Pixel on Spike Mark + safeWaitSeconds(0); + //sleep(200); rightFinger.setPosition(Evolution.RIGHT_FINGER_DROP); sleep(200); @@ -432,7 +441,7 @@ public void runAutonoumousMode() { //TODO : Code to intake pixel from stack //safeWaitSeconds(1); - rightFinger.setPosition(PIXEL_STACK_FINGER_GRAB); + rightFinger.setPosition(Evolution.RIGHT_FINGER_INTAKE); for(int e = 0; e<50; e++) { moveServoGradually(elbow, ELBOW_TOP_ONE); sleep(15); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java index 5f2a767ed1..c6a6964130 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Evolution.java @@ -67,11 +67,11 @@ public class Evolution extends LinearOpMode { public static final double ELBOW_INTAKE = 0.83; public static final double WRIST_DRIVE = 0.195; public static final double WRIST_INTAKE = 0.465; - public static final double LEFT_FINGER_GRIP = 0.72; - public static final double LEFT_FINGER_DROP = 0.5; + public static final double LEFT_FINGER_GRIP = 0.67; + public static final double LEFT_FINGER_DROP = 1; public static final double LEFT_FINGER_INTAKE = 1; - public static final double RIGHT_FINGER_GRIP = .27; - public static final double RIGHT_FINGER_DROP = .5; + public static final double RIGHT_FINGER_GRIP = .33; + public static final double RIGHT_FINGER_DROP = 0; public static final double RIGHT_FINGER_INTAKE = 0; public static final double TRIGGER_THRESHOLD = 0.5; public static final double LAUNCHER_START_POS = 0.8; @@ -84,7 +84,7 @@ public class Evolution extends LinearOpMode { // intake two off a stack of five public static final double SHOULDER_TOP_TWO = 0.425; public static final double WRIST_TOP_TWO = 0.59; - public static final double ELBOW_TOP_TWO = 0.74; + public static final double ELBOW_TOP_TWO = 0.81; // intake two off a stack of three public static final double SHOULDER_NEXT_TWO = 0.425; From 6e21640414ca6ed4cfb81362affb4c8b832c71e5 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 9 Jan 2024 16:52:46 -0700 Subject: [PATCH 149/154] auto tuning --- .../main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java | 2 +- .../firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 76390c49aa..96b1ff1d91 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -97,7 +97,7 @@ public static class Params { // path controller gains //TODO Step 13 Set value of Gains after running ManualFeedbackTuner - public double axialGain = 6.0; + public double axialGain = 20.0; public double lateralGain = 3.0; public double headingGain = 16.5; // shared with turn diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java index 0b6f1933e0..536ff628b0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java @@ -3,12 +3,14 @@ import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.MecanumDrive; import org.firstinspires.ftc.teamcode.TankDrive; import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer; import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer; +@TeleOp public final class ManualFeedbackTuner extends LinearOpMode { public static double DISTANCE = 64; From e8173aa05e52d06aead13f42f5a6da98ca3fc6d8 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Tue, 9 Jan 2024 19:18:53 -0700 Subject: [PATCH 150/154] auto stuff popcorn --- .../ftc/teamcode/EnigmaAuto.java | 48 +++++++++++++------ .../ftc/teamcode/MecanumDrive.java | 2 +- 2 files changed, 35 insertions(+), 15 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java index 2d2541fed3..1c301c92b1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EnigmaAuto.java @@ -283,7 +283,7 @@ public void runAutonoumousMode() { switch(identifiedSpikeMarkLocation){ case LEFT: dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up - dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); + dropPurplePixelPose = new Pose2d(16.27, 13.295, Math.toRadians(0)); dropYellowPixelPose = new Pose2d(19, 38.5, Math.toRadians(-90)); break; case MIDDLE: @@ -308,18 +308,18 @@ public void runAutonoumousMode() { switch(identifiedSpikeMarkLocation){ case LEFT: dropPurplePixelPosePush = new Pose2d(27, 8, Math.toRadians(0)); // change up - dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(70)); - dropYellowPixelPose = new Pose2d(34, -38.5, Math.toRadians(90)); + dropPurplePixelPose = new Pose2d(21.75, 0, Math.toRadians(70)); + dropYellowPixelPose = new Pose2d(34.25, -38.5, Math.toRadians(90)); break; case MIDDLE: dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up - dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); + dropPurplePixelPose = new Pose2d(22.75, 0, Math.toRadians(0)); dropYellowPixelPose = new Pose2d(27, -38.5, Math.toRadians(90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); dropPurplePixelPose = new Pose2d(22, 0, Math.toRadians(-35)); - dropYellowPixelPose = new Pose2d(20, -38.5, Math.toRadians(90)); + dropYellowPixelPose = new Pose2d(19.75, -38.5, Math.toRadians(90)); break; } midwayPose1 = new Pose2d(14, -13, Math.toRadians(45)); @@ -338,7 +338,7 @@ public void runAutonoumousMode() { case MIDDLE: dropPurplePixelPosePush = new Pose2d(32, 0, Math.toRadians(0)); // change up dropPurplePixelPose = new Pose2d(23.25, 0, Math.toRadians(0)); - dropYellowPixelPose = new Pose2d(26.25, 87.5, Math.toRadians(-90)); + dropYellowPixelPose = new Pose2d(28.25, 87.5, Math.toRadians(-90)); break; case RIGHT: dropPurplePixelPosePush = new Pose2d(27, -9, Math.toRadians(-45)); @@ -348,7 +348,7 @@ public void runAutonoumousMode() { } midwayPose1 = new Pose2d(8, -8, Math.toRadians(0)); midwayPose1a = new Pose2d(18, -21, Math.toRadians(-90)); - intakeStack = new Pose2d(53.25, -15,Math.toRadians(-90)); + intakeStack = new Pose2d(53.25, -20,Math.toRadians(-90)); intakeStack2 = new Pose2d(53.25, -17,Math.toRadians(-90)); midwayPose2 = new Pose2d(52, 62, Math.toRadians(-90)); waitSecondsBeforeDrop = 0; //TODO: Adjust time to wait for alliance partner to move from board @@ -383,6 +383,21 @@ public void runAutonoumousMode() { break; } + for(int c = 0; c<40; c++) { + moveServoGradually(shoulder, Evolution.SHOULDER_DRIVE); + sleep(5); + } + for(int c = 0; c<40; c++) { + moveServoGradually(wrist, Evolution.WRIST_INTAKE); + sleep(5); + } + for(int c = 0; c<60; c++) { + if(elbow.getPosition() < ELBOW_INTAKE){ + elbow.setPosition(elbow.getPosition() + 0.01); + } + sleep(10); + } + //Move robot to dropPurplePixel based on identified Spike Mark Location Actions.runBlocking( drive.actionBuilder(drive.pose) @@ -392,6 +407,7 @@ public void runAutonoumousMode() { //.strafeToLinearHeading(dropPurplePixelPosePos.position, dropPurplePixelPosePos.heading) .build()); + /* for(int c = 0; c<40; c++) { moveServoGradually(shoulder, Evolution.SHOULDER_DRIVE); sleep(5); @@ -400,10 +416,11 @@ public void runAutonoumousMode() { moveServoGradually(wrist, Evolution.WRIST_INTAKE); sleep(5); } - for(int c = 0; c<40; c++) { - moveServoGradually(elbow, Evolution.ELBOW_TOP_TWO); - sleep(5); + for(int c = 0; c<60; c++) { + moveServoGradually(elbow, Evolution.ELBOW_INTAKE); + sleep(3); } + */ //Move robot to dropPurplePixel based on identified Spike Mark Location Actions.runBlocking( @@ -438,6 +455,11 @@ public void runAutonoumousMode() { .strafeToLinearHeading(intakeStack.position, intakeStack.heading) .build()); + Actions.runBlocking( + drive.actionBuilder(drive.pose) + .strafeToLinearHeading(intakeStack2.position, intakeStack2.heading) + .build()); + //TODO : Code to intake pixel from stack //safeWaitSeconds(1); @@ -454,10 +476,7 @@ public void runAutonoumousMode() { moveServoGradually(shoulder, SHOULDER_TOP_ONE); sleep(15); } - Actions.runBlocking( - drive.actionBuilder(drive.pose) - .strafeToLinearHeading(intakeStack2.position, intakeStack2.heading) - .build()); + sleep(300); rightFinger.setPosition(Evolution.RIGHT_FINGER_GRIP); sleep(650); @@ -509,6 +528,7 @@ public void runAutonoumousMode() { moveServoGradually(wrist, WRIST_INTAKE); sleep(10); } + for(int e = 0; e<20; e++) { moveServoGradually(elbow, ELBOW_DRIVE); sleep(10); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 96b1ff1d91..76390c49aa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -97,7 +97,7 @@ public static class Params { // path controller gains //TODO Step 13 Set value of Gains after running ManualFeedbackTuner - public double axialGain = 20.0; + public double axialGain = 6.0; public double lateralGain = 3.0; public double headingGain = 16.5; // shared with turn From f762fd8a630932342519d712c29860488ac82720 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 10 Jan 2024 17:04:00 -0700 Subject: [PATCH 151/154] pushing --- .../ftc/teamcode/AberhamLinconTest.java | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AberhamLinconTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AberhamLinconTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AberhamLinconTest.java new file mode 100644 index 0000000000..82ce79b8aa --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AberhamLinconTest.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.Servo; + +@TeleOp +public class AberhamLinconTest extends LinearOpMode { + private ColorSensor color; + + private void function() { + } + @Override + public void runOpMode() throws InterruptedException { + + color = hardwareMap.get(ColorSensor.class, "color"); + + telemetry.update(); + waitForStart(); + color.enableLed(true); + + while(opModeIsActive()) { + + + telemetry.update(); + } + } +} From 7f586a727af25fafb2fa8ba01794bade1a37a498 Mon Sep 17 00:00:00 2001 From: Gabriel Date: Wed, 10 Jan 2024 17:14:51 -0700 Subject: [PATCH 152/154] start of bonus auto pixels --- .../firstinspires/ftc/teamcode/aLincoln.java | 135 ++++++++++++++++++ 1 file changed, 135 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java new file mode 100644 index 0000000000..15dcc850b5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java @@ -0,0 +1,135 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.internal.system.Deadline; + +import java.util.concurrent.TimeUnit; + +/* + * Display patterns of a REV Robotics Blinkin LED Driver. + * AUTO mode cycles through all of the patterns. + * MANUAL mode allows the user to manually change patterns using the + * left and right bumpers of a gamepad. + * + * Configure the driver on a servo port, and name it "blinkin". + * + * Displays the first pattern upon init. + */ +@TeleOp(name="BlinkinExample") +@Disabled +public class aLincoln extends OpMode { + + /* + * Change the pattern every 10 seconds in AUTO mode. + */ + private final static int LED_PERIOD = 10; + + /* + * Rate limit gamepad button presses to every 500ms. + */ + private final static int GAMEPAD_LOCKOUT = 500; + + RevBlinkinLedDriver blinkinLedDriver; + RevBlinkinLedDriver.BlinkinPattern pattern; + + Telemetry.Item patternName; + Telemetry.Item display; + DisplayKind displayKind; + Deadline ledCycleDeadline; + Deadline gamepadRateLimit; + + protected enum DisplayKind { + MANUAL, + AUTO + } + + @Override + public void init() + { + displayKind = DisplayKind.AUTO; + + blinkinLedDriver = hardwareMap.get(RevBlinkinLedDriver.class, "blinkin"); + pattern = RevBlinkinLedDriver.BlinkinPattern.RAINBOW_RAINBOW_PALETTE; + blinkinLedDriver.setPattern(pattern); + + display = telemetry.addData("Display Kind: ", displayKind.toString()); + patternName = telemetry.addData("Pattern: ", pattern.toString()); + + ledCycleDeadline = new Deadline(LED_PERIOD, TimeUnit.SECONDS); + gamepadRateLimit = new Deadline(GAMEPAD_LOCKOUT, TimeUnit.MILLISECONDS); + } + + @Override + public void loop() + { + handleGamepad(); + + if (displayKind == DisplayKind.AUTO) { + doAutoDisplay(); + } else { + /* + * MANUAL mode: Nothing to do, setting the pattern as a result of a gamepad event. + */ + } + } + + /* + * handleGamepad + * + * Responds to a gamepad button press. Demonstrates rate limiting for + * button presses. If loop() is called every 10ms and and you don't rate + * limit, then any given button press may register as multiple button presses, + * which in this application is problematic. + * + * A: Manual mode, Right bumper displays the next pattern, left bumper displays the previous pattern. + * B: Auto mode, pattern cycles, changing every LED_PERIOD seconds. + */ + protected void handleGamepad() + { + if (!gamepadRateLimit.hasExpired()) { + return; + } + + if (gamepad1.a) { + setDisplayKind(DisplayKind.MANUAL); + gamepadRateLimit.reset(); + } else if (gamepad1.b) { + setDisplayKind(DisplayKind.AUTO); + gamepadRateLimit.reset(); + } else if ((displayKind == DisplayKind.MANUAL) && (gamepad1.left_bumper)) { + pattern = pattern.previous(); + displayPattern(); + gamepadRateLimit.reset(); + } else if ((displayKind == DisplayKind.MANUAL) && (gamepad1.right_bumper)) { + pattern = pattern.next(); + displayPattern(); + gamepadRateLimit.reset(); + } + } + + protected void setDisplayKind(DisplayKind displayKind) + { + this.displayKind = displayKind; + display.setValue(displayKind.toString()); + } + + protected void doAutoDisplay() + { + if (ledCycleDeadline.hasExpired()) { + pattern = pattern.next(); + displayPattern(); + ledCycleDeadline.reset(); + } + } + + protected void displayPattern() + { + blinkinLedDriver.setPattern(pattern); + patternName.setValue(pattern.toString()); + } +} \ No newline at end of file From 53deb2a42c9a7e7798d29afa957ffdfea91d7f2a Mon Sep 17 00:00:00 2001 From: Gabriel Date: Wed, 10 Jan 2024 17:43:43 -0700 Subject: [PATCH 153/154] start of bonus auto pixels --- .../src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java index 15dcc850b5..e2819811cd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java @@ -109,6 +109,8 @@ protected void handleGamepad() pattern = pattern.next(); displayPattern(); gamepadRateLimit.reset(); + } else if ((displayKind == DisplayKind.MANUAL) && (gamepad1.dpad_down)) { + blinkinLedDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN); } } From 24b5f8a7bc5ef7c1f53828d207e45f435d7a1f52 Mon Sep 17 00:00:00 2001 From: BudsterGaming Date: Wed, 10 Jan 2024 17:58:16 -0700 Subject: [PATCH 154/154] Aberham Blinking --- .../java/org/firstinspires/ftc/teamcode/AberhamLinconTest.java | 1 + .../src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AberhamLinconTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AberhamLinconTest.java index 82ce79b8aa..136d489f95 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AberhamLinconTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AberhamLinconTest.java @@ -19,6 +19,7 @@ public void runOpMode() throws InterruptedException { telemetry.update(); waitForStart(); color.enableLed(true); + color.green(); while(opModeIsActive()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java index e2819811cd..6560eee046 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/aLincoln.java @@ -21,7 +21,6 @@ * Displays the first pattern upon init. */ @TeleOp(name="BlinkinExample") -@Disabled public class aLincoln extends OpMode { /* @@ -114,6 +113,7 @@ protected void handleGamepad() } } + protected void setDisplayKind(DisplayKind displayKind) { this.displayKind = displayKind; @@ -134,4 +134,5 @@ protected void displayPattern() blinkinLedDriver.setPattern(pattern); patternName.setValue(pattern.toString()); } + } \ No newline at end of file