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

Added safeguards that you can ONLY move to BUCKET state if the arm is in PARK state. This prevents the driver from hitting the high/low bucket actions while in the SUBMARINE area.

Browse Source
This commit is contained in:
Carlos Rivas
2024-11-12 09:39:50 -08:00
parent de70c14c6e
commit 4bcfdc6e15
1 changed files with 40 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java
View File

@ -106,24 +106,24 @@ public class CometBotTeleopCompetition {
*/
public void toHighBucketScore() {
if (this.currentGP2.triangle && !this.previousGP2.triangle) {
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
this.wrist.toFloorPosition(),
this.arm.toParkPosition(),
this.lift.toHighBucketPosition(),
new SleepAction(.5),
this.arm.toBucketPosition(),
new SleepAction(.5),
this.wrist.toBucketPosition(),
new SleepAction(.5),
this.claw.openClaw(),
new SleepAction(.5),
this.wrist.toFloorPosition(),
new SleepAction(.5),
this.arm.toParkPosition(),
this.lift.toFloorPosition()
));
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
this.lift.toHighBucketPosition(),
new SleepAction(.5),
this.arm.toBucketPosition(),
new SleepAction(.5),
this.wrist.toBucketPosition(),
new SleepAction(.5),
this.claw.openClaw(),
new SleepAction(.5),
this.wrist.toFloorPosition(),
new SleepAction(.5),
this.arm.toParkPosition(),
this.lift.toFloorPosition()
));
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
}
}
}
@ -139,24 +139,24 @@ public class CometBotTeleopCompetition {
*/
public void toLowBucketScore() {
if (this.currentGP2.circle && !this.previousGP2.circle) {
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
this.wrist.toFloorPosition(),
this.arm.toParkPosition(),
this.lift.toLowBucketPosition(),
new SleepAction(.5),
this.arm.toBucketPosition(),
new SleepAction(.5),
this.wrist.toBucketPosition(),
new SleepAction(.5),
this.claw.openClaw(),
new SleepAction(.5),
this.wrist.toFloorPosition(),
new SleepAction(.5),
this.arm.toParkPosition(),
this.lift.toFloorPosition()
));
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
this.lift.toLowBucketPosition(),
new SleepAction(.5),
this.arm.toBucketPosition(),
new SleepAction(.5),
this.wrist.toBucketPosition(),
new SleepAction(.5),
this.claw.openClaw(),
new SleepAction(.5),
this.wrist.toFloorPosition(),
new SleepAction(.5),
this.arm.toParkPosition(),
this.lift.toFloorPosition()
));
fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
}
}
}
@ -183,7 +183,10 @@ public class CometBotTeleopCompetition {
*/
public void toArmParkPosition() {
if (this.currentGP2.square && !this.previousGP2.square) {
Actions.runBlocking(this.arm.toParkPosition());
Actions.runBlocking(new SequentialAction(
this.wrist.toFloorPosition(),
this.arm.toParkPosition()
));
}
}