From 983e75ed9f27b465a217645deca557c55f45563c Mon Sep 17 00:00:00 2001 From: RZW-13 Date: Sat, 20 Jan 2024 03:50:43 -0700 Subject: [PATCH 1/5] working teleop cocde --- .../ftc/teamcode/auton/autonPark.java | 2 + .../auton/{ => tests}/DistanceSensorTest.java | 2 +- .../auton/tests/RobotMethodsTest.java | 93 +++++++++++++ .../auton/{ => tests}/TouchSensorTest.java | 2 +- .../ftc/teamcode/teleop/TeleOp.java | 131 +++++++++++++++--- 5 files changed, 207 insertions(+), 23 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/{ => tests}/DistanceSensorTest.java (95%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/{ => tests}/TouchSensorTest.java (94%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/autonPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/autonPark.java index 03435690..0545e3c5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/autonPark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/autonPark.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.auton; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; @@ -11,6 +12,7 @@ import org.firstinspires.ftc.teamcode.subsystems.robotMethods.*; +@Autonomous(name = "Auton Park") public class autonPark extends OpMode { byte startingLocationIndex = 0; double[][] startingLocation = new double[][] {new double[] {0, 0, 0}, new double[] {0, 0, 0}, diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/DistanceSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/DistanceSensorTest.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/DistanceSensorTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/DistanceSensorTest.java index 53a784d1..550edd31 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/DistanceSensorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/DistanceSensorTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.auton; +package org.firstinspires.ftc.teamcode.auton.tests; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java new file mode 100644 index 00000000..7e50cbb7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java @@ -0,0 +1,93 @@ +package org.firstinspires.ftc.teamcode.auton.tests; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware; +import org.firstinspires.ftc.teamcode.subsystems.hardware.HardwareElement; +import org.firstinspires.ftc.teamcode.subsystems.robotMethods.RobotMethods; +import org.firstinspires.ftc.teamcode.subsystems.stateMachineController.Context; + +@Autonomous(name = "Robot Methods Test") +public class RobotMethodsTest extends OpMode { + Hardware robot; + Context context; + double motorRunPower = 0.5; + double tolerance = 0; + + @Override + public void init() { + telemetry.addData("Status", "Initializing"); + + robot = new Hardware(hardwareMap, telemetry); + context = new Context(); + + // Init Servos. + robot.introduce(new HardwareElement<>(Servo.class, hardwareMap, "clawLock")); + robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "clawJoint")); + + // Init CR Servos. + robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "intakeGeckoWheels")); + robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "intakeTube")); + robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeJoint1")); + robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeJoint2")); + robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeTube")); + + // Init DcMotors. + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftFront", "setDirection:FORWARD")); + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftRear", "setDirection:FORWARD")); + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightFront", "setDirection:FORWARD")); + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightRear", "setDirection:FORWARD")); + + // Init arm and Viper Slide DcMotors. + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "armJoint", "setMode:RUN_USING_ENCODER")); + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftViperSlide", "setMode:RUN_USING_ENCODER")); + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightViperSlide", "setMode:RUN_USING_ENCODER")); + + telemetry.addData("Status", "Initialized"); + } + + @Override + public void start() { + robot.get("leftFront").setTargetPosition(robot.get("leftFront").getCurrentPosition() + 1000); + robot.get("leftRear").setTargetPosition(robot.get("leftRear").getCurrentPosition() + 1000); + robot.get("rightFront").setTargetPosition(robot.get("rightFront").getCurrentPosition() + 1000); + robot.get("rightRear").setTargetPosition(robot.get("rightRear").getCurrentPosition() + 1000); + } + + @Override + public void loop() { + double power = 0.25; + if (robot.get("leftFront").getCurrentPosition() < robot.get("leftFront").getTargetPosition() + && power > 0) { + robot.get("leftFront").setPower(0.25); + } + if (robot.get("leftRear").getCurrentPosition() < robot.get("leftFront").getTargetPosition() + && power > 0) { + robot.get("leftRear").setPower(0.25); + } + if (robot.get("rightFront").getCurrentPosition() < robot.get("leftFront").getTargetPosition() + && power > 0) { + robot.get("rightFront").setPower(0.25); + } + if (robot.get("rightRear").getCurrentPosition() < robot.get("leftFront").getTargetPosition() + && power > 0) { + robot.get("rightRear").setPower(0.25); + } + + telemetry.addData("leftFront current pos", robot.get("leftFront").getCurrentPosition()); + telemetry.addData("leftFront target", robot.get("leftFront").getTargetPosition()); + telemetry.addLine("***"); + telemetry.addData("leftRear current pos", robot.get("leftFront").getCurrentPosition()); + telemetry.addData("leftRear target", robot.get("leftFront").getTargetPosition()); + telemetry.addLine("***"); + telemetry.addData("rightFront current pos", robot.get("leftFront").getCurrentPosition()); + telemetry.addData("rightFront target", robot.get("leftFront").getTargetPosition()); + telemetry.addLine("***"); + telemetry.addData("rightRear current pos", robot.get("leftFront").getCurrentPosition()); + telemetry.addData("rightRear target", robot.get("leftFront").getTargetPosition()); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TouchSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/TouchSensorTest.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TouchSensorTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/TouchSensorTest.java index 06e22156..e5ca4a9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TouchSensorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/TouchSensorTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.auton; +package org.firstinspires.ftc.teamcode.auton.tests; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java index 03d52790..aab26c9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.hardware.CRServo; 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.teamcode.subsystems.hardware.Hardware; @@ -30,6 +31,17 @@ public class TeleOp extends OpMode { private final double jointStartPos = 0.0; private final double jointTargetPos = 0.0; boolean scoringATM; + byte lockPositionIndex = 0; + byte slidePositionIndex = 0; + byte armPositionIndex = 0; + + double[] lockPositions = new double[] {0.0, 0.4, 0.8}; + double[] slidePositions = new double[] {0.0, 1.0}; + double[] armPositions = new double[] {0.0, 1.0}; + + ElapsedTime clawPositionChange = new ElapsedTime(); + ElapsedTime viperSlidePositionChange = new ElapsedTime(); + ElapsedTime armPositionChange = new ElapsedTime(); @Override public void init() { @@ -44,8 +56,7 @@ public void init() { // Init CR Servos. robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "intakeGeckoWheels")); robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "intakeTube")); - robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeJoint1")); - robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeJoint2")); + robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeJoint")); robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeTube")); // Init DcMotors. @@ -70,9 +81,9 @@ public void init() { " *** \n" + " * Gamepad 2\n" + " * Right Trigger = Claw Joint -\n" + - " * Right Trigger = Claw Joint -\n" + - " * X = Automatic Scoring\n" + - " * Y = VS, Arm and Claw reset (Automatic Scoring Reset)\n" + + " * Right Bumper = Claw Joint +\n" + + " * X = Arm +\n" + + " * Y = Arm -\n" + " * A = Activate Intake (Hold)\n" + " * Left Bumper = Claw Lock + (Manual)\n" + " * Left Trigger = Claw Lock - (Manual)\n" + @@ -80,20 +91,46 @@ public void init() { " * D Pad Down = VS Down (Manual)"); } + public void start() { + clawPositionChange.reset(); + viperSlidePositionChange.reset(); + armPositionChange.reset(); + } + @Override public void loop() { - automaticScoring(); + telemetry.addLine("Controls:\n" + + " *** \n" + + " * Gamepad 1\n" + + " * Left Stick y = Drive\n" + + " * Left Stick x = Strafe\n" + + " * Right Stick x = Turn\n" + + " * \n" + + " *** \n" + + " * Gamepad 2\n" + + " * Right Trigger = Claw Joint -\n" + + " * Right Bumper = Claw Joint +\n" + + " * X = Arm + \n" + + " * Y = Arm - \n" + + " * A = Activate Intake (Hold)\n" + + " * Left Bumper = Claw Lock + (Manual)\n" + + " * Left Trigger = Claw Lock - (Manual)\n" + + " * D Pad Up = VS Up (Manual)\n" + + " * D Pad Down = VS Down (Manual)"); + + // automaticScoring(); clawLock(); viperSlides(); clawJoint(); intake(); + arm(); outerIntakeJoints(); driving(); } public void automaticScoring() { // Calling scoring() via left bumper and resetting via left trigger - if (gamepad1.x) { + if (gamepad1.dpad_left) { scoring(); } else if (gamepad1.y && !scoringATM) { resetPos(); @@ -103,21 +140,50 @@ public void automaticScoring() { public void clawLock() { // Move claw via x and y. - if (gamepad1.left_bumper) { - robot.get("clawLock").setPosition(robot.get("clawLock").getPosition() + -0.125); - } else if (gamepad1.left_trigger > 0.5) { - robot.get("clawLock").setPosition(robot.get("clawLock").getPosition() + 0.125); + if (clawPositionChange.time() > 0.25){ + if (gamepad1.left_bumper) { + lockPositionIndex ++; + if(lockPositionIndex > 2) { + lockPositionIndex = 2; + } + robot.get("clawLock").setPosition(lockPositions[lockPositionIndex]); + clawPositionChange.reset(); + } else if (gamepad1.left_trigger > 0.5) { + lockPositionIndex --; + if(lockPositionIndex < 0) { + lockPositionIndex = 0; + } + robot.get("clawLock").setPosition(lockPositions[lockPositionIndex]); + clawPositionChange.reset(); + } } + telemetry.addData("claw lock position", robot.get("clawLock").getPosition()); + telemetry.addData("claw lock index", lockPositionIndex); } public void viperSlides() { + // Moving VS to pre-set positions. + if (viperSlidePositionChange.time() > 0.5){ + if (gamepad1.b) { + if (lockPositionIndex == 1) { + lockPositionIndex = 0; + } else { + lockPositionIndex = 1; + } + + robot.get("leftViperSlide").setTargetPosition((int) slidePositions[slidePositionIndex]); + robot.get("rightViperSlide").setTargetPosition((int) slidePositions[slidePositionIndex]); + viperSlidePositionChange.reset(); + } + } + // Moving VS manually via dpad up and down. if (gamepad1.dpad_up) { - robot.get("leftViperSlide").setPower(0.5); - robot.get("rightViperSlide").setPower(0.5); + robot.get("leftViperSlide").setPower(0.25); + robot.get("rightViperSlide").setPower(-0.25); } else if (gamepad1.dpad_down) { - robot.get("leftViperSlide").setPower(-0.5); - robot.get("rightViperSlide").setPower(-0.5); + robot.get("leftViperSlide").setPower(-0.25); + robot.get("rightViperSlide").setPower(0.25); } else { robot.get("leftViperSlide").setPower(0); robot.get("rightViperSlide").setPower(0); @@ -150,13 +216,36 @@ public void intake() { } } + public void arm() { + // Moving arm to pre-set positions. + if (armPositionChange.time() > 0.5){ + if (gamepad1.x) { + if (armPositionIndex == 1) { + armPositionIndex = 0; + } else { + armPositionIndex = 1; + } + + robot.get("armJoint").setTargetPosition((int) armPositions[armPositionIndex]); + armPositionChange.reset(); + } + } + + // Moving arm manually. + if (gamepad1.dpad_left) { + robot.get("armJoint").setPower(0.15); + } else if (gamepad1.dpad_right) { + robot.get("armJoint").setPower(-0.15); + } else { + robot.get("armJoint").setPower(0); + } + } + public void outerIntakeJoints() { - if (gamepad1.right_bumper) { - robot.get("outerIntakeJoint1").setPower(-0.25); - robot.get("outerIntakeJoint2").setPower(0.25); - } else if (gamepad1.right_trigger > 0.5) { - robot.get("outerIntakeJoint1").setPower(0.25); - robot.get("outerIntakeJoint2").setPower(-0.25); + if (gamepad1.dpad_up) { + robot.get("outerIntakeJoint").setPower(-0.25); + } else if (gamepad1.dpad_up) { + robot.get("outerIntakeJoint").setPower(0.25); } } From 6572ab74531394dd6046e1f22709b7cdcbcb29ce Mon Sep 17 00:00:00 2001 From: RZW-13 Date: Sat, 20 Jan 2024 13:03:08 -0700 Subject: [PATCH 2/5] teleop progress --- .../ftc/teamcode/teleop/TeleOp.java | 100 ++++++++---------- 1 file changed, 46 insertions(+), 54 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java index aab26c9a..2586e16d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java @@ -63,7 +63,7 @@ public void init() { robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftFront", "setDirection:FORWARD")); robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftRear", "setDirection:FORWARD")); robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightFront", "setDirection:FORWARD")); - robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightRear", "setDirection:FORWARD")); + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightRear")); // Init arm and Viper Slide DcMotors. robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "armJoint", "setMode:RUN_USING_ENCODER")); @@ -71,24 +71,8 @@ public void init() { robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightViperSlide", "setMode:RUN_USING_ENCODER")); telemetry.addData("Status", "Initialized"); - telemetry.addLine("Controls:\n" + - " *** \n" + - " * Gamepad 1\n" + - " * Left Stick y = Drive\n" + - " * Left Stick x = Strafe\n" + - " * Right Stick x = Turn\n" + - " * \n" + - " *** \n" + - " * Gamepad 2\n" + - " * Right Trigger = Claw Joint -\n" + - " * Right Bumper = Claw Joint +\n" + - " * X = Arm +\n" + - " * Y = Arm -\n" + - " * A = Activate Intake (Hold)\n" + - " * Left Bumper = Claw Lock + (Manual)\n" + - " * Left Trigger = Claw Lock - (Manual)\n" + - " * D Pad Up = VS Up (Manual)\n" + - " * D Pad Down = VS Down (Manual)"); + + listControls(); } public void start() { @@ -99,24 +83,7 @@ public void start() { @Override public void loop() { - telemetry.addLine("Controls:\n" + - " *** \n" + - " * Gamepad 1\n" + - " * Left Stick y = Drive\n" + - " * Left Stick x = Strafe\n" + - " * Right Stick x = Turn\n" + - " * \n" + - " *** \n" + - " * Gamepad 2\n" + - " * Right Trigger = Claw Joint -\n" + - " * Right Bumper = Claw Joint +\n" + - " * X = Arm + \n" + - " * Y = Arm - \n" + - " * A = Activate Intake (Hold)\n" + - " * Left Bumper = Claw Lock + (Manual)\n" + - " * Left Trigger = Claw Lock - (Manual)\n" + - " * D Pad Up = VS Up (Manual)\n" + - " * D Pad Down = VS Down (Manual)"); + listControls(); // automaticScoring(); clawLock(); @@ -164,7 +131,7 @@ public void clawLock() { public void viperSlides() { // Moving VS to pre-set positions. if (viperSlidePositionChange.time() > 0.5){ - if (gamepad1.b) { + if (false) { // gamepad1.b if (lockPositionIndex == 1) { lockPositionIndex = 0; } else { @@ -178,10 +145,10 @@ public void viperSlides() { } // Moving VS manually via dpad up and down. - if (gamepad1.dpad_up) { + if (gamepad1.b) { robot.get("leftViperSlide").setPower(0.25); robot.get("rightViperSlide").setPower(-0.25); - } else if (gamepad1.dpad_down) { + } else if (gamepad1.y) { robot.get("leftViperSlide").setPower(-0.25); robot.get("rightViperSlide").setPower(0.25); } else { @@ -205,8 +172,8 @@ public void intake() { // Activating Intake via gamepad a. if (gamepad1.a) { robot.get("intakeTube").setPower(intakePower); - robot.get("intakeGeckoWheels").setPower(intakePower); - robot.get("outerIntakeTube").setPower(-0.5); + robot.get("intakeGeckoWheels").setPower(0.2); + robot.get("outerIntakeTube").setPower(0.5); telemetry.addData("Intake", "Running"); } else { robot.get("intakeTube").setPower(0); @@ -214,21 +181,25 @@ public void intake() { robot.get("outerIntakeTube").setPower(0); telemetry.addData("Intake", "Stopped"); } + + if (gamepad1.y) { + robot.get("intakeGeckoWheels").setPower(-0.2); + } } public void arm() { // Moving arm to pre-set positions. if (armPositionChange.time() > 0.5){ - if (gamepad1.x) { - if (armPositionIndex == 1) { - armPositionIndex = 0; - } else { - armPositionIndex = 1; - } - - robot.get("armJoint").setTargetPosition((int) armPositions[armPositionIndex]); - armPositionChange.reset(); - } +// if (gamepad1.y) { +// if (armPositionIndex == 1) { +// armPositionIndex = 0; +// } else { +// armPositionIndex = 1; +// } +// +// robot.get("armJoint").setTargetPosition((int) armPositions[armPositionIndex]); +// armPositionChange.reset(); +// } } // Moving arm manually. @@ -244,7 +215,7 @@ public void arm() { public void outerIntakeJoints() { if (gamepad1.dpad_up) { robot.get("outerIntakeJoint").setPower(-0.25); - } else if (gamepad1.dpad_up) { + } else if (gamepad1.dpad_down) { robot.get("outerIntakeJoint").setPower(0.25); } } @@ -260,7 +231,7 @@ public void driving() { robot.get("leftFront").setPower(leftFPower); robot.get("leftRear").setPower(leftBPower); robot.get("rightFront").setPower(rightFPower); - robot.get("rightRear").setPower(rightBPower); + robot.get("rightRear").setPower(-rightBPower); telemetry.addData("leftFPower: ", leftFPower); telemetry.addData("leftBPower: ", leftBPower); @@ -382,4 +353,25 @@ public void runViperSlidesToPosition(double position, double power) { robot.get("rightViperSlide").setPower(power); } } + + public void listControls() { + telemetry.addLine("Controls:\n" + + " *** \n" + + " * Gamepad 1\n" + + " * Left Stick y = Drive\n" + + " * Left Stick x = Strafe\n" + + " * Right Stick x = Turn\n" + + " * Left Trigger = Claw Lock -\n" + + " * Left Bumper = Claw Lock +\n" + + " * Right Trigger = Claw Joint -\n" + + " * Right Bumper = Claw Joint +\n" + + " * A = Run Intake (Toggle)\n" + + " * B = VS +\n" + + " * X = VS -\n" + + " * Y = Arm Position Change\n" + + " * D Pad Left = Arm -\n" + + " * D Pad Right = Arm +\n" + + " * D Pad Up = Outer Intake Down\n" + + " * D Pad Down = Outer Intake Up"); + } } From f91f750dfdd91d492bd0f8ac1049a4abb9f9eab2 Mon Sep 17 00:00:00 2001 From: RZW-13 Date: Mon, 22 Jan 2024 10:55:26 -0700 Subject: [PATCH 3/5] fpa code --- .../auton/tests/RobotMethodsTest.java | 37 +++++++++++-------- .../ftc/teamcode/teleop/TeleOp.java | 22 +++++++---- 2 files changed, 35 insertions(+), 24 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java index 7e50cbb7..1fc68ff6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java @@ -32,8 +32,7 @@ public void init() { // Init CR Servos. robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "intakeGeckoWheels")); robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "intakeTube")); - robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeJoint1")); - robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeJoint2")); + robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeJoint")); robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeTube")); // Init DcMotors. @@ -52,30 +51,36 @@ public void init() { @Override public void start() { - robot.get("leftFront").setTargetPosition(robot.get("leftFront").getCurrentPosition() + 1000); - robot.get("leftRear").setTargetPosition(robot.get("leftRear").getCurrentPosition() + 1000); - robot.get("rightFront").setTargetPosition(robot.get("rightFront").getCurrentPosition() + 1000); - robot.get("rightRear").setTargetPosition(robot.get("rightRear").getCurrentPosition() + 1000); + robot.get("leftFront").setTargetPosition(robot.get("leftFront").getCurrentPosition() + 100); + robot.get("leftRear").setTargetPosition(robot.get("leftRear").getCurrentPosition() + 100); + robot.get("rightFront").setTargetPosition(robot.get("rightFront").getCurrentPosition() + 100); + robot.get("rightRear").setTargetPosition(robot.get("rightRear").getCurrentPosition() - 100); } @Override public void loop() { - double power = 0.25; - if (robot.get("leftFront").getCurrentPosition() < robot.get("leftFront").getTargetPosition() - && power > 0) { + if (robot.get("leftFront").getCurrentPosition() < robot.get("leftFront").getTargetPosition()) { robot.get("leftFront").setPower(0.25); + } else { + robot.get("leftFront").setPower(0); } - if (robot.get("leftRear").getCurrentPosition() < robot.get("leftFront").getTargetPosition() - && power > 0) { + + if (robot.get("leftRear").getCurrentPosition() < robot.get("leftFront").getTargetPosition()) { robot.get("leftRear").setPower(0.25); + } else { + robot.get("leftRear").setPower(0); } - if (robot.get("rightFront").getCurrentPosition() < robot.get("leftFront").getTargetPosition() - && power > 0) { + + if (robot.get("rightFront").getCurrentPosition() < robot.get("leftFront").getTargetPosition()) { robot.get("rightFront").setPower(0.25); + } else { + robot.get("rightFront").setPower(0); } - if (robot.get("rightRear").getCurrentPosition() < robot.get("leftFront").getTargetPosition() - && power > 0) { - robot.get("rightRear").setPower(0.25); + + if (robot.get("rightRear").getCurrentPosition() > robot.get("leftFront").getTargetPosition()) { + robot.get("rightRear").setPower(-0.25); + } else { + robot.get("rightRear").setPower(0); } telemetry.addData("leftFront current pos", robot.get("leftFront").getCurrentPosition()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java index 2586e16d..0ba7d4f4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOp.java @@ -145,10 +145,10 @@ public void viperSlides() { } // Moving VS manually via dpad up and down. - if (gamepad1.b) { + if (gamepad1.dpad_up) { robot.get("leftViperSlide").setPower(0.25); robot.get("rightViperSlide").setPower(-0.25); - } else if (gamepad1.y) { + } else if (gamepad1.dpad_down) { robot.get("leftViperSlide").setPower(-0.25); robot.get("rightViperSlide").setPower(0.25); } else { @@ -175,7 +175,13 @@ public void intake() { robot.get("intakeGeckoWheels").setPower(0.2); robot.get("outerIntakeTube").setPower(0.5); telemetry.addData("Intake", "Running"); - } else { + } else if (gamepad1.x) { + robot.get("intakeTube").setPower(-intakePower); + robot.get("intakeGeckoWheels").setPower(-0.2); + robot.get("outerIntakeTube").setPower(-0.5); + telemetry.addData("Intake", "Running"); + } + else { robot.get("intakeTube").setPower(0); robot.get("intakeGeckoWheels").setPower(0); robot.get("outerIntakeTube").setPower(0); @@ -213,11 +219,11 @@ public void arm() { } public void outerIntakeJoints() { - if (gamepad1.dpad_up) { - robot.get("outerIntakeJoint").setPower(-0.25); - } else if (gamepad1.dpad_down) { - robot.get("outerIntakeJoint").setPower(0.25); - } +// if (gamepad1.dpad_up) { +// robot.get("outerIntakeJoint").setPower(-0.25); +// } else if (gamepad1.dpad_down) { +// robot.get("outerIntakeJoint").setPower(0.25); +// } } // Strafe Drive using sticks on Gamepad 1. From 339b5e3a70db4845709a6db2ac2c5820a39d6da7 Mon Sep 17 00:00:00 2001 From: RZW-13 Date: Mon, 22 Jan 2024 20:50:59 -0700 Subject: [PATCH 4/5] working motor encoders --- .../ftc/teamcode/auton/tests/DcMotorTest.java | 46 +++++++++++++ .../auton/tests/RobotMethodsTest.java | 64 ++----------------- .../subsystems/robotMethods/RobotMethods.java | 23 +++++-- 3 files changed, 70 insertions(+), 63 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/DcMotorTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/DcMotorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/DcMotorTest.java new file mode 100644 index 00000000..336f92fd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/DcMotorTest.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.auton.tests; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware; +import org.firstinspires.ftc.teamcode.subsystems.hardware.HardwareElement; + +@Autonomous(name = "DcMotor Test") +public class DcMotorTest extends OpMode { + Hardware robot; + + @Override + public void init() { + telemetry.addData("Status", "Initializing"); + + robot = new Hardware(hardwareMap, telemetry); + + // Init DcMotors. + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "testMotor")); + robot.get("testMotor").setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + telemetry.addData("Status", "Initialized"); + } + + @Override + public void start() { + robot.get("testMotor").setTargetPosition(-300); + robot.get("testMotor").setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.get("testMotor").setPower(-0.75); + } + + @Override + public void loop() { + if (robot.get("testMotor").getCurrentPosition() > 100) { + robot.get("testMotor").setPower(0.75); + } else if (robot.get("testMotor").getCurrentPosition() <= + robot.get("testMotor").getTargetPosition()) { + robot.get("testMotor").setPower(0); + } + + telemetry.addData("target pos", robot.get("testMotor").getTargetPosition()); + telemetry.addData("current pos", robot.get("testMotor").getCurrentPosition()); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java index 1fc68ff6..ff985add 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java @@ -2,9 +2,8 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.robot.Robot; import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware; import org.firstinspires.ftc.teamcode.subsystems.hardware.HardwareElement; @@ -25,74 +24,23 @@ public void init() { robot = new Hardware(hardwareMap, telemetry); context = new Context(); - // Init Servos. - robot.introduce(new HardwareElement<>(Servo.class, hardwareMap, "clawLock")); - robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "clawJoint")); - - // Init CR Servos. - robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "intakeGeckoWheels")); - robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "intakeTube")); - robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeJoint")); - robot.introduce(new HardwareElement<>(CRServo.class, hardwareMap, "outerIntakeTube")); - // Init DcMotors. - robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftFront", "setDirection:FORWARD")); - robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftRear", "setDirection:FORWARD")); - robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightFront", "setDirection:FORWARD")); - robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightRear", "setDirection:FORWARD")); - - // Init arm and Viper Slide DcMotors. - robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "armJoint", "setMode:RUN_USING_ENCODER")); - robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftViperSlide", "setMode:RUN_USING_ENCODER")); - robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightViperSlide", "setMode:RUN_USING_ENCODER")); + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "testMotor")); telemetry.addData("Status", "Initialized"); } @Override public void start() { - robot.get("leftFront").setTargetPosition(robot.get("leftFront").getCurrentPosition() + 100); - robot.get("leftRear").setTargetPosition(robot.get("leftRear").getCurrentPosition() + 100); - robot.get("rightFront").setTargetPosition(robot.get("rightFront").getCurrentPosition() + 100); - robot.get("rightRear").setTargetPosition(robot.get("rightRear").getCurrentPosition() - 100); + RobotMethods.setTargetPosition(robot.get("testMotor"), 300); } @Override public void loop() { - if (robot.get("leftFront").getCurrentPosition() < robot.get("leftFront").getTargetPosition()) { - robot.get("leftFront").setPower(0.25); - } else { - robot.get("leftFront").setPower(0); - } + RobotMethods.updateMotor(telemetry, robot.get("testMotor"), motorRunPower); - if (robot.get("leftRear").getCurrentPosition() < robot.get("leftFront").getTargetPosition()) { - robot.get("leftRear").setPower(0.25); - } else { - robot.get("leftRear").setPower(0); + if (gamepad1.a) { + robot.get("testMotor").setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); } - - if (robot.get("rightFront").getCurrentPosition() < robot.get("leftFront").getTargetPosition()) { - robot.get("rightFront").setPower(0.25); - } else { - robot.get("rightFront").setPower(0); - } - - if (robot.get("rightRear").getCurrentPosition() > robot.get("leftFront").getTargetPosition()) { - robot.get("rightRear").setPower(-0.25); - } else { - robot.get("rightRear").setPower(0); - } - - telemetry.addData("leftFront current pos", robot.get("leftFront").getCurrentPosition()); - telemetry.addData("leftFront target", robot.get("leftFront").getTargetPosition()); - telemetry.addLine("***"); - telemetry.addData("leftRear current pos", robot.get("leftFront").getCurrentPosition()); - telemetry.addData("leftRear target", robot.get("leftFront").getTargetPosition()); - telemetry.addLine("***"); - telemetry.addData("rightFront current pos", robot.get("leftFront").getCurrentPosition()); - telemetry.addData("rightFront target", robot.get("leftFront").getTargetPosition()); - telemetry.addLine("***"); - telemetry.addData("rightRear current pos", robot.get("leftFront").getCurrentPosition()); - telemetry.addData("rightRear target", robot.get("leftFront").getTargetPosition()); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/robotMethods/RobotMethods.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/robotMethods/RobotMethods.java index d22c687f..740dd340 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/robotMethods/RobotMethods.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/robotMethods/RobotMethods.java @@ -118,21 +118,34 @@ private static void setWheelTargets(Hardware robot, double mm) { setTargetPosition(robot.get("rightRear"), mm); } - private static void setTargetPosition(DcMotor motor, double mm) { + public static void setTargetPosition(DcMotor motor, double mm) { motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); motor.setTargetPosition((int) Math.round(mm/mmPerEncoderTick)); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } - private static void updateMotor(Telemetry telemetry, DcMotor motor, double motorRunPower) { + public static void updateMotor(Telemetry telemetry, DcMotor motor, double motorRunPower) { showMotorStatus(motor, telemetry); updateMotorPower(motor, motorRunPower); } private static void updateMotorPower(DcMotor motor, double motorRunPower) { - if (motor.getTargetPosition() == motor.getCurrentPosition()) { + // Is motor in position? + if ((motor.getTargetPosition() <= 0 && motor.getCurrentPosition() <= motor.getTargetPosition() || + motor.getTargetPosition() >= 0 && motor.getCurrentPosition() >= motor.getTargetPosition())) { motor.setPower(0); - } else { - motor.setPower(motorRunPower); + } else if (motor.getPower() == 0) { // If not in position and power is zero, set power. + if (motor.getTargetPosition() > 0){ + motor.setPower(motorRunPower); + } else if (motor.getTargetPosition() > 0) { + motor.setPower(-motorRunPower); + } + } + + // Catch any motors running the wrong way. + if (motor.getTargetPosition() < motor.getCurrentPosition() && motor.getCurrentPosition() > 0 || + motor.getTargetPosition() > motor.getCurrentPosition() && motor.getCurrentPosition() < 0) { + motor.setPower(-motor.getPower()); } } From f3c35d2483de753c34e4f5e2d0582819328b0826 Mon Sep 17 00:00:00 2001 From: RZW-13 Date: Tue, 30 Jan 2024 17:04:44 -0700 Subject: [PATCH 5/5] renamed folder --- .../teamcode/auton/tests/AutonWheelsTest.java | 58 +++++++++++++++++++ .../auton/tests/RobotMethodsTest.java | 2 - .../{Tests => tests}/BasicMotorTest.java | 2 +- .../teleop/{Tests => tests}/CRServoTest.java | 0 .../teleop/{Tests => tests}/ServoTest.java | 0 5 files changed, 59 insertions(+), 3 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/AutonWheelsTest.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/{Tests => tests}/BasicMotorTest.java (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/{Tests => tests}/CRServoTest.java (100%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/{Tests => tests}/ServoTest.java (100%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/AutonWheelsTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/AutonWheelsTest.java new file mode 100644 index 00000000..6b2036d1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/AutonWheelsTest.java @@ -0,0 +1,58 @@ +package org.firstinspires.ftc.teamcode.auton.tests; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware; +import org.firstinspires.ftc.teamcode.subsystems.hardware.HardwareElement; +import org.firstinspires.ftc.teamcode.subsystems.robotMethods.RobotMethods; +import org.firstinspires.ftc.teamcode.subsystems.stateMachineController.Context; + +@Autonomous(name = "Auton Wheels Test") +public class AutonWheelsTest extends OpMode { + Hardware robot; + Context context; + double motorRunPower = 0.5; + + @Override + public void init() { + telemetry.addData("Status", "Initializing"); + + robot = new Hardware(hardwareMap, telemetry); + context = new Context(); + + // Init DcMotors. + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftFront", + "setDirection:REVERSE")); + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "leftRear", + "setDirection:REVERSE")); + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightFront")); + robot.introduce(new HardwareElement<>(DcMotor.class, hardwareMap, "rightRear")); + + telemetry.addData("Status", "Initialized"); + } + + @Override + public void start() { + RobotMethods.setTargetPosition(robot.get("leftFront"), 300); + RobotMethods.setTargetPosition(robot.get("leftRear"), 300); + RobotMethods.setTargetPosition(robot.get("rightFront"), 300); + RobotMethods.setTargetPosition(robot.get("rightRear"), 300); + } + + @Override + public void loop() { + if (gamepad1.a) { + robot.get("leftFront").setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.get("leftRear").setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.get("rightFront").setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.get("rightRear").setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + } else { + RobotMethods.updateMotor(telemetry, robot.get("leftFront"), motorRunPower); + RobotMethods.updateMotor(telemetry, robot.get("leftRear"), motorRunPower); + RobotMethods.updateMotor(telemetry, robot.get("rightFront"), motorRunPower); + RobotMethods.updateMotor(telemetry, robot.get("rightRear"), motorRunPower); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java index ff985add..7290ab1e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/tests/RobotMethodsTest.java @@ -3,7 +3,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.robot.Robot; import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware; import org.firstinspires.ftc.teamcode.subsystems.hardware.HardwareElement; @@ -15,7 +14,6 @@ public class RobotMethodsTest extends OpMode { Hardware robot; Context context; double motorRunPower = 0.5; - double tolerance = 0; @Override public void init() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Tests/BasicMotorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/tests/BasicMotorTest.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Tests/BasicMotorTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/tests/BasicMotorTest.java index e76f96af..2dbf419b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Tests/BasicMotorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/tests/BasicMotorTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.teleop.Tests; +package org.firstinspires.ftc.teamcode.teleop.tests; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Tests/CRServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/tests/CRServoTest.java similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Tests/CRServoTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/tests/CRServoTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Tests/ServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/tests/ServoTest.java similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Tests/ServoTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/tests/ServoTest.java