diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java index 0013451..6eb0d6c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java @@ -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() + )); } }