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"); } }