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 new file mode 100644 index 0000000..9e5ed10 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java @@ -0,0 +1,26 @@ +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.DepositorArmPosState; +import org.timecrafters.CenterStage.Common.PrototypeRobot; +import org.timecrafters.CenterStage.Common.ServoTestRobot; +import org.timecrafters.CenterStage.TeleOp.States.PrototypeRobotDrivetrainState; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@Autonomous(name = "Servo Test", group = "Testing") + +public class ServoTestEngine extends CyberarmEngine { + + ServoTestRobot robot; + @Override + public void setup() { + this.robot = new ServoTestRobot("Hello World"); + this.robot.setup(); + + addState(new DepositorArmPosState(robot, "ServoPositions", "01-0")); + + } +} 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 2ebe289..fd28817 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 @@ -1,50 +1,91 @@ package org.timecrafters.CenterStage.Autonomous.States; import org.timecrafters.CenterStage.Common.PrototypeRobot; +import org.timecrafters.CenterStage.Common.ServoTestRobot; import dev.cyberarm.engine.V2.CyberarmState; public class DepositorArmPosState extends CyberarmState { private final boolean stateDisabled; - PrototypeRobot robot; + ServoTestRobot robot; + private long startOfSequencerTime; + private long totalWaitedTime; + private long lastMeasuredTime; + private boolean actionsFinished = false; - public DepositorArmPosState(PrototypeRobot robot, String groupName, String actionName) { + private int armPosition; + + public DepositorArmPosState(ServoTestRobot robot, String groupName, String actionName) { this.robot = robot; this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; + armPosition = robot.configuration.variable(groupName, actionName, "armPosition").value(); } @Override public void start() { - //add variables that need to be reinitillized + startOfSequencerTime = System.currentTimeMillis(); } @Override public void exec() { - if (robot.armPosition == 0) { // transfer mode - if (robot.oldArmPosititon == 0){ - setHasFinished(true); - } else if (robot.oldArmPosititon == 1){ - robot.depositorElbow.setPosition(robot.ELBOW_COLLECT); - } else if (robot.oldArmPosititon == 2){ + if (actionsFinished) { + setHasFinished(true); + } else { + lastMeasuredTime = System.currentTimeMillis(); + if (armPosition == robot.oldArmPosititon){ + actionsFinished = true; + } else if (robot.oldArmPosititon == 1){ + + robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); + robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_PASSIVE, robot.COLLECTOR_SHOULDER_IN); + totalWaitedTime = robot.servoWaitTime; + if (System.currentTimeMillis() - startOfSequencerTime >= robot.servoWaitTime){ + robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN); + robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); + totalWaitedTime += robot.servoWaitTime; + if (System.currentTimeMillis() - lastMeasuredTime >= totalWaitedTime){ + actionsFinished = true; + } + } } -// } else if (robot.armPosition == 1) { // drive mode -// if (robot.oldArmPosititon == 1){ -// setHasFinished(true); -// } else if (robot.oldArmPosititon == 0){ -// -// } else if (robot.oldArmPosititon == 2){ -// -// } -// } else if (robot.armPosition == 2) { // deposit mode -// if (robot.oldArmPosititon == 2){ -// setHasFinished(true); -// } else if (robot.oldArmPosititon == 0){ -// -// } else if (robot.oldArmPosititon == 1){ -// -// } + + // -----------------------------------------------------------------------------------------------------0, drive to transfer + // if already at 0, actions have finished = true + + // 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 + + // 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 + + + + + } } } 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 adba0b3..83574bc 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java @@ -19,11 +19,14 @@ public class PrototypeRobot extends Robot { public int armPosition = 0; public int oldArmPosititon; + 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; @@ -82,6 +85,8 @@ public class PrototypeRobot extends Robot { 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(); } @Override @@ -99,7 +104,7 @@ public class PrototypeRobot extends Robot { backLeft = new MotorEx(hardwareMap, "backLeft"); lift = new MotorEx(hardwareMap, "lift"); - configuration = new TimeCraftersConfiguration("Robbie"); + configuration = new TimeCraftersConfiguration("Blue Crab"); initConstants(); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ServoTestRobot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ServoTestRobot.java new file mode 100644 index 0000000..d387ba5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ServoTestRobot.java @@ -0,0 +1,106 @@ +package org.timecrafters.CenterStage.Common; + +import com.arcrobotics.ftclib.drivebase.HDrive; +import com.arcrobotics.ftclib.hardware.motors.MotorEx; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +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; + +import org.timecrafters.Library.Robot; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +public class ServoTestRobot extends Robot { + public int oldArmPosititon; + public long servoWaitTime; + public double servoSecPerDeg = 0.14/60; + public long waitTime; + + 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 currentSetPosShoulder; + public float currentSetPosElbow; + private HardwareMap hardwareMap; + public IMU imu; + public Servo depositorShoulder, depositorElbow, collectorShoulder, collectorElbow; + private String string; + + + + private CyberarmEngine engine; + + public TimeCraftersConfiguration configuration; + + public ServoTestRobot(String string) { + this.engine = engine; + setup(); + this.string = string; + } + + public void initConstants(){ + 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 + public void setup() { + System.out.println("Bacon: " + this.string); + this.hardwareMap = CyberarmEngine.instance.hardwareMap; + this.engine = CyberarmEngine.instance; + + imu = engine.hardwareMap.get(IMU.class, "imu"); + + configuration = new TimeCraftersConfiguration("Blue Crab"); + + initConstants(); + + //IMU + IMU.Parameters parameters = new IMU.Parameters( + new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.LEFT, + RevHubOrientationOnRobot.UsbFacingDirection.UP)); + + imu.initialize(parameters); + + //SERVO + depositorShoulder = hardwareMap.servo.get("depositor_shoulder"); + depositorElbow = hardwareMap.servo.get("depositor_elbow"); + 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); + + } + + public void ServoWaitTime(Float lastSetPos, Float currentSetPos){ + + servoWaitTime = (long) (servoSecPerDeg * (Math.abs(lastSetPos - currentSetPosShoulder))); + + } + + +} \ No newline at end of file