TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 16:52:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
NerdyBirdy460
2023-10-26 20:29:28 -05:00
parent 6eae3fa305 a22b36142d
commit 84fe96fa6e
1 changed files with 66 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

111
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java
View File

@@ -10,8 +10,8 @@ public class DepositorArmPosState extends CyberarmState {
private final boolean stateDisabled;
ServoTestRobot robot;
private long startOfSequencerTime;
private long totalWaitedTime;
private long lastMeasuredTime;
private long totalWaitedTime = 0;
private long lastMeasuredTime = System.currentTimeMillis();
private boolean actionsFinished = false;
private int armPosition;
@@ -29,63 +29,84 @@ public class DepositorArmPosState extends CyberarmState {
@Override
public void exec() {
if (actionsFinished) {
setHasFinished(true);
} else {
lastMeasuredTime = System.currentTimeMillis();
if (armPosition == robot.oldArmPosititon){
actionsFinished = true;
} else if (robot.oldArmPosititon == 1){
lastMeasuredTime = System.currentTimeMillis();
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;
}
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);
}
}
} 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) {
setHasFinished(true);
}
}
// -----------------------------------------------------------------------------------------------------0, drive to transfer
// if already at 0, actions have finished = true
// -----------------------------------------------------------------------------------------------------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 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 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
// 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
// -----------------------------------------------------------------------------------------------------1, drive to driving mode
// -----------------------------------------------------------------------------------------------------2, drive to collect
// -----------------------------------------------------------------------------------------------------3, drive to deposit
// -----------------------------------------------------------------------------------------------------2, drive to collect
// -----------------------------------------------------------------------------------------------------3, drive to deposit
}
}
}
}
}