diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java index 9e5ed10..9fd1f9e 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java @@ -3,6 +3,7 @@ package org.timecrafters.CenterStage.Autonomous.Engines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import org.timecrafters.CenterStage.Autonomous.States.AutoStateScrimmage; import org.timecrafters.CenterStage.Autonomous.States.DepositorArmPosState; import org.timecrafters.CenterStage.Common.PrototypeRobot; import org.timecrafters.CenterStage.Common.ServoTestRobot; @@ -10,17 +11,16 @@ import org.timecrafters.CenterStage.TeleOp.States.PrototypeRobotDrivetrainState; import dev.cyberarm.engine.V2.CyberarmEngine; -@Autonomous(name = "Servo Test", group = "Testing") +@Autonomous(name = "Scrimmage Auto", group = "PROTOTYPE", preselectTeleOp = "Prototype Robot") public class ServoTestEngine extends CyberarmEngine { - ServoTestRobot robot; + PrototypeRobot robot; @Override public void setup() { - this.robot = new ServoTestRobot("Hello World"); + this.robot = new PrototypeRobot("Hello World"); this.robot.setup(); - addState(new DepositorArmPosState(robot, "ServoPositions", "01-0")); - + addState(new AutoStateScrimmage(robot)); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateScrimmage.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateScrimmage.java new file mode 100644 index 0000000..d0ebc50 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateScrimmage.java @@ -0,0 +1,103 @@ +package org.timecrafters.CenterStage.Autonomous.States; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.timecrafters.CenterStage.Common.PrototypeRobot; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class AutoStateScrimmage extends CyberarmState { + + private int distanceDriven; + private int targetDistance = 38000; + private int stepsFinished = 0; + private boolean firstDrivePos = false; + private boolean secondDrivePos = false; + private boolean armToFIrstPos = false; + private boolean armToSecondPos = false; + private boolean collect = false; + PrototypeRobot robot; + public AutoStateScrimmage(PrototypeRobot robot) { + this.robot = robot; + } + + @Override + public void start() { + + robot.odometerR.setMode(DcMotor.RunMode.RESET_ENCODERS); + } + + @Override + public void exec() { + distanceDriven = robot.currentRightPosition; + + if (stepsFinished == 5) { +// setHasFinished(true); + } + + if (robot.odometerR.getCurrentPosition() >= targetDistance && firstDrivePos == false){ + robot.frontRight.setPower(0); + robot.frontLeft.setPower(0); + robot.backRight.setPower(0); + robot.backLeft.setPower(0); + stepsFinished += 1; + firstDrivePos = true; + robot.startOfSequencerTime = System.currentTimeMillis(); + } else if (robot.odometerR.getCurrentPosition() < targetDistance){ + robot.frontRight.setPower(0.5); + robot.frontLeft.setPower(0.5); + robot.backRight.setPower(0.5); + robot.backLeft.setPower(0.5); + } + + if (stepsFinished == 1 && firstDrivePos && armToFIrstPos == false){ + robot.armPosition = 2; + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) { + robot.oldArmPosition = 2; + armToFIrstPos = true; + } + } + } + + if (firstDrivePos && armToFIrstPos){ + robot.collector.setPosition(1F); + collect = true; + } + +// if (stepsFinished == 3 && firstDrivePos && armToFIrstPos && collect && secondDrivePos == false){ +// if (robot.odometerR.getCurrentPosition() <= targetDistance - 500){ +// robot.frontRight.setPower(0); +// robot.frontLeft.setPower(0); +// robot.backRight.setPower(0); +// robot.backLeft.setPower(0); +// stepsFinished += 1; +// robot.startOfSequencerTime = System.currentTimeMillis(); +// secondDrivePos = true; +// } else { +// robot.frontRight.setPower(-0.3); +// robot.frontLeft.setPower(-0.3); +// robot.backRight.setPower(-0.3); +// robot.backLeft.setPower(-0.3); +// } +// } + if (firstDrivePos && armToFIrstPos && collect){ + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { + robot.oldArmPosition = 0; + } + } + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("current pos", robot.odometerR.getCurrentPosition()); + engine.telemetry.addData("time", System.currentTimeMillis() - robot.startOfSequencerTime); + } +} + diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java index 399d154..3d7ca10 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java @@ -122,6 +122,7 @@ public class PrototypeRobot extends Robot { backRight = engine.hardwareMap.dcMotor.get("backRight"); backLeft = engine.hardwareMap.dcMotor.get("backLeft"); lift = engine.hardwareMap.dcMotor.get("Lift"); + odometerR = engine.hardwareMap.dcMotor.get("odometerR"); configuration = new TimeCraftersConfiguration("Blue Crab"); @@ -134,6 +135,8 @@ public class PrototypeRobot extends Robot { lift.setDirection(DcMotorSimple.Direction.FORWARD); + + frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -159,10 +162,9 @@ public class PrototypeRobot extends Robot { depositorElbow.setPosition(DEPOSITOR_ELBOW_IN); collectorElbow.setPosition(COLLECTOR_ELBOW_IN); collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); + collector.setPosition(0F); -// depositorShoulder.setPosition(COLLECTOR_SHOULDER_IN); -// depositorElbow.setPosition(COLLECTOR_ELBOW_IN); } @@ -263,10 +265,10 @@ public class PrototypeRobot extends Robot { public void DepositorToggle(){ boolean rbs2 = engine.gamepad2.right_stick_button; if (rbs2 && !rbsVar2) { - if (depositorPos == 1F) { + if (depositorPos == 0.6F) { depositorPos = 0F; } else { - depositorPos = 1F; + depositorPos = 0.6F; } } rbsVar2 = rbs2; @@ -277,7 +279,6 @@ public class PrototypeRobot extends Robot { switch (oldArmPosition) { case 0: // transfer - break; case 1: // driving collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position @@ -287,7 +288,6 @@ public class PrototypeRobot extends Robot { oldArmPosition = 0; } } - break; case 2: // collect collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position @@ -297,7 +297,6 @@ public class PrototypeRobot extends Robot { oldArmPosition = 0; } } - break; case 3: // deposit depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position @@ -313,141 +312,139 @@ public class PrototypeRobot extends Robot { } } } - break; } - break; - case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos - switch (oldArmPosition) { - case 0: - // transfer - collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); - if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { - oldArmPosition = 1; - } - } - break; - case 1: - // drive pos - break; - case 2: - // collect - collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 600) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); - if (System.currentTimeMillis() - startOfSequencerTime >= 2100) { - oldArmPosition = 1; - } - } - break; - case 3: - // deposit - depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met - depositorElbow.setPosition(DEPOSITOR_ELBOW_IN); - if (System.currentTimeMillis() - startOfSequencerTime >= 1600) { - collectorShoulder.setPosition(COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_ELBOW_PASSIVE); - if (System.currentTimeMillis() - startOfSequencerTime >= 3100) { - oldArmPosition = 1; - } - } - } - } - break; - } - break; +// case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos +// switch (oldArmPosition) { +// case 0: +// // transfer +// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met +// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); +// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { +// oldArmPosition = 1; +// } +// } +// break; +// case 1: +// // drive pos +// break; +// case 2: +// // collect +// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - startOfSequencerTime >= 600) { // wait to move till time is met +// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); +// if (System.currentTimeMillis() - startOfSequencerTime >= 2100) { +// oldArmPosition = 1; +// } +// } +// break; +// case 3: +// // deposit +// depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met +// depositorElbow.setPosition(DEPOSITOR_ELBOW_IN); +// if (System.currentTimeMillis() - startOfSequencerTime >= 1600) { +// collectorShoulder.setPosition(COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met +// collectorElbow.setPosition(COLLECTOR_ELBOW_PASSIVE); +// if (System.currentTimeMillis() - startOfSequencerTime >= 3100) { +// oldArmPosition = 1; +// } +// } +// } +// } +// break; +// } +// break; +// +// case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos +// switch (oldArmPosition) { +// case 0: +// // transfer +// collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met +// collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT); +// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { +// oldArmPosition = 2; +// } +// } +// break; +// case 1: +// // driving +// collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met +// collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT); +// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { +// oldArmPosition = 2; +// } +// } +// break; +// case 2: +// // collect +// collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met +// collectorElbow.setPosition(COLLECTOR_ELBOW_IN); +// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { +// oldArmPosition = 2; +// } +// } +// break; +// case 3: +// // deposit +// depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met +// depositorElbow.setPosition(DEPOSITOR_ELBOW_IN); +// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { +// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met +// collectorElbow.setPosition(COLLECTOR_ELBOW_OUT); +// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { +// oldArmPosition = 2; +// } +// } +// } +// } +// break; +// } +// break; - case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos - switch (oldArmPosition) { - case 0: - // transfer - collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT); - if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { - oldArmPosition = 2; - } - } - break; - case 1: - // driving - collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT); - if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { - oldArmPosition = 2; - } - } - break; - case 2: - // collect - collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_ELBOW_IN); - if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { - oldArmPosition = 2; - } - } - break; - case 3: - // deposit - depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met - depositorElbow.setPosition(DEPOSITOR_ELBOW_IN); - if (System.currentTimeMillis() - startOfSequencerTime >= 100) { - if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_ELBOW_OUT); - if (System.currentTimeMillis() - startOfSequencerTime >= 100) { - oldArmPosition = 2; - } - } - } - } - break; - } - break; - - case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos - switch (oldArmPosition) { - case 0: - // transfer - collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); - if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { - oldArmPosition = 3; - } - } - break; - case 1: - // driving - collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 700) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); - if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { - oldArmPosition = 3; - } - } - break; - case 2: - // collect - collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_ELBOW_IN); - if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { - oldArmPosition = 3; - } - } - break; - case 3: - // deposit - break; - } - break; +// case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos +// switch (oldArmPosition) { +// case 0: +// // transfer +// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met +// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); +// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { +// oldArmPosition = 3; +// } +// } +// break; +// case 1: +// // driving +// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - startOfSequencerTime >= 700) { // wait to move till time is met +// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); +// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { +// oldArmPosition = 3; +// } +// } +// break; +// case 2: +// // collect +// collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position +// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met +// collectorElbow.setPosition(COLLECTOR_ELBOW_IN); +// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { +// oldArmPosition = 3; +// } +// } +// break; +// case 3: +// // deposit +// break; +// } +// break; } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java index ec6fbea..8dae6a0 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java @@ -1,5 +1,7 @@ package org.timecrafters.CenterStage.TeleOp.States; +import com.arcrobotics.ftclib.controller.PIDController; + import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.timecrafters.CenterStage.Common.PrototypeRobot; @@ -9,74 +11,299 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { private PrototypeRobot robot; private int maxExtension = 2000; private int minExtension = 0; + private PIDController HeadingPidController; + private double targetHeading; + private final double p = 0, i = 0, d = 0; + + private long lastCheckedTime; public PrototypeRobotDrivetrainState(PrototypeRobot robot) { this.robot = robot; + HeadingPidController = new PIDController(p, i, d); + } -// } + // } // --------------------------------------------------------------------------------------------------------- Slider control function - private void sliderTeleOp(){ - if (engine.gamepad1.right_trigger != 0){ - if (robot.lift.getCurrentPosition() >= maxExtension){ + private void SliderTeleOp() { + if (engine.gamepad2.right_trigger != 0) { + if (robot.lift.getCurrentPosition() >= maxExtension) { robot.lift.setPower(0); - } else if (robot.lift.getCurrentPosition() >= maxExtension - 200){ + } else if (robot.lift.getCurrentPosition() >= maxExtension - 200) { robot.lift.setPower(0.35); - }else { - robot.lift.setPower(engine.gamepad1.right_trigger); + } else { + robot.lift.setPower(engine.gamepad2.right_trigger); } - } else if (engine.gamepad1.left_trigger != 0){ + } else if (engine.gamepad2.left_trigger != 0) { if (robot.lift.getCurrentPosition() <= minExtension) { robot.lift.setPower(0); - } else if (robot.lift.getCurrentPosition() < 350){ + } else if (robot.lift.getCurrentPosition() < 350) { robot.lift.setPower(-0.3); - }else { - robot.lift.setPower(-engine.gamepad1.left_trigger); + } else { + robot.lift.setPower(-engine.gamepad2.left_trigger); } } else { robot.lift.setPower(0); } } + public double angleWrap(double radians) { + while (radians > Math.PI) { + radians -= 2 * Math.PI; + } + while (radians < -Math.PI){ + radians += 2 * Math.PI; + } + return radians; + } + + private void HeadingLock(){ + HeadingPidController.setPID(p, i, d); + double currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + double pid = HeadingPidController.calculate(currentHeading, targetHeading); + double r = pid; + + } + + @Override + public void init() { + HeadingPidController = new PIDController(p, i, d); + } @Override public void exec() { - if (engine.gamepad2.a){ + + if (engine.gamepad1.right_stick_button) { + robot.imu.resetYaw(); + } + + if (engine.gamepad2.a) { robot.armPosition = 0; - } else if (engine.gamepad2.x){ + robot.startOfSequencerTime = System.currentTimeMillis(); + } else if (engine.gamepad2.x) { robot.armPosition = 1; - } else if (engine.gamepad2.b){ + robot.startOfSequencerTime = System.currentTimeMillis(); + } else if (engine.gamepad2.b) { robot.armPosition = 2; - } else if (engine.gamepad2.y){ + robot.startOfSequencerTime = System.currentTimeMillis(); + } else if (engine.gamepad2.y) { robot.armPosition = 3; + robot.startOfSequencerTime = System.currentTimeMillis(); } robot.depositor.setPosition(robot.depositorPos); robot.collector.setPosition(robot.collectorPos); - // drivetrain - robot.driveTrainTeleOp(); + // arm sequencer - robot.ArmSequences(); - // lift - sliderTeleOp(); - // collector depositor - robot.CollectorToggle(); - // depositor toggle - robot.DepositorToggle(); +// robot.ArmSequences(); + switch (robot.armPosition) { + case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos + switch (robot.oldArmPosition) { + case 0: + // transfer + robot.oldArmPosition = 0; + break; + case 1: + // driving + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); + robot.oldArmPosition = 0; + } + break; + case 2: + // collect + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { + robot.oldArmPosition = 0; + } + } + break; + case 3: + // deposit + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); // drive the shoulder to the out position + robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met + robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1100) { + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { // wait to move till time is met + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) { + robot.oldArmPosition = 0; + } + } + } + } + break; + } + break; + case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos + switch (robot.oldArmPosition) { + case 0: + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { + robot.oldArmPosition = 1; + } + } + break; + case 1: + // drive pos + robot.oldArmPosition = 1; + + break; + case 2: + // collect + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 900) { // wait to move till time is met + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { + robot.oldArmPosition = 1; + } + } + break; + case 3: + // deposit + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT - 0.1); // drive the shoulder to the out position + robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 800) { // wait to move till time is met + robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1100) { + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); + robot.oldArmPosition = 1; + } + } + break; + } + break; + + case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos + switch (robot.oldArmPosition) { + case 0: + // transfer + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000) { + robot.oldArmPosition = 2; + } + } + break; + case 1: + // driving + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { + robot.oldArmPosition = 2; + } + } + break; + case 2: + // collect + break; + case 3: + // deposit + robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { // wait to move till time is met + robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 100) { // wait to move till time is met + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 700) { + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1000) { + robot.oldArmPosition = 2; + } + } + } + } + } + } + break; + case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos + switch (robot.oldArmPosition) { + case 0: + // transfer + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1200) { + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500){ + robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_OUT); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000){ + robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_OUT); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2300){ + robot.oldArmPosition = 3; + } + } + } + } + } + break; + case 1: + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 760) { // wait to move till time is met + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1200) { + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500){ + robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_OUT); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2000){ + robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_OUT); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 2300){ + robot.oldArmPosition = 3; + } + } + } + } + } + break; + case 2: + // collect + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 750) { // wait to move till time is met + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); + if (System.currentTimeMillis() - robot.startOfSequencerTime >= 1500) { + robot.oldArmPosition = 3; + } + } + break; + case 3: + // deposit + break; + } + break; + + } + + // drivetrain + robot.driveTrainTeleOp(); + // lift + SliderTeleOp(); + // collector depositor + robot.CollectorToggle(); + // depositor toggle + robot.DepositorToggle(); + } - - @Override - public void telemetry() { - engine.telemetry.addData("Lift Encoder Pos", robot.lift.getCurrentPosition()); - engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); - engine.telemetry.addData("arm Pos", robot.armPosition); - engine.telemetry.addData("old arm pos", robot.oldArmPosition); - engine.telemetry.addData("depositor pos", robot.depositorPos); - engine.telemetry.addData("collector pos", robot.collectorPos); + @Override + public void telemetry () { + engine.telemetry.addData("Lift Encoder Pos", robot.lift.getCurrentPosition()); + engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + engine.telemetry.addData("arm Pos", robot.armPosition); + engine.telemetry.addData("old arm pos", robot.oldArmPosition); + engine.telemetry.addData("depositor pos", robot.depositorPos); + engine.telemetry.addData("collector pos", robot.collectorPos); + engine.telemetry.addData("time", System.currentTimeMillis() - robot.startOfSequencerTime); + } } -} +