From 611c6483e50ff224eabd49402eea97d25675653b Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Thu, 16 Nov 2023 20:41:55 -0600 Subject: [PATCH 1/4] Began Pizza bot autonomous --- .../Engines/SodiPizzaAutoRedRightEngine.java | 7 +++- .../States/SodiPizzaAutoDriveState.java | 36 +++++++++++++++++++ .../Common/SodiPizzaMinibotObject.java | 15 ++++++++ 3 files changed, 57 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoDriveState.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 df321ed..7691329 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 @@ -1,10 +1,15 @@ package org.timecrafters.CenterStage.Autonomous.Engines; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoDriveState; + import dev.cyberarm.engine.V2.CyberarmEngine; +@Autonomous(name = "Sodi's Pizza Box Bot Auto", group = "") public class SodiPizzaAutoRedRightEngine extends CyberarmEngine { @Override public void setup() { - + addState(new SodiPizzaAutoDriveState()); } } 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 new file mode 100644 index 0000000..08ebd70 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoDriveState.java @@ -0,0 +1,36 @@ +package org.timecrafters.CenterStage.Autonomous.States; + +import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class SodiPizzaAutoDriveState extends CyberarmState{ + final private SodiPizzaMinibotObject robot; + final private String groupName, actionName; + + public SodiPizzaAutoDriveState() { + groupName = " "; + actionName = " "; + robot = new SodiPizzaMinibotObject(); + robot.setup(); + } + + @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.flDrive.getPower() == 0 && robot.flDrive.getCurrentPosition() >= robot.flDrive.getTargetPosition()) { + setHasFinished(true); + } + } + } 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 8c84712..ead9895 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java @@ -18,6 +18,18 @@ public class SodiPizzaMinibotObject extends Robot { public IMU imu; private String string; + public static double GRIPPER_CLOSED = 0.333; // ~90 degrees + public static double GRIPPER_OPEN = 0.75; // ~205 degrees + + public static double ARM_COLLECT = 0.0; // ~? degrees + public static double ARM_PRECOLLECT = 0.05; // ~? degrees + public static double ARM_DELIVER = 0.28; // ~? degrees + public static double ARM_STOW = 0.72; // ~? degrees + public static double ARM_HOVER_5_STACK = 0.10; + public static double ARM_HOVER_4_STACK = 0.08; + public static double ARM_HOVER_3_STACK = 0.07; + public static double ARM_HOVER_2_STACK = 0.06; + private CyberarmEngine engine; public TimeCraftersConfiguration configuration; @@ -35,6 +47,9 @@ public class SodiPizzaMinibotObject extends Robot { blDrive = engine.hardwareMap.dcMotor.get("BL Drive"); brDrive = engine.hardwareMap.dcMotor.get("BR Drive"); + //Servo Defining + shoulder = engine.hardwareMap.servo.get("Shoulder"); + hand = engine.hardwareMap.servo.get("Hand"); } } 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 2/4] 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"); } } From 43e59d8f6d21c4bca8b6d5fe540d9b738fdde39d Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Thu, 23 Nov 2023 18:46:46 -0600 Subject: [PATCH 3/4] Who last configured Pizzabot? >:( Anyway Pizza bot autonomous testing has been mostly resolved. --- .../Engines/SodiPizzaAutoRedRightEngine.java | 8 ++- ...java => SodiPizzaAutoFirstDriveState.java} | 27 ++----- .../Autonomous/States/SodiPizzaWheelTest.java | 72 ++++++++++++++++++- .../Common/SodiPizzaMinibotObject.java | 8 +-- 4 files changed, 85 insertions(+), 30 deletions(-) rename TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/{SodiPizzaAutoDriveState.java => SodiPizzaAutoFirstDriveState.java} (65%) 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 236379d..2d796ca 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 @@ -3,7 +3,8 @@ 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 org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoFirstDriveState; +import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaWheelTest; import dev.cyberarm.engine.V2.CyberarmEngine; @@ -11,7 +12,8 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class SodiPizzaAutoRedRightEngine extends CyberarmEngine { @Override public void setup() { - addState(new SodiPizzaAutoDriveState()); - addState(new SodiPizzaAutoArmState()); +// addState(new SodiPizzaAutoFirstDriveState()); +// addState(new SodiPizzaAutoArmState()); + addState(new SodiPizzaWheelTest()); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java similarity index 65% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoDriveState.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java index 13cdbff..aa6d0a8 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoDriveState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java @@ -1,17 +1,15 @@ package org.timecrafters.CenterStage.Autonomous.States; -import com.qualcomm.robotcore.hardware.DcMotor; - import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; import dev.cyberarm.engine.V2.CyberarmState; -public class SodiPizzaAutoDriveState extends CyberarmState{ +public class SodiPizzaAutoFirstDriveState extends CyberarmState{ final private SodiPizzaMinibotObject robot; final private String groupName, actionName; private long lastMoveTime; - public SodiPizzaAutoDriveState() { + public SodiPizzaAutoFirstDriveState() { groupName = " "; actionName = " "; robot = new SodiPizzaMinibotObject(); @@ -46,27 +44,12 @@ public class SodiPizzaAutoDriveState extends CyberarmState{ robot.leftBack.setPower(0.5); robot.rightBack.setPower(0.5); - } - - 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) { + } else { robot.leftFront.setPower(0); - robot.rightFront.setPower(0); robot.leftBack.setPower(0); + robot.rightFront.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 index 0429d64..87bcf10 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java @@ -1,5 +1,7 @@ package org.timecrafters.CenterStage.Autonomous.States; +import android.annotation.SuppressLint; + import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; import dev.cyberarm.engine.V2.CyberarmState; @@ -7,6 +9,8 @@ import dev.cyberarm.engine.V2.CyberarmState; public class SodiPizzaWheelTest extends CyberarmState { private SodiPizzaMinibotObject robot; private String groupName, actionName; + private long lastMoveTime; + private double lDrivePower, rDrivePower; public SodiPizzaWheelTest() { groupName = " "; @@ -14,10 +18,76 @@ public class SodiPizzaWheelTest extends CyberarmState { robot = new SodiPizzaMinibotObject(); robot.setup(); } + + private double getrDrivePower() { + rDrivePower = lDrivePower * 0.75; + return rDrivePower; + } + @Override + public void telemetry() { + + engine.telemetry.addData("Current Pos? ", robot.leftFront.getCurrentPosition()); + engine.telemetry.addData("Last Moved Time? ", lastMoveTime); + engine.telemetry.addData("System Current Time? ", System.currentTimeMillis()); + + engine.telemetry.addData("Diff in Right/Left Front Wheel Power? ", robot.rightFront.getPower() - robot.leftFront.getPower()); + } + + @Override + public void start() { + + lastMoveTime = System.currentTimeMillis(); + lDrivePower = 0; + rDrivePower = 0; + robot.leftFront.setPower(lDrivePower); + robot.leftBack.setPower(lDrivePower); + robot.rightFront.setPower(rDrivePower); + robot.rightBack.setPower(rDrivePower); + + } + + @SuppressLint("SuspiciousIndentation") @Override public void exec() { - +/* lDrivePower = 0.3; + robot.leftFront.setPower(lDrivePower); + robot.leftBack.setPower(lDrivePower); + robot.rightFront.setPower(lDrivePower); + robot.rightBack.setPower(lDrivePower); +*/ + if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10 && System.currentTimeMillis() - lastMoveTime >= 3000) { + robot.leftFront.setTargetPosition(500); + robot.leftBack.setTargetPosition(500); + robot.rightFront.setTargetPosition(500); + robot.rightBack.setTargetPosition(500); + + lDrivePower = 0.3; + rDrivePower = lDrivePower * 0.75; + + robot.leftFront.setPower(lDrivePower); + robot.leftBack.setPower(lDrivePower); + robot.rightFront.setPower(rDrivePower); + robot.rightBack.setPower(rDrivePower); + + lastMoveTime = System.currentTimeMillis(); + + } else if (robot.leftFront.getCurrentPosition() >= 500 && System.currentTimeMillis() - lastMoveTime >= 3000) { + robot.leftFront.setTargetPosition(0); + robot.leftBack.setTargetPosition(0); + robot.rightFront.setTargetPosition(0); + robot.rightBack.setTargetPosition(0); + + lDrivePower = -0.3; + rDrivePower = lDrivePower * 0.75; + + robot.leftFront.setPower(lDrivePower); + robot.leftBack.setPower(lDrivePower); + robot.rightFront.setPower(rDrivePower); + robot.rightBack.setPower(rDrivePower); + + lastMoveTime = System.currentTimeMillis(); + } } } 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 98fcf53..238812f 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java @@ -48,10 +48,10 @@ public class SodiPizzaMinibotObject extends Robot { 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.setDirection(DcMotorSimple.Direction.REVERSE); + leftBack.setDirection(DcMotorSimple.Direction.REVERSE); + rightFront.setDirection(DcMotorSimple.Direction.FORWARD); + rightBack.setDirection(DcMotorSimple.Direction.FORWARD); leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); From fe2c3de40b3418bbe7810d0fe19e308ec718bb06 Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Fri, 24 Nov 2023 15:10:58 -0600 Subject: [PATCH 4/4] Added turning with IMU, config angry :( --- .../Engines/SodiPizzaAutoRedRightEngine.java | 6 +- .../States/SodiPizzaAutoFirstDriveState.java | 94 ++++++++++++++++--- .../States/SodiPizzaAutoTurnState.java | 71 ++++++++++++++ .../Common/SodiPizzaMinibotObject.java | 2 + 4 files changed, 157 insertions(+), 16 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.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 2d796ca..a491415 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 @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoArmState; import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoFirstDriveState; +import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoTurnState; import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaWheelTest; import dev.cyberarm.engine.V2.CyberarmEngine; @@ -12,8 +13,9 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class SodiPizzaAutoRedRightEngine extends CyberarmEngine { @Override public void setup() { -// addState(new SodiPizzaAutoFirstDriveState()); + addState(new SodiPizzaAutoFirstDriveState()); + addState(new SodiPizzaAutoTurnState()); // addState(new SodiPizzaAutoArmState()); - addState(new SodiPizzaWheelTest()); +// addState(new SodiPizzaWheelTest()); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java index aa6d0a8..b9a40bd 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java @@ -1,5 +1,7 @@ package org.timecrafters.CenterStage.Autonomous.States; +import android.annotation.SuppressLint; + import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; import dev.cyberarm.engine.V2.CyberarmState; @@ -8,6 +10,9 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ final private SodiPizzaMinibotObject robot; final private String groupName, actionName; private long lastMoveTime; + private int targetPos = 2500; + private double drivePower; + private boolean lastHalf; public SodiPizzaAutoFirstDriveState() { groupName = " "; @@ -15,10 +20,16 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ robot = new SodiPizzaMinibotObject(); robot.setup(); } + + @SuppressLint("SuspiciousIndentation") @Override public void start() { + lastHalf = false; + lastMoveTime = System.currentTimeMillis(); + robot.readyToTurn = 0; + } @Override @@ -31,26 +42,81 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{ @Override public void exec() { + // Move forward from 0 to targetPos + if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10 + && !lastHalf) { + robot.leftFront.setTargetPosition(targetPos); + robot.leftBack.setTargetPosition(targetPos); + robot.rightFront.setTargetPosition(targetPos); + robot.rightBack.setTargetPosition(targetPos); - if (robot.leftFront.getCurrentPosition() < 1000) { + drivePower = 0.5; - robot.leftFront.setTargetPosition(1000); - robot.leftBack.setTargetPosition(1000); - robot.rightFront.setTargetPosition(1000); - robot.rightBack.setTargetPosition(1000); + robot.leftFront.setPower(drivePower); + robot.leftBack.setPower(drivePower); + robot.rightFront.setPower(drivePower); + robot.rightBack.setPower(drivePower); - robot.leftFront.setPower(0.5); - robot.rightFront.setPower(0.5); - robot.leftBack.setPower(0.5); - robot.rightBack.setPower(0.5); + } + //Stop and finish set after return to 0 + else if (robot.leftFront.getCurrentPosition() >= targetPos - 10 && robot.leftFront.getCurrentPosition() <= targetPos + 10) { - } else { - robot.leftFront.setPower(0); - robot.leftBack.setPower(0); - robot.rightFront.setPower(0); - robot.rightBack.setPower(0); + drivePower = 0; + + robot.leftFront.setPower(drivePower); + robot.leftBack.setPower(drivePower); + robot.rightFront.setPower(drivePower); + robot.rightBack.setPower(drivePower); + + robot.readyToTurn = 1; setHasFinished(true); } + +/* //Stop moving and update lastMoveTime + if (robot.leftFront.getCurrentPosition() >= targetPos && drivePower > 0) { + + lastHalf = true; + + drivePower = 0; + + robot.leftFront.setPower(drivePower); + robot.leftBack.setPower(drivePower); + robot.rightFront.setPower(drivePower); + robot.rightBack.setPower(drivePower); + + lastMoveTime = System.currentTimeMillis(); + } + + //Move backwards from targetPos to 0 + if (robot.leftFront.getCurrentPosition() >= targetPos && drivePower == 0 + && System.currentTimeMillis() - lastMoveTime >= 500 && lastHalf) { + + robot.leftFront.setTargetPosition(0); + robot.leftBack.setTargetPosition(0); + robot.rightFront.setTargetPosition(0); + robot.rightBack.setTargetPosition(0); + + drivePower = -0.5; + + robot.leftFront.setPower(drivePower); + robot.leftBack.setPower(drivePower); + robot.rightFront.setPower(drivePower); + robot.rightBack.setPower(drivePower); + } */ + + /* if (robot.leftFront.getCurrentPosition() < 1250 && robot.leftFront.getCurrentPosition() >= targetPos && + 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); + } +*/ } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java new file mode 100644 index 0000000..3a31fe5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java @@ -0,0 +1,71 @@ +package org.timecrafters.CenterStage.Autonomous.States; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class SodiPizzaAutoTurnState extends CyberarmState { + final private SodiPizzaMinibotObject robot = new SodiPizzaMinibotObject(); + final private String groupName, actionName; + private long lastMoveTime; + private double turnSpeedRaw, turnSpeed; + private int startPos; + private double targetRot; + private double currentRot = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + private double neededRot = targetRot - currentRot; + /** Rot = rotation **/ + + public SodiPizzaAutoTurnState() { + groupName = " "; + actionName = " "; + robot.setup(); + } + + private double getTurnSpeed() { + if (Math.abs(neededRot) > 5) { + turnSpeed = turnSpeedRaw * neededRot / 10; + } + return turnSpeed; + } + + @Override + public void start() { + + startPos = robot.leftFront.getCurrentPosition(); + turnSpeedRaw = 0; + + robot.leftFront.setPower(turnSpeed); + robot.leftBack.setPower(turnSpeed); + robot.rightFront.setPower(turnSpeed); + robot.rightBack.setPower(turnSpeed); + + robot.imu.resetYaw(); + } + + + @Override + public void exec() { + + if (robot.readyToTurn == 1 && robot.leftFront.getCurrentPosition() == startPos && Math.abs(neededRot) > 10) { + + targetRot = currentRot + 90; + + turnSpeedRaw = 0.3; + + robot.leftFront.setPower(turnSpeed); + robot.leftBack.setPower(turnSpeed); + robot.rightFront.setPower(-turnSpeed); + robot.rightBack.setPower(-turnSpeed); + + } else if (robot.readyToTurn == 1 && Math.abs(neededRot) < 10) { + turnSpeedRaw = 0; + robot.leftFront.setPower(turnSpeed); + robot.leftBack.setPower(turnSpeed); + robot.rightFront.setPower(turnSpeed); + robot.rightBack.setPower(turnSpeed); + } + + } +} 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 238812f..9421567 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java @@ -19,6 +19,8 @@ public class SodiPizzaMinibotObject extends Robot { public IMU imu; private String string; + public int readyToTurn; + public static double GRIPPER_CLOSED = 0.333; // ~90 degrees public static double GRIPPER_OPEN = 0.75; // ~205 degrees