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 72f8120..7b7a0eb 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 @@ -11,10 +11,11 @@ public class DepositorArmPosState extends CyberarmState { ServoTestRobot robot; private long startOfSequencerTime; private long totalWaitedTime = 0; - private long lastMeasuredTime = System.currentTimeMillis(); private boolean actionsFinished = false; + private boolean enteredLoop = false; - private int armPosition; + private int armPosition = 0; + private int oldArmPosition; public DepositorArmPosState(ServoTestRobot robot, String groupName, String actionName) { this.robot = robot; @@ -30,83 +31,256 @@ public class DepositorArmPosState extends CyberarmState { @Override public void exec() { - lastMeasuredTime = System.currentTimeMillis(); - - if (armPosition == robot.oldArmPosititon) { - actionsFinished = true; - } else if (robot.oldArmPosititon == 1) { - - 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 - totalWaitedTime = robot.servoWaitTime; - if (System.currentTimeMillis() - startOfSequencerTime >= robot.servoWaitTime) { // wait to move till time is met - robot.collectorElbow.setPosition(robot.COLLECTOR_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() - lastMeasuredTime >= totalWaitedTime) { - setHasFinished(true); + switch (armPosition) { + case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos + switch (oldArmPosition) { + case 0: + // transfer + break; + case 10: + // 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 + totalWaitedTime = (long) robot.servoWaitTime; + if (System.currentTimeMillis() - startOfSequencerTime >= totalWaitedTime) { // wait to move till time is met + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); + enteredLoop = true; + 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); + } + } + break; + case 20: + // 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: + // 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); + } + } + } + } + break; } - } - } else if (robot.oldArmPosititon == 2) { - 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 = robot.servoWaitTime; - if (System.currentTimeMillis() - startOfSequencerTime >= robot.servoWaitTime) { // 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() - lastMeasuredTime >= totalWaitedTime) { - setHasFinished(true); - } - } - } else if (robot.oldArmPosititon == 3) { - 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 - totalWaitedTime = robot.servoWaitTime; - if (System.currentTimeMillis() - startOfSequencerTime >= robot.servoWaitTime) { // wait to move till time is met - robot.collectorElbow.setPosition(robot.COLLECTOR_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() - lastMeasuredTime >= totalWaitedTime) { - robot.collectorElbow.setPosition(robot.COLLECTOR_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() - lastMeasuredTime >= totalWaitedTime) { + break; + + case 10:// ----------------------------------------------------------------------------------------------- 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: + // drive pos setHasFinished(true); - } + break; + case 20: + // 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: + // 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); + } + } + } + } + break; } + break; - // -----------------------------------------------------------------------------------------------------0, drive to transfer - // if already at 0, actions have finished = true + case 20:// ----------------------------------------------------------------------------------------------- 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: + // 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: + // 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: + // 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); + } + } + } + } + break; + } + break; - // else if at 1, - // drive collector shoulder to position - // do math and wait till wait time is met - // once met, drive the elbow - // set has finished = true + case 30:// ----------------------------------------------------------------------------------------------- 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: + // 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: + // 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: + // deposit + break; + } + break; - // else if at 2, - // drive collector shoulder to position - // do math and wait till wait time is met - // once met, drive the elbow - // set has finished = true - - // else if at 3, - // drive collector shoulder to position - // do math and wait till wait time is met - // once met, drive the elbow - // set has finished = true - - // -----------------------------------------------------------------------------------------------------1, drive to driving mode - - - // -----------------------------------------------------------------------------------------------------2, drive to collect - - - // -----------------------------------------------------------------------------------------------------3, drive to deposit - - - } } - } -} \ No newline at end of file + } + + @Override + public void telemetry(){ + engine.telemetry.addData("Current Time",System.currentTimeMillis()-startOfSequencerTime); + engine.telemetry.addData("servo wait time",robot.servoWaitTime); + engine.telemetry.addData("servo wait time (long)", (long)robot.servoWaitTime); + engine.telemetry.addData("entered loop",enteredLoop); + } +} 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 83574bc..cd28bdd 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java @@ -22,21 +22,25 @@ public class PrototypeRobot extends Robot { public long waitTime; public double servoWaitTime; public double servoSecPerDeg = 0.14/60; - public float ELBOW_COLLECT; - public float ELBOW_DRIVING; - public float ELBOW_DEPOSIT; - public float SHOULDER_COLLECT; - public float SHOULDER_DRIVING; - public float SHOULDER_DEPOSIT; - public float lastSetPosShoulder = SHOULDER_COLLECT; - public float lastSetPosElbow = ELBOW_COLLECT; + public float DEPOSITOR_SHOULDER_IN; + public float DEPOSITOR_SHOULDER_OUT; + public float DEPOSITOR_ELBOW_IN; + public float DEPOSITOR_ELBOW_OUT; + public float COLLECTOR_SHOULDER_IN; + public float COLLECTOR_SHOULDER_PASSIVE; + public float COLLECTOR_SHOULDER_OUT; + public float COLLECTOR_ELBOW_IN; + public float COLLECTOR_ELBOW_PASSIVE; + public float COLLECTOR_ELBOW_OUT; + public float lastSetPosShoulder; + public float lastSetPosElbow; public float currentSetPosShoulder; public float currentSetPosElbow; private HardwareMap hardwareMap; public MotorEx frontLeft, frontRight, backLeft, backRight, lift; public DcMotor odometerR, odometerL, odometerA; public IMU imu; - public Servo depositorShoulder, depositorElbow, depositor; +// public Servo depositorShoulder, depositorElbow, depositor; private HDrive xDrive; private String string; public double xMultiplier = 1; @@ -81,12 +85,16 @@ public class PrototypeRobot extends Robot { } public void initConstants(){ - ELBOW_DEPOSIT = configuration.variable("Robot", "Tuning", "ELBOW_DEPOSIT").value(); - ELBOW_COLLECT = configuration.variable("Robot", "Tuning", "ELBOW_COLLECT").value(); - SHOULDER_DEPOSIT = configuration.variable("Robot", "Tuning", "SHOULDER_DEPOSIT").value(); - SHOULDER_COLLECT = configuration.variable("Robot", "Tuning", "SHOULDER_COLLECT").value(); - SHOULDER_DRIVING = configuration.variable("Robot", "Tuning", "SHOULDER_DRIVING").value(); - ELBOW_DRIVING = configuration.variable("Robot", "Tuning", "ELBOW_DRIVING").value(); + DEPOSITOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_IN").value(); + DEPOSITOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_OUT").value(); + DEPOSITOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_IN").value(); + DEPOSITOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_OUT").value(); + COLLECTOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_IN").value(); + COLLECTOR_SHOULDER_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_PASSIVE").value(); + COLLECTOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_OUT").value(); + COLLECTOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_IN").value(); + COLLECTOR_ELBOW_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_PASSIVE").value(); + COLLECTOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_OUT").value(); } @Override @@ -98,11 +106,11 @@ public class PrototypeRobot extends Robot { imu = engine.hardwareMap.get(IMU.class, "imu"); //MOTORS - frontRight = new MotorEx(hardwareMap, "frontRight"); - frontLeft = new MotorEx(hardwareMap, "frontLeft"); - backRight = new MotorEx(hardwareMap, "backRight"); - backLeft = new MotorEx(hardwareMap, "backLeft"); - lift = new MotorEx(hardwareMap, "lift"); + frontRight = new MotorEx(hardwareMap, "FrontRight"); + frontLeft = new MotorEx(hardwareMap, "FrontLeft"); + backRight = new MotorEx(hardwareMap, "BackRight"); + backLeft = new MotorEx(hardwareMap, "BackLeft"); + lift = new MotorEx(hardwareMap, "Lift"); configuration = new TimeCraftersConfiguration("Blue Crab"); @@ -128,17 +136,17 @@ public class PrototypeRobot extends Robot { imu.initialize(parameters); - //SERVO - depositorShoulder = hardwareMap.servo.get("depositor_shoulder"); - depositorElbow = hardwareMap.servo.get("depositor_elbow"); - depositor = hardwareMap.servo.get("depositor"); +// //SERVO +// depositorShoulder = hardwareMap.servo.get("depositor_shoulder"); +// depositorElbow = hardwareMap.servo.get("depositor_elbow"); +// depositor = hardwareMap.servo.get("depositor"); // input motors exactly as shown below xDrive = new HDrive(frontLeft, frontRight, backLeft, backRight); - depositorShoulder.setPosition(SHOULDER_COLLECT); - depositorElbow.setPosition(ELBOW_COLLECT); +// depositorShoulder.setPosition(COLLECTOR_SHOULDER_IN); +// depositorElbow.setPosition(COLLECTOR_ELBOW_IN); } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ServoTestRobot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ServoTestRobot.java index d387ba5..0830529 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ServoTestRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ServoTestRobot.java @@ -16,8 +16,8 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class ServoTestRobot extends Robot { public int oldArmPosititon; - public long servoWaitTime; - public double servoSecPerDeg = 0.14/60; + public double servoWaitTime; + public double servoSecPerDeg = 0.14/(60/270); public long waitTime; public float DEPOSITOR_SHOULDER_IN; @@ -89,16 +89,16 @@ public class ServoTestRobot extends Robot { collectorShoulder = hardwareMap.servo.get("collector_shoulder"); collectorElbow = hardwareMap.servo.get("collector_elbow"); - depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); - depositorElbow.setPosition(DEPOSITOR_ELBOW_IN); - collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); - collectorElbow.setPosition(COLLECTOR_ELBOW_IN); + depositorShoulder.setPosition(DEPOSITOR_SHOULDER_OUT); + depositorElbow.setPosition(DEPOSITOR_ELBOW_OUT); + collectorShoulder.setPosition(COLLECTOR_SHOULDER_PASSIVE); + collectorElbow.setPosition(COLLECTOR_ELBOW_PASSIVE); } public void ServoWaitTime(Float lastSetPos, Float currentSetPos){ - servoWaitTime = (long) (servoSecPerDeg * (Math.abs(lastSetPos - currentSetPosShoulder))); + servoWaitTime = 1000 * (servoSecPerDeg * (Math.abs((double)lastSetPos - (double)currentSetPos))); } 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 a2c420d..a3478c0 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 @@ -18,66 +18,66 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { private void depositorAutomation(){ // TODO: 10/7/2023 Workout Logic to move each limb step by step - lastCheckedTime = System.currentTimeMillis(); - - if (engine.gamepad2.a){ - // setting Servo Positions to do time Math - robot.depositorShoulder.setPosition(robot.currentSetPosShoulder); - // running math function to determine time - robot.ShoulderServoWaitTime(); - // determining if the time is met to do the next action - if (lastCheckedTime - System.currentTimeMillis() >= robot.servoWaitTime){ - robot.lastSetPosShoulder = robot.currentSetPosShoulder; - // setting Servo Positions to do time Math - robot.currentSetPosElbow = robot.ELBOW_COLLECT; - robot.depositorElbow.setPosition(robot.currentSetPosElbow); - robot.lastSetPosElbow = robot.currentSetPosElbow; - } - } - if (engine.gamepad2.y){ - // setting Servo Positions to do time Math - robot.currentSetPosShoulder = robot.SHOULDER_DEPOSIT; - robot.depositorShoulder.setPosition(robot.currentSetPosShoulder); - // running math function to determine time - robot.ShoulderServoWaitTime(); - // determining if the time is met to do the next action - if (lastCheckedTime - System.currentTimeMillis() >= robot.servoWaitTime){ - robot.lastSetPosShoulder = robot.currentSetPosShoulder; - // setting Servo Positions to do time Math - robot.currentSetPosElbow = robot.ELBOW_COLLECT; - robot.depositorElbow.setPosition(robot.currentSetPosElbow); - robot.lastSetPosElbow = robot.currentSetPosElbow; - - } - } - } +// lastCheckedTime = System.currentTimeMillis(); +// +// if (engine.gamepad2.a){ +// // setting Servo Positions to do time Math +// robot.depositorShoulder.setPosition(robot.currentSetPosShoulder); +// // running math function to determine time +// robot.ShoulderServoWaitTime(); +// // determining if the time is met to do the next action +// if (lastCheckedTime - System.currentTimeMillis() >= robot.servoWaitTime){ +// robot.lastSetPosShoulder = robot.currentSetPosShoulder; +// // setting Servo Positions to do time Math +// robot.currentSetPosElbow = robot.ELBOW_COLLECT; +// robot.depositorElbow.setPosition(robot.currentSetPosElbow); +// robot.lastSetPosElbow = robot.currentSetPosElbow; +// } +// } +// if (engine.gamepad2.y){ +// // setting Servo Positions to do time Math +// robot.currentSetPosShoulder = robot.SHOULDER_DEPOSIT; +// robot.depositorShoulder.setPosition(robot.currentSetPosShoulder); +// // running math function to determine time +// robot.ShoulderServoWaitTime(); +// // determining if the time is met to do the next action +// if (lastCheckedTime - System.currentTimeMillis() >= robot.servoWaitTime){ +// robot.lastSetPosShoulder = robot.currentSetPosShoulder; +// // setting Servo Positions to do time Math +// robot.currentSetPosElbow = robot.ELBOW_COLLECT; +// robot.depositorElbow.setPosition(robot.currentSetPosElbow); +// robot.lastSetPosElbow = robot.currentSetPosElbow; +// +// } +// } +// } // --------------------------------------------------------------------------------------------------------- Depositor control function - private void depositorTeleOp(){ - // flip arms - if (engine.gamepad1.x) { - // shoulder deposit - robot.depositorShoulder.setPosition(robot.SHOULDER_DEPOSIT); - // shoulder collect - } else if (engine.gamepad1.a) { - robot.depositorShoulder.setPosition(robot.SHOULDER_COLLECT); - } - if (engine.gamepad1.y){ - // elbow deposit - robot.depositorElbow.setPosition(robot.ELBOW_DEPOSIT); - // elbow collect - } else if (engine.gamepad1.b){ - robot.depositorElbow.setPosition(robot.ELBOW_COLLECT); // Collect / transfer = 0 +// private void depositorTeleOp(){ +// // flip arms +// if (engine.gamepad1.x) { +// // shoulder deposit +// robot.depositorShoulder.setPosition(robot.SHOULDER_DEPOSIT); +// // shoulder collect +// } else if (engine.gamepad1.a) { +// robot.depositorShoulder.setPosition(robot.SHOULDER_COLLECT); +// } +// if (engine.gamepad1.y){ +// // elbow deposit +// robot.depositorElbow.setPosition(robot.ELBOW_DEPOSIT); +// // elbow collect +// } else if (engine.gamepad1.b){ +// robot.depositorElbow.setPosition(robot.ELBOW_COLLECT); // Collect / transfer = 0 +// } + +// // depositor +// if (engine.gamepad1.right_bumper) { +// robot.depositor.setPosition(0.8); +// } else if (engine.gamepad1.left_bumper) { +// robot.depositor.setPosition(0.2); } - // depositor - if (engine.gamepad1.right_bumper) { - robot.depositor.setPosition(0.8); - } else if (engine.gamepad1.left_bumper) { - robot.depositor.setPosition(0.2); - } - - } +// } // --------------------------------------------------------------------------------------------------------- Slider control function private void sliderTeleOp(){ if (engine.gamepad1.right_trigger != 0){ @@ -108,7 +108,7 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { robot.driveTrainTeleOp(); // depositor - depositorTeleOp(); +// depositorTeleOp(); // lift sliderTeleOp(); @@ -120,9 +120,9 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { public void telemetry() { engine.telemetry.addData("Lift Encoder Pos", robot.lift.motor.getCurrentPosition()); engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); - engine.telemetry.addData("Elbow Collect", robot.ELBOW_COLLECT); - engine.telemetry.addData("Elbow Deposit", robot.ELBOW_DEPOSIT); - engine.telemetry.addData("Shoulder Collect", robot.SHOULDER_COLLECT); - engine.telemetry.addData("Shoulder Deposit", robot.SHOULDER_DEPOSIT); +// engine.telemetry.addData("Elbow Collect", robot.ELBOW_COLLECT); +// engine.telemetry.addData("Elbow Deposit", robot.ELBOW_DEPOSIT); +// engine.telemetry.addData("Shoulder Collect", robot.SHOULDER_COLLECT); +// engine.telemetry.addData("Shoulder Deposit", robot.SHOULDER_DEPOSIT); } }