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

worked on writing the correct structure for the arm sequences and positions. Started to fix and simplify some functions so my code can be more modular.

Browse Source
This commit is contained in:
SpencerPiha
2023-10-26 20:26:25 -05:00
parent c32ab723af
commit 67b0e4daee
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
}
}
}
}
}