From 7d3d5da8de8c4a2fb5ca6f0ddceaca26e40c08c2 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Thu, 9 Nov 2023 20:31:55 -0600 Subject: [PATCH] wrote teleOp for arm sequences --- .../States/DepositorArmPosState.java | 91 ++------- .../CenterStage/Common/PrototypeRobot.java | 186 +++++++++++++++++- .../CenterStage/TeleOp/States/ArmPosTest.java | 36 ++-- .../States/PrototypeRobotDrivetrainState.java | 13 +- 4 files changed, 236 insertions(+), 90 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java index 7b7a0eb..ca8688c 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java @@ -37,7 +37,7 @@ public class DepositorArmPosState extends CyberarmState { case 0: // transfer break; - case 10: + case 1: // driving robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_PASSIVE, robot.COLLECTOR_SHOULDER_IN); // calculate time to move @@ -48,41 +48,29 @@ public class DepositorArmPosState extends CyberarmState { robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move totalWaitedTime += (long) robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= totalWaitedTime) { -// setHasFinished(true); + setHasFinished(true); } } break; - case 20: + case 2: // collect robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { setHasFinished(true); } } break; - case 30: + case 3: // deposit robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_PASSIVE, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 1600) { robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 3100) { setHasFinished(true); } @@ -93,57 +81,41 @@ public class DepositorArmPosState extends CyberarmState { } break; - case 10:// ----------------------------------------------------------------------------------------------- drive to driving pos + case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos switch (oldArmPosition) { case 0: // transfer robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { setHasFinished(true); } } break; - case 10: + case 1: // drive pos setHasFinished(true); break; - case 20: + case 2: // collect robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 600) { // wait to move till time is met robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 2100) { setHasFinished(true); } } break; - case 30: + case 3: // deposit robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_PASSIVE, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 1600) { robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 3100) { setHasFinished(true); } @@ -154,65 +126,46 @@ public class DepositorArmPosState extends CyberarmState { } break; - case 20:// ----------------------------------------------------------------------------------------------- drive to collect pos + case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos switch (oldArmPosition) { case 0: // transfer robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_OUT); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { setHasFinished(true); } } break; - case 10: + case 1: // driving robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_OUT); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { setHasFinished(true); } } break; - case 20: + case 2: // collect robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { setHasFinished(true); } } break; - case 30: + case 3: // deposit robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 100) { - robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 100) { setHasFinished(true); } @@ -223,51 +176,39 @@ public class DepositorArmPosState extends CyberarmState { } break; - case 30:// ----------------------------------------------------------------------------------------------- drive to deposit pos + case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos switch (oldArmPosition) { case 0: // transfer robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { setHasFinished(true); } } break; - case 10: + case 1: // driving robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 700) { // wait to move till time is met robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { setHasFinished(true); } } break; - case 20: + case 2: // collect robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position -// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move -// totalWaitedTime = (long) robot.servoWaitTime; if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); -// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move -// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { setHasFinished(true); } } break; - case 30: + case 3: // deposit break; } 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 aac935c..3915bca 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java @@ -17,8 +17,9 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class PrototypeRobot extends Robot { public int armPosition = 0; - - public int oldArmPosititon; + public boolean stateFinished; + public long startOfSequencerTime; + public int oldArmPosition = 0; public long waitTime; public double servoWaitTime; public double servoSecPerDeg = 0.14/60; @@ -203,4 +204,185 @@ public class PrototypeRobot extends Robot { positionH += dtheta; } + + public void ArmSequences(){ + switch (armPosition) { + case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos + switch (oldArmPosition) { + case 0: + // transfer + break; + case 1: + // driving + collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met + collectorElbow.setPosition(COLLECTOR_ELBOW_IN); + if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { + oldArmPosition = 0; + } + } + 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 = 0; + } + } + 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_IN); // drive the shoulder to the transfer position + if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met + collectorElbow.setPosition(COLLECTOR_ELBOW_IN); + if (System.currentTimeMillis() - startOfSequencerTime >= 3100) { + oldArmPosition = 0; + } + } + } + } + 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 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; + + } + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/ArmPosTest.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/ArmPosTest.java index 6fbedf7..edcdc85 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/ArmPosTest.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/ArmPosTest.java @@ -7,7 +7,8 @@ import dev.cyberarm.engine.V2.CyberarmState; public class ArmPosTest extends CyberarmState { - private float armPos; + private float armPosE; + private float armPosS; private long lastMeasuredTime; private long waitTime; PrototypeRobot robot; @@ -17,7 +18,8 @@ public class ArmPosTest extends CyberarmState { @Override public void start() { - armPos = 0F; + armPosS = 0F; + armPosE = 0F; lastMeasuredTime = System.currentTimeMillis(); waitTime = 500; @@ -27,22 +29,31 @@ public class ArmPosTest extends CyberarmState { @Override public void exec(){ - robot.collectorElbow.setPosition(armPos); + robot.collectorShoulder.setPosition(armPosS); + robot.collectorElbow.setPosition(armPosE); - if (engine.gamepad1.a){ - robot.collectorShoulder.setPosition(0.75); - } else if (engine.gamepad1.y){ - robot.collectorShoulder.setPosition(0.65); - }else if (engine.gamepad1.x){ - robot.collectorShoulder.setPosition(0.4); +// if (engine.gamepad1.a){ +// robot.collectorShoulder.setPosition(0.75); +// } else if (engine.gamepad1.y){ +// robot.collectorShoulder.setPosition(0.65); +// }else if (engine.gamepad1.x){ +// robot.collectorShoulder.setPosition(0.4); +// } + + if (engine.gamepad1.y && System.currentTimeMillis() - lastMeasuredTime > 500){ + lastMeasuredTime = System.currentTimeMillis(); + armPosS += 0.05; + } else if (engine.gamepad1.a && System.currentTimeMillis() - lastMeasuredTime > 500){ + lastMeasuredTime = System.currentTimeMillis(); + armPosS -= 0.05; } if (engine.gamepad2.y && System.currentTimeMillis() - lastMeasuredTime > 500){ lastMeasuredTime = System.currentTimeMillis(); - armPos += 0.05; + armPosE += 0.05; } else if (engine.gamepad2.a && System.currentTimeMillis() - lastMeasuredTime > 500){ lastMeasuredTime = System.currentTimeMillis(); - armPos -= 0.05; + armPosE -= 0.05; } } @@ -50,6 +61,7 @@ public class ArmPosTest extends CyberarmState { @Override public void telemetry() { - engine.telemetry.addData("arm pos current", armPos); + engine.telemetry.addData("arm pos current Shoulder", armPosS); + engine.telemetry.addData("arm pos current Elbow", armPosE); } } 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 eaeba79..4da33a4 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 @@ -43,9 +43,20 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { @Override public void exec() { + if (engine.gamepad2.a){ + robot.armPosition = 0; + } else if (engine.gamepad2.x){ + robot.armPosition = 1; + } else if (engine.gamepad2.b){ + robot.armPosition = 2; + } else if (engine.gamepad2.y){ + robot.armPosition = 3; + } + // drivetrain robot.driveTrainTeleOp(); - + // arm sequencer + robot.ArmSequences(); // lift sliderTeleOp();