Skip to content

Commit

Permalink
this is how we recorded the vid
Browse files Browse the repository at this point in the history
  • Loading branch information
@jvandegriff
jvandegriff committed Apr 17, 2021
1 parent b655097 commit 5c5da8f
Show file tree
Hide file tree
Showing 2 changed files with 22 additions and 18 deletions.
33 changes: 15 additions & 18 deletions ...c/main/java/org/firstinspires/ftc/teamcode/GameChangersTester/GameChangersAutonomous.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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();
Expand All @@ -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);
Expand Down Expand Up @@ -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
}


Expand Down
7 changes: 7 additions & 0 deletions .../src/main/java/org/firstinspires/ftc/teamcode/GameChangersTester/WobbleGoalCollector.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -56,6 +59,10 @@ public void moveArmUp() {
targetPosition = upPosition;
}

public void moveArmMid() {
targetPosition = midPosition;
}

public void moveArmDown(){
targetPosition = downPosition;
}
Expand Down

0 comments on commit 5c5da8f

Please sign in to comment.