diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GameChangersTester/GameChangersAutonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GameChangersTester/GameChangersAutonomous.java index 97432f786099..824d1058ad64 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GameChangersTester/GameChangersAutonomous.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GameChangersTester/GameChangersAutonomous.java @@ -32,8 +32,6 @@ import ftc.electronvolts.util.units.Angle; import ftc.electronvolts.util.units.Distance; import ftc.electronvolts.util.units.Time; -import ftc.evlib.hardware.config.RobotCfg; -import ftc.evlib.hardware.motors.MotorEncEx; import ftc.evlib.opmodes.AbstractAutoOp; import ftc.evlib.statemachine.EVEndConditions; import ftc.evlib.statemachine.EVStateMachineBuilder; @@ -333,33 +331,32 @@ public StateMachine buildStates() { return S.WAIT_FOR_DROP_4; }); b.addWait(S.WAIT_FOR_DROP_4, S.DROP_WOBBLE_GOAL_4, wobbleGoalWaitTime); - b.addServo(S.DROP_WOBBLE_GOAL_4, S.MOVE_ARM_UP_6, robotCfg.getPincher().getName(), ServoPresets.WobblePincher.OPENED, servoReleaseWaitTime,true); - b.add(S.MOVE_ARM_UP_6, () -> { - robotCfg.getWobbleGoalArm().moveArmUp(); + b.addServo(S.DROP_WOBBLE_GOAL_4, S.MOVE_ARM_MID, robotCfg.getPincher().getName(), ServoPresets.WobblePincher.OPENED, servoReleaseWaitTime,true); + b.add(S.MOVE_ARM_MID, () -> { + robotCfg.getWobbleGoalArm().moveArmMid(); return S.DRIVE_BACK; }); - b.addDrive(S.DRIVE_BACK, S.DRIVE_BACK_2, Distance.fromFeet(.85), 0.7, 0, 0); - b.addDrive(S.DRIVE_BACK_2, S.MOVE_ARM_DOWN_7, Distance.fromFeet(2.5), .7, 90, 0); + b.addDrive(S.DRIVE_BACK, S.DRIVE_INTO_WALL, Distance.fromFeet(.9), 0.7, 3, 0); + b.addDrive(S.DRIVE_INTO_WALL, S.DRIVE_BACK_2, Distance.fromFeet(.3), 0.7, 270, 0); + b.addDrive(S.DRIVE_BACK_2, S.MOVE_ARM_DOWN_7, Distance.fromFeet(2.52), .7, 90, 0); b.add(S.MOVE_ARM_DOWN_7, () -> { robotCfg.getWobbleGoalArm().moveArmDown(); return S.DRIVE_BACK_3; }); - b.addDrive(S.DRIVE_BACK_3, S.STOP, Distance.fromFeet(.2), .3, 180, 0); -// b.addWait(S.WAIT_FOR_DROP_6, S.PICKUP_WOBBLE, wobbleGoalWaitTime); - b.addServo(S.PICKUP_WOBBLE,S.MOVE_ARM_UP_7,robotCfg.getPincher().getName(), ServoPresets.WobblePincher.CLOSED,true); + b.addDrive(S.DRIVE_BACK_3, S.WAIT_TO_GRAB, Distance.fromFeet(.14), .3, 180, 0); + b.addWait(S.WAIT_TO_GRAB, S.PICKUP_WOBBLE, 300L); + b.addServo(S.PICKUP_WOBBLE,S.WAIT_FOR_PICKUP,robotCfg.getPincher().getName(), ServoPresets.WobblePincher.CLOSED,true); + b.addWait(S.WAIT_FOR_PICKUP, S.MOVE_ARM_UP_7, 300L); b.add(S.MOVE_ARM_UP_7, () -> { robotCfg.getWobbleGoalArm().moveArmUp(); return S.DRIVE_BACK_5; }); - b.addDrive(S.DRIVE_BACK_5, S.DRIVE_FORWARD_1, Distance.fromFeet(.95), 0.7, 90, 0); - b.addDrive(S.DRIVE_FORWARD_1, S.DRIVE_FORWARD_2, Distance.fromFeet(1.5), 1.0, 275, 0); - b.addDrive(S.DRIVE_FORWARD_2, S.DRIVE_FORWARD_3, Distance.fromFeet(.32), 0.5, 180, 0); - b.addDrive(S.DRIVE_FORWARD_3, S.WAIT_FOR_DROP_5, Distance.fromFeet(1.55), 0.7, 255, 0); - b.addWait(S.WAIT_FOR_DROP_5, S.MOVE_ARM_DOWN_8, wobbleGoalWaitTime); + b.addDrive(S.DRIVE_BACK_5, S.MOVE_ARM_DOWN_8, Distance.fromFeet(2.4), 0.7, 252, 0); b.add(S.MOVE_ARM_DOWN_8, () -> { robotCfg.getWobbleGoalArm().moveArmDown(); - return S.DROP_WOBBLE_GOAL_5; + return S.WAIT_FOR_DROP_5; }); + b.addWait(S.WAIT_FOR_DROP_5, S.DROP_WOBBLE_GOAL_5, wobbleGoalWaitTime); b.addServo(S.DROP_WOBBLE_GOAL_5, S.MOVE_ARM_UP_5, robotCfg.getPincher().getName(), ServoPresets.WobblePincher.OPENED, servoReleaseWaitTime,true); b.add(S.MOVE_ARM_UP_5, () -> { robotCfg.getWobbleGoalArm().moveArmUp(); @@ -369,7 +366,7 @@ public StateMachine buildStates() { return S.PARK_4_A; } }); b.addDrive(S.PARK_4_A, S.PARK_4, Distance.fromFeet(1), 1, 5, 0); - b.addDrive(S.PARK_4, S.STOP, Distance.fromFeet(1.35), 1, 55, 0); + b.addDrive(S.PARK_4, S.STOP, Distance.fromFeet(1), 1, 90, 0); b.addStop(S.STOP); @@ -768,7 +765,7 @@ public enum S implements StateName { BLUE_VUFORIA_EXPLORE, BLUE_WAIT, BLUE_DRIVE_1C, BLUE_DRIVE_1B, BLUE_DRIVE_1, SET_VUCALC, WAIT_FOR_VUFORIA_INIT, ACTIVATE_TARGETS, DEACTIVATE_TARGETS, BLUE_SET_VUCALC, BLUE_TARGETS_ACTIVATE, BLUE_WAIT_FOR_VUFORIA_INIT, BLUE_DEACTIVATE_TARGETS, ELEVATE_SHOOTER, BLUE_START_FLYWHEEL, BLUE_WOBBLE_TURN, BLUE_WOBBLE_TURN_1, BLUE_WOBBLE_TURN_4, AFTER_PARK, PARK_0_A, PARK_1_A, PARK_4_A, BLUE_PARK_0_A, BLUE_PARK_1_A, BLUE_PARK_4_A, BLUE_PARK_4C, - SECOND_RING_COLLECTION, DRIVE_BACKWARDS_TO_COLLECT, TURN_ON_COLLECTOR, TURN_OFF_COLLECTOR, ACTIVATE_TARGETS_2, VUFORIA_LINEUP, RETURN_TO_SHOOT, SET_VUCALC_2, START_FLYWHEEL_2, DEACTIVATE_TARGETS_2, TURN_OFF_SHOOTER_2, SHOOT_RINGS_2, TURN_AIM_SHOOT_2, DRIVE_SLOW_SHOOT, THREE_RING_COLLECTION, BUMP_RING_STACK, TURN_ON_COLLECTOR_B, PAUSE_1, THREE_RING_COLLECTION_1, THREE_RING_COLLECTION_2, PAUSE_2, THREE_RING_COLLECTION_3, PAUSE_3, RETURN_FROM_FOUR_RINGS, SET_SHOOTER_SERVO, WAIT_COLLECT, BLUE_SECOND_RING_COLLECTION, BLUE_TURN_ON_COLLECTOR, BLUE_BUMP_RING_STACK, BLUE_DRIVE_BACKWARDS_TO_COLLECT, BLUE_RETURN_TO_SHOOT, BLUE_TURN_OFF_COLLECTOR, BLUE_WAIT_COLLECT, BLUE_TURN_ON_COLLECTOR_B, BLUE_THREE_RING_COLLECTION_1, BLUE_PAUSE_1, BLUE_THREE_RING_COLLECTION_2, BLUE_PAUSE_2, BLUE_RETURN_FROM_FOUR_RINGS, BLUE_SET_VUCALC_2, BLUE_ACTIVATE_TARGETS_2, BLUE_VUFORIA_LINEUP, BLUE_DEACTIVATE_TARGETS_2, BLUE_START_FLYWHEEL_2, BLUE_TURN_AIM_SHOOT_2, BLUE_SET_SHOOTER_SERVO, BLUE_SHOOT_RINGS_2, BLUE_TURN_OFF_SHOOTER_2, DRIVE_BACK, DRIVE_BACK_2, DRIVE_BACK_3, WOBBLE_DRIVE, PICKUP_WOBBLE, DRIVE_BACK_5, DRIVE_FORWARD_1, DRIVE_FORWARD_2, DRIVE_FORWARD_3, MOVE_ARM_DOWN_5, WAIT_FOR_DROP_5, DROP_WOBBLE_GOAL_5, MOVE_ARM_UP_5, MOVE_ARM_UP_6, WAIT_FOR_DROP_6, MOVE_ARM_DOWN_7, MOVE_ARM_UP_7, MOVE_ARM_DOWN_8, OPENCV_INIT + SECOND_RING_COLLECTION, DRIVE_BACKWARDS_TO_COLLECT, TURN_ON_COLLECTOR, TURN_OFF_COLLECTOR, ACTIVATE_TARGETS_2, VUFORIA_LINEUP, RETURN_TO_SHOOT, SET_VUCALC_2, START_FLYWHEEL_2, DEACTIVATE_TARGETS_2, TURN_OFF_SHOOTER_2, SHOOT_RINGS_2, TURN_AIM_SHOOT_2, DRIVE_SLOW_SHOOT, THREE_RING_COLLECTION, BUMP_RING_STACK, TURN_ON_COLLECTOR_B, PAUSE_1, THREE_RING_COLLECTION_1, THREE_RING_COLLECTION_2, PAUSE_2, THREE_RING_COLLECTION_3, PAUSE_3, RETURN_FROM_FOUR_RINGS, SET_SHOOTER_SERVO, WAIT_COLLECT, BLUE_SECOND_RING_COLLECTION, BLUE_TURN_ON_COLLECTOR, BLUE_BUMP_RING_STACK, BLUE_DRIVE_BACKWARDS_TO_COLLECT, BLUE_RETURN_TO_SHOOT, BLUE_TURN_OFF_COLLECTOR, BLUE_WAIT_COLLECT, BLUE_TURN_ON_COLLECTOR_B, BLUE_THREE_RING_COLLECTION_1, BLUE_PAUSE_1, BLUE_THREE_RING_COLLECTION_2, BLUE_PAUSE_2, BLUE_RETURN_FROM_FOUR_RINGS, BLUE_SET_VUCALC_2, BLUE_ACTIVATE_TARGETS_2, BLUE_VUFORIA_LINEUP, BLUE_DEACTIVATE_TARGETS_2, BLUE_START_FLYWHEEL_2, BLUE_TURN_AIM_SHOOT_2, BLUE_SET_SHOOTER_SERVO, BLUE_SHOOT_RINGS_2, BLUE_TURN_OFF_SHOOTER_2, DRIVE_BACK, DRIVE_BACK_2, DRIVE_BACK_3, WOBBLE_DRIVE, PICKUP_WOBBLE, DRIVE_BACK_5, DRIVE_FORWARD_1, DRIVE_FORWARD_2, DRIVE_FORWARD_3, MOVE_ARM_DOWN_5, WAIT_FOR_DROP_5, DROP_WOBBLE_GOAL_5, MOVE_ARM_UP_5, MOVE_ARM_UP_6, WAIT_FOR_DROP_6, MOVE_ARM_DOWN_7, MOVE_ARM_UP_7, MOVE_ARM_DOWN_8, WAIT_FOR_PICKUP, WAIT_TO_GRAB, DRIVE_INTO_WALL, MOVE_ARM_MID, OPENCV_INIT } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GameChangersTester/WobbleGoalCollector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GameChangersTester/WobbleGoalCollector.java index 1478de455e25..42da3ecde641 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GameChangersTester/WobbleGoalCollector.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GameChangersTester/WobbleGoalCollector.java @@ -17,6 +17,9 @@ public class WobbleGoalCollector { //positions private final double upPosition = 0.84; + + private final double midPosition = 0.6; //me me big boi was here + private final double downPosition = 0.3; private final double moreDownPosition = 0.2; private final double maxPosition = 0.9; @@ -56,6 +59,10 @@ public void moveArmUp() { targetPosition = upPosition; } + public void moveArmMid() { + targetPosition = midPosition; + } + public void moveArmDown(){ targetPosition = downPosition; }