diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java index 1696a7e..9e2bf89 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java @@ -9,6 +9,7 @@ public class RobotConstants { public final static double armFloor = 0.7; public final static double armSubmarine = 0.55; + public final static double armReverseBucket = 0.08; public final static double armPark = 0.33; //value for grabbing the hook Specimen public final static double grabBlueberry = 0.3; @@ -16,12 +17,14 @@ public class RobotConstants { public final static double wristGrabBlueberrySkyhook = 0.1; public final static double armHangBlueberrySkyhook = 0.6; public final static double wristHangBlueberrySkyhook = 0.3; + public final static int backwardBucketDrop = 4670; public final static double armBucket = 0.45; public final static double armInit = 0.135; public final static double wristInit = 0.25; public final static double wristPickup = 0.475; public final static double wristBucket = 0.56; public final static double wristFloor = 0.75; + public final static double wristBackwardBucket = 0.775; public final static double wristRung = 0.55; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java index 5af501c..5c4137f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java @@ -7,6 +7,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armGrabBlueb import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armHangBlueberrySkyhook; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armInit; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armReverseBucket; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.ServoImplEx; @@ -14,7 +15,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx; public class ArmSubsystem { public enum ArmState { - PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT + PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERE_BUCKET } private ServoImplEx arm; @@ -31,7 +32,10 @@ public class ArmSubsystem { setState(ArmState.HANG_BLUEBERRY_SKYHOOK); arm.setPosition(armHangBlueberrySkyhook); } - + public void toReverseBucket(){ + arm.setPosition(armReverseBucket); + setState(ArmState.REVERE_BUCKET); + } public void toParkPosition() { arm.setPosition(armPark); setState(ArmState.PARK); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java index a8dbf15..3bf3826 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystem; import static org.firstinspires.ftc.teamcode.PedroConstants.WRIST_SERVO; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBackwardBucket; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristGrabBlueberrySkyhook; @@ -20,7 +21,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx; public class WristSubsystem { public enum WristState { - FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT + FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERSE_BUCKET } public ServoImplEx wrist; @@ -92,7 +93,10 @@ public class WristSubsystem { toFloorPosition(); } } - + public void toReverseBucket(){ + wrist.setPosition(wristBackwardBucket); + setState(WristState.REVERSE_BUCKET); + } public WristState getState() { return this.state; }