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..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 @@ -1,10 +1,21 @@ 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.SodiPizzaAutoFirstDriveState; +import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoTurnState; +import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaWheelTest; + 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 SodiPizzaAutoFirstDriveState()); + addState(new SodiPizzaAutoTurnState()); +// addState(new SodiPizzaAutoArmState()); +// addState(new SodiPizzaWheelTest()); } } 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/SodiPizzaAutoFirstDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java new file mode 100644 index 0000000..b9a40bd --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java @@ -0,0 +1,122 @@ +package org.timecrafters.CenterStage.Autonomous.States; + +import android.annotation.SuppressLint; + +import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; + +import dev.cyberarm.engine.V2.CyberarmState; + +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 = " "; + actionName = " "; + robot = new SodiPizzaMinibotObject(); + robot.setup(); + } + + @SuppressLint("SuspiciousIndentation") + + @Override + public void start() { + + lastHalf = false; + lastMoveTime = System.currentTimeMillis(); + robot.readyToTurn = 0; + + } + + @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() { + // 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); + + drivePower = 0.5; + + robot.leftFront.setPower(drivePower); + robot.leftBack.setPower(drivePower); + robot.rightFront.setPower(drivePower); + robot.rightBack.setPower(drivePower); + + } + //Stop and finish set after return to 0 + else if (robot.leftFront.getCurrentPosition() >= targetPos - 10 && robot.leftFront.getCurrentPosition() <= targetPos + 10) { + + 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/Autonomous/States/SodiPizzaWheelTest.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java new file mode 100644 index 0000000..87bcf10 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java @@ -0,0 +1,93 @@ +package org.timecrafters.CenterStage.Autonomous.States; + +import android.annotation.SuppressLint; + +import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; + +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 = " "; + actionName = " "; + 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 8c84712..9421567 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,11 +14,25 @@ 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; + public int readyToTurn; + + 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; @@ -30,11 +45,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.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); + 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("arm"); + gripper = engine.hardwareMap.servo.get("gripper"); } }