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