From d3b6ac873e8b8208619577567dfbe78bff71281a Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Mon, 30 Oct 2023 18:21:58 -0500 Subject: [PATCH] Minibot drive test:Added stop and reset encoder on all motors, need to work on game strategy --- .../Autonomous/States/ProtoBotStateSodi.java | 57 +++++++++---------- .../CenterStage/Common/ProtoBotSodi.java | 15 +++-- 2 files changed, 38 insertions(+), 34 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java index 182ff88..5384597 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java @@ -1,5 +1,6 @@ package org.timecrafters.CenterStage.Autonomous.States; +import com.arcrobotics.ftclib.hardware.motors.Motor; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; @@ -28,15 +29,23 @@ public class ProtoBotStateSodi extends CyberarmState { engine.telemetry.addData("Front Right Power", robot.frDrive.motor.getPower()); engine.telemetry.addData("Back Left Power", robot.blDrive.motor.getPower()); engine.telemetry.addData("Back Right Power", robot.brDrive.motor.getPower()); + engine.telemetry.addData("FL Position",robot.flDrive.motor.getCurrentPosition()); + engine.telemetry.addData("Test Sequence", testSequence); } + @Override + public void start() { + testSequence = 0; + } + @Override public void init() { robot.flDrive.motor.setPower(0); robot.frDrive.motor.setPower(0); robot.blDrive.motor.setPower(0); robot.brDrive.motor.setPower(0); + // robot.liftMotor.motor.setPower(0); // // robot.grabJaw.setPosition(0); @@ -47,7 +56,7 @@ public class ProtoBotStateSodi extends CyberarmState { // robot.dropJaw.setPosition(0); lastTimeChecked = System.currentTimeMillis(); - testSequence = 0; + @@ -63,37 +72,27 @@ public class ProtoBotStateSodi extends CyberarmState { testSequence = 1; } - switch (testSequence) { - case 1: - - robot.flDrive.motor.setTargetPosition(500); - robot.frDrive.motor.setTargetPosition(500); - robot.blDrive.motor.setTargetPosition(500); - robot.brDrive.motor.setTargetPosition(500); - - if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() - 50) { - robot.flDrive.motor.setPower(0.5 * (robot.flDrive.motor.getTargetPosition() - robot.flDrive.motor.getCurrentPosition())); - robot.frDrive.motor.setPower(0.5 * (robot.frDrive.motor.getTargetPosition() - robot.frDrive.motor.getCurrentPosition())); - robot.blDrive.motor.setPower(0.5 * (robot.blDrive.motor.getTargetPosition() - robot.blDrive.motor.getCurrentPosition())); - robot.brDrive.motor.setPower(0.5 * (robot.brDrive.motor.getTargetPosition() - robot.brDrive.motor.getCurrentPosition())); - } - else if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() + 50 || - robot.flDrive.motor.getCurrentPosition() > robot.flDrive.motor.getTargetPosition() - 50) { - - robot.flDrive.motor.setPower(0); - robot.frDrive.motor.setPower(0); - robot.blDrive.motor.setPower(0); - robot.brDrive.motor.setPower(0); - -// testSequence = 2; - } - - break; -// case 2: - + if (testSequence == 1) { + robot.flDrive.motor.setTargetPosition(1000); + robot.frDrive.motor.setTargetPosition(1000); + robot.blDrive.motor.setTargetPosition(1000); + robot.brDrive.motor.setTargetPosition(1000); + if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() - 50) { + robot.flDrive.motor.setPower(0.5 * (robot.flDrive.motor.getTargetPosition() - robot.flDrive.motor.getCurrentPosition())); + robot.frDrive.motor.setPower(0.5 * (robot.frDrive.motor.getTargetPosition() - robot.frDrive.motor.getCurrentPosition())); + robot.blDrive.motor.setPower(0.5 * (robot.blDrive.motor.getTargetPosition() - robot.blDrive.motor.getCurrentPosition())); + robot.brDrive.motor.setPower(0.5 * (robot.brDrive.motor.getTargetPosition() - robot.brDrive.motor.getCurrentPosition())); + } else if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() + 50 || + robot.flDrive.motor.getCurrentPosition() > robot.flDrive.motor.getTargetPosition() - 50) { + robot.flDrive.motor.setPower(0); + robot.frDrive.motor.setPower(0); + robot.blDrive.motor.setPower(0); + robot.brDrive.motor.setPower(0); + testSequence = 2; + } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java index 8630b1d..ee6b8a0 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java @@ -52,11 +52,16 @@ public class ProtoBotSodi extends Robot { blDrive = new MotorEx(hardwareMap, "BackLeft"); // liftMotor = new MotorEx(hardwareMap, "Lift"); -// flDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); -// frDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); -// blDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); -// brDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); -// liftMotor.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + flDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + blDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + brDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); +// liftMotor.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); flDrive.motor.setDirection(FORWARD); // frDrive.motor.setDirection(REVERSE);