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 e7996c6..94145c0 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java @@ -17,11 +17,16 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; public class PrototypeRobot extends Robot { - + public double servoWaitTime; + public double servoSecPerDeg = 0.14/60;; public float ELBOW_COLLECT; public float ELBOW_DEPOSIT; public float SHOULDER_COLLECT; public float SHOULDER_DEPOSIT; + public float lastSetPosShoulder = SHOULDER_COLLECT; + public float lastSetPosElbow = ELBOW_COLLECT; + public float currentSetPosShoulder; + public float currentSetPosElbow; private HardwareMap hardwareMap; public MotorEx frontLeft, frontRight, backLeft, backRight, lift; public IMU imu; @@ -91,9 +96,24 @@ public class PrototypeRobot extends Robot { xDrive = new HDrive(frontLeft, frontRight, backLeft, backRight); + depositorShoulder.setPosition(SHOULDER_COLLECT); + depositorElbow.setPosition(ELBOW_COLLECT); + } public void driveTrainTeleOp() { xDrive.driveRobotCentric(-engine.gamepad1.left_stick_x, engine.gamepad1.left_stick_y, -engine.gamepad1.right_stick_x); } -} + + public void ShoulderServoWaitTime(){ + + servoWaitTime = servoSecPerDeg * (Math.abs(lastSetPosShoulder - currentSetPosShoulder)); + + } + + public void ElbowServoWaitTime(){ + + servoWaitTime = 1000 * (servoSecPerDeg * (Math.abs(lastSetPosElbow - currentSetPosElbow))); + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java index 354aef6..0a0cc00 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java @@ -10,9 +10,48 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { private int maxExtension = 2000; private int minExtension = 0; + private long lastCheckedTime; + public PrototypeRobotDrivetrainState(PrototypeRobot robot) { this.robot = robot; } + 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.currentSetPosShoulder = robot.SHOULDER_COLL + 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(){