From 198ea7c9c498d79a035615c13be05687d3986e9e Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Tue, 21 Nov 2023 17:39:50 -0600 Subject: [PATCH] Pizza bot autonomous, only wants to veer right no matter what I say, any ideas? --- .../Engines/SodiPizzaAutoRedRightEngine.java | 2 + .../States/SodiPizzaAutoArmState.java | 21 ++++++ .../States/SodiPizzaAutoDriveState.java | 65 +++++++++++++++---- .../Autonomous/States/SodiPizzaWheelTest.java | 23 +++++++ .../Common/SodiPizzaMinibotObject.java | 32 ++++++--- 5 files changed, 121 insertions(+), 22 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoArmState.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java index 7691329..236379d 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java @@ -2,6 +2,7 @@ package org.timecrafters.CenterStage.Autonomous.Engines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoArmState; import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoDriveState; import dev.cyberarm.engine.V2.CyberarmEngine; @@ -11,5 +12,6 @@ public class SodiPizzaAutoRedRightEngine extends CyberarmEngine { @Override public void setup() { addState(new SodiPizzaAutoDriveState()); + addState(new SodiPizzaAutoArmState()); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoArmState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoArmState.java new file mode 100644 index 0000000..f9651a3 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoArmState.java @@ -0,0 +1,21 @@ +package org.timecrafters.CenterStage.Autonomous.States; + +import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class SodiPizzaAutoArmState extends CyberarmState { + + private SodiPizzaMinibotObject robot; + private String groupName, actionName; + + public void SodiPizzaAutoDriveState() { + groupName = " "; + actionName = " "; + robot = new SodiPizzaMinibotObject(); + robot.setup(); + } + @Override + public void exec() { + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoDriveState.java index 08ebd70..13cdbff 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoDriveState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoDriveState.java @@ -1,5 +1,7 @@ package org.timecrafters.CenterStage.Autonomous.States; +import com.qualcomm.robotcore.hardware.DcMotor; + import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; import dev.cyberarm.engine.V2.CyberarmState; @@ -7,6 +9,7 @@ import dev.cyberarm.engine.V2.CyberarmState; public class SodiPizzaAutoDriveState extends CyberarmState{ final private SodiPizzaMinibotObject robot; final private String groupName, actionName; + private long lastMoveTime; public SodiPizzaAutoDriveState() { groupName = " "; @@ -15,22 +18,56 @@ public class SodiPizzaAutoDriveState extends CyberarmState{ robot.setup(); } + @Override + public void start() { + + } + + @Override + public void telemetry() { + engine.telemetry.addData("Target Ticks?", robot.leftFront.getTargetPosition()); + engine.telemetry.addData("Current Ticks?", robot.leftFront.getCurrentPosition()); + engine.telemetry.addData("Ticks Needed?", robot.leftFront.getTargetPosition() - + robot.leftFront.getCurrentPosition()); + } + @Override public void exec() { - robot.flDrive.setTargetPosition(2800); - if (robot.flDrive.getCurrentPosition() < robot.flDrive.getTargetPosition()) { - robot.flDrive.setPower(0.75); - robot.frDrive.setPower(0.75); - robot.blDrive.setPower(0.75); - robot.brDrive.setPower(0.75); - } else { - robot.flDrive.setPower(0); - robot.frDrive.setPower(0); - robot.blDrive.setPower(0); - robot.brDrive.setPower(0); + + if (robot.leftFront.getCurrentPosition() < 1000) { + + robot.leftFront.setTargetPosition(1000); + robot.leftBack.setTargetPosition(1000); + robot.rightFront.setTargetPosition(1000); + robot.rightBack.setTargetPosition(1000); + + robot.leftFront.setPower(0.5); + robot.rightFront.setPower(0.5); + robot.leftBack.setPower(0.5); + robot.rightBack.setPower(0.5); + } - if (robot.flDrive.getPower() == 0 && robot.flDrive.getCurrentPosition() >= robot.flDrive.getTargetPosition()) { - setHasFinished(true); - } + + if (robot.leftFront.getCurrentPosition() < 1250 && robot.leftFront.getCurrentPosition() >= 1000 && + robot.rightBack.getCurrentPosition() > 750) { + robot.leftFront.setPower(0.5); + robot.leftBack.setPower(0.5); + robot.rightFront.setPower(-0.5); + robot.rightBack.setPower(-0.5); + + robot.leftFront.setTargetPosition(1250); + robot.leftBack.setTargetPosition(1250); + robot.rightFront.setTargetPosition(750); + robot.rightBack.setTargetPosition(750); + } + + + if (robot.leftFront.getCurrentPosition() >= 1250 && robot.rightBack.getCurrentPosition() <= 750) { + robot.leftFront.setPower(0); + robot.rightFront.setPower(0); + robot.leftBack.setPower(0); + robot.rightBack.setPower(0); + setHasFinished(true); } } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java new file mode 100644 index 0000000..0429d64 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java @@ -0,0 +1,23 @@ +package org.timecrafters.CenterStage.Autonomous.States; + +import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class SodiPizzaWheelTest extends CyberarmState { + private SodiPizzaMinibotObject robot; + private String groupName, actionName; + + public SodiPizzaWheelTest() { + groupName = " "; + actionName = " "; + robot = new SodiPizzaMinibotObject(); + robot.setup(); + } + + @Override + public void exec() { + + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java index ead9895..98fcf53 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java @@ -1,6 +1,7 @@ package org.timecrafters.CenterStage.Common; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; @@ -13,8 +14,8 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class SodiPizzaMinibotObject extends Robot { public HardwareMap hardwareMap; - public DcMotor flDrive, frDrive, blDrive, brDrive; - public Servo shoulder, hand; + public DcMotor leftFront, rightFront, leftBack, rightBack; + public Servo shoulder, gripper; public IMU imu; private String string; @@ -42,14 +43,29 @@ public class SodiPizzaMinibotObject extends Robot { this.hardwareMap = CyberarmEngine.instance.hardwareMap; //Motor defining - flDrive = engine.hardwareMap.dcMotor.get("FL Drive"); - frDrive = engine.hardwareMap.dcMotor.get("FR Drive"); - blDrive = engine.hardwareMap.dcMotor.get("BL Drive"); - brDrive = engine.hardwareMap.dcMotor.get("BR Drive"); + leftFront = engine.hardwareMap.dcMotor.get("leftFront"); + rightFront = engine.hardwareMap.dcMotor.get("rightFront"); + leftBack = engine.hardwareMap.dcMotor.get("leftBack"); + rightBack = engine.hardwareMap.dcMotor.get("rightBack"); + + leftFront.setDirection(DcMotorSimple.Direction.FORWARD); + leftBack.setDirection(DcMotorSimple.Direction.FORWARD); + rightFront.setDirection(DcMotorSimple.Direction.REVERSE); + rightBack.setDirection(DcMotorSimple.Direction.REVERSE); + + leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); //Servo Defining - shoulder = engine.hardwareMap.servo.get("Shoulder"); - hand = engine.hardwareMap.servo.get("Hand"); + shoulder = engine.hardwareMap.servo.get("arm"); + gripper = engine.hardwareMap.servo.get("gripper"); } }