SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Commit with states

Browse Source
This commit is contained in:
Carlos
2024-11-03 21:36:07 -08:00
parent 04b61d7aa7
commit edf0ec572a
1 changed files with 48 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java
View File

@ -33,6 +33,8 @@ public class Auto {
public Follower follower; public Follower follower;
public Telemetry telemetry; public Telemetry telemetry;
public int caseState = 1;
public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) { public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK); arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
@ -61,23 +63,52 @@ public class Auto {
} }
public void update() { public void update() {
//follower.update();
if (clawTimer.getElapsedTimeSeconds() > 2) { this.telemetry.addData("Current State", caseState);
claw.openClaw(); this.telemetry.addData("Claw Timer", clawTimer.getElapsedTimeSeconds());
} this.telemetry.addData("Arm Timer", armTimer.getElapsedTimeSeconds());
if (armTimer.getElapsedTimeSeconds() > 4) { this.telemetry.addData("Wrist Timer", wristTimer.getElapsedTimeSeconds());
arm.engageArm(); this.telemetry.update();
}
if (clawTimer.getElapsedTimeSeconds() > 6) { switch(caseState) {
claw.closeClaw(); case 1:
} claw.openClaw();
if (armTimer.getElapsedTimeSeconds() > 8) { caseState = 2;
arm.bucketArm(); break;
wrist.bucketWrist(); case 2:
} if (clawTimer.getElapsedTimeSeconds() > 2) {
if (clawTimer.getElapsedTimeSeconds() > 10) { arm.engageArm();
claw.openClaw(); caseState = 3;
}
break;
case 3:
if (armTimer.getElapsedTimeSeconds() > 4) {
wrist.floorWrist();
caseState = 4;
}
break;
case 4:
if (clawTimer.getElapsedTimeSeconds() > 6) {
claw.closeClaw();
caseState = 5;
}
break;
case 5:
if (armTimer.getElapsedTimeSeconds() > 8) {
arm.bucketArm();
wrist.bucketWrist();
caseState = 6;
}
break;
case 6:
if (clawTimer.getElapsedTimeSeconds() > 10) {
claw.openClaw();
caseState = 7;
}
break;
case 7:
this.init();
break;
} }
} }
} }