Upload Day

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java

@@ -12,11 +12,13 @@ public class DriveToCoordinates extends CyberarmState {
     private double yTarget;
     private float faceAngle;
     private double tolerancePos;
-    private double power;
+    private double basePower;
     private boolean braking;
     private long breakStartTime;
     private long brakeTime;
     private boolean autoFace;
+    private double decelRange;
+    private double decelMin;
     private boolean scoringArea;
 
     public DriveToCoordinates(Robot robot, String groupName, String actionName) {
@@ -38,27 +40,51 @@ public class DriveToCoordinates extends CyberarmState {
         this.yTarget = yTarget;
         this.faceAngle = faceAngle;
         this.tolerancePos = tolerance;
-        this.power = power;
+        this.basePower = power;
         this.brakeTime = brakeTime;
     }
 
+    public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long brakeTime, double  decelRange, double decelMin) {
+        this.robot = robot;
+        this.xTarget = xTarget;
+        this.yTarget = yTarget;
+        this.faceAngle = faceAngle;
+        this.tolerancePos = tolerance;
+        this.basePower = power;
+        this.brakeTime = brakeTime;
+        this.decelRange = decelRange;
+        this.decelMin = decelMin;
+    }
+
     @Override
     public void init() {
         if (!groupName.equals("manual")) {
             xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value());
             yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
-            power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
+            basePower = robot.stateConfiguration.variable(groupName, actionName, "power").value();
             tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value());
             brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
 
             try {
                 faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value();
-            } catch (NullPointerException e) {
+            } catch (RuntimeException  e) {
                 autoFace = true;
             }
+
+            try {
+                decelRange = robot.stateConfiguration.variable(groupName, actionName, "decelR").value();
+                decelMin = robot.stateConfiguration.variable(groupName, actionName, "decelM").value();
+            } catch (RuntimeException e) {
+                decelRange = 0;
             }
         }
 
+        if (decelRange > 0) {
+            decelRange = robot.inchesToTicks(decelRange);
+        }
+
+    }
+
     @Override
     public void start() {
         if (!groupName.equals("manual")) {
@@ -87,9 +113,16 @@ public class DriveToCoordinates extends CyberarmState {
 
         double distanceToTarget = Math.hypot(xTarget - robot.getLocationX(), yTarget - robot.getLocationY());
 
+        // deceleration
+        double power = basePower;
+        if (distanceToTarget < decelRange) {
+            power = ((distanceToTarget / decelRange) * (basePower - decelMin)) + decelMin;
+        }
+
         double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle);
         robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]);
 
+        //Once the robot is within the acceptable range,
         if (distanceToTarget > tolerancePos) {
             braking = false;
         } else {

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java

@@ -18,6 +18,7 @@ public class FindWobbleGoal extends CyberarmState {
     private float startRotation;
     private boolean foundGoalRotation;
     private float wobbleGoalRotation;
+    private long timeLimit;
 
     public FindWobbleGoal(Robot robot, String groupName, String actionName) {
         this.robot = robot;
@@ -31,6 +32,7 @@ public class FindWobbleGoal extends CyberarmState {
         turnCheck = robot.stateConfiguration.variable(groupName,actionName,"max").value();
         driveCheck = robot.stateConfiguration.variable(groupName,actionName,"min").value();
         range = robot.stateConfiguration.variable(groupName,actionName,"r").value();
+        timeLimit = robot.stateConfiguration.variable(groupName,actionName,"limit").value();
     }
 
     @Override
@@ -71,12 +73,20 @@ public class FindWobbleGoal extends CyberarmState {
                  double[] powers = robot.getMecanumPowers(wobbleGoalRotation - 90, power*2 , wobbleGoalRotation);
                  robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
              } else {
+                 endSearch();
+             }
+        }
+
+        if (runTime() > timeLimit) {
+            endSearch();
+        }
+    }
+
+    private void endSearch() {
         robot.setDrivePower(0,0,0,0);
         robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
         setHasFinished(true);
     }
-        }
-    }
 
     @Override
     public void telemetry() {

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Park.java

@@ -15,6 +15,8 @@ public class Park extends CyberarmState {
     double parkTolerance;
     double parkPower;
     long parkBrakeTime;
+    double  parkDecelRange;
+    double parkDecelMin;
 
     public Park(Robot robot, String groupName, String actionName) {
         this.robot = robot;
@@ -29,12 +31,15 @@ public class Park extends CyberarmState {
         parkTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto",actionName,"tolPos").value());
         parkPower = robot.stateConfiguration.variable(groupName,actionName,"power").value();
         parkBrakeTime = robot.stateConfiguration.variable(groupName,actionName,"brakeMS").value();
+        parkDecelMin = robot.stateConfiguration.variable(groupName,actionName,"decelM").value();
+        parkDecelRange = robot.stateConfiguration.variable(groupName,actionName,"decelR").value();
     }
 
     @Override
     public void start() {
-        if (Math.abs(robot.getLocationY()) > robot.inchesToTicks(8))
+        if (Math.abs(robot.getLocationY()) > robot.inchesToTicks(8)) {
-        addParallelState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower,parkBrakeTime));
+            addParallelState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower, parkBrakeTime,parkDecelRange,parkDecelMin));
+        }
     }
 
     @Override

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java

@@ -31,7 +31,7 @@ public class ProgressRingBelt extends CyberarmState {
             targetPos = currentPos + robot.ringBeltGap;
             prep();
         } else if (robot.ringBeltStage == 2) {
-            targetPos = currentPos + 240;
+            targetPos = currentPos + 150;
             prep();
             prepLaunch = !robot.initLauncher;
         } else if (robot.ringBeltStage > 2) {

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ResetRingBelt.java

@@ -0,0 +1,57 @@
+package org.timecrafters.UltimateGoal.Competition;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class ResetRingBelt extends CyberarmState {
+
+    private Robot robot;
+    boolean detectedPass;
+    int reducePos;
+    boolean reduceConditionPrev;
+
+    public ResetRingBelt(Robot robot) {
+        this.robot = robot;
+    }
+
+
+    @Override
+    public void start() {
+        robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
+        reducePos = robot.ringBeltMotor.getCurrentPosition() + robot.reduceLaunchPos;
+    }
+
+    @Override
+    public void exec() {
+        //detect when limit switch is initially triggered
+        boolean detectingPass = robot.limitSwitch.isPressed();
+        int beltPos = robot.ringBeltMotor.getCurrentPosition();
+
+        if (detectingPass && !detectedPass) {
+            //finish once the ring belt has cycled all the way through and then returned to
+            //the first receiving position.
+
+
+            robot.ringBeltStage = 0;
+            robot.ringBeltMotor.setTargetPosition(beltPos);
+            robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+            robot.launchMotor.setPower(0);
+            setHasFinished(true);
+        }
+        detectedPass = detectingPass;
+
+        boolean reduceCondition = (beltPos > reducePos);
+        if (reduceCondition && !reduceConditionPrev){
+            robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER);
+
+            //the ring belt stage lets other states know that the robot has finished launching all three rings
+            //and is now returning to loading position.
+
+        }
+        reduceConditionPrev = reduceCondition;
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java

@@ -10,7 +10,6 @@ import android.os.Environment;
 import android.util.Log;
 
 import com.qualcomm.hardware.bosch.BNO055IMU;
-import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
 import com.qualcomm.hardware.rev.RevColorSensorV3;
 import com.qualcomm.hardware.rev.RevTouchSensor;
@@ -86,6 +85,7 @@ public class Robot {
     static final double FACE_MOMENTUM_MAX_CORRECTION = 1.1;
     static final double FACE_MOMENTUM_CORRECTION = 1.06;
     static final double FACE_MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(FACE_MOMENTUM_MAX_CORRECTION-1)/Math.log10(FACE_MOMENTUM_CORRECTION));
+    static final double ZERO_POWER_THRESHOLD = 0.25;
 
     //Unit Conversion Constants
     static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622;
@@ -432,18 +432,12 @@ public class Robot {
         locationY += yChange;
 
         Robot.rotation += rotationChange;
-
+        Robot.rotation = scaleAngleRange(Robot.rotation);
 
         totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange);
-
-        if (Robot.rotation > 180) {
-            Robot.rotation -= 360;
-        }
-        if (Robot.rotation < -180) {
-            Robot.rotation += 360;
     }
 
-    }
+
 
     public void syncIfStationary() {
         if (totalV < minCheckVelocity) {
@@ -551,16 +545,7 @@ public class Robot {
     //to the right (clockwise) of the current. Negative angles indicate that the reference is to the
     //left.
     public float getRelativeAngle(float reference, float current) {
-        float relative = current - reference;
+        return scaleAngleRange(current - reference);
-
-        if (relative < -180) {
-            relative += 360;
-        }
-
-        if (relative > 180) {
-            relative -= 360;
-        }
-        return relative;
     }
 
     //Drive Functions
@@ -628,6 +613,15 @@ public class Robot {
 
         double[] powers = {powerForwardLeft, powerForwardRight, powerBackLeft, powerBackRight};
 
+        double totalPower = Math.abs(powerForwardLeft) +
+                Math.abs(powerForwardRight) +
+                Math.abs(powerBackLeft) +
+                Math.abs(powerBackRight);
+        if (totalPower < ZERO_POWER_THRESHOLD) {
+            powers = new double[] {0,0,0,0};
+        }
+
+
         return powers;
     }
 
@@ -635,7 +629,8 @@ public class Robot {
     public double[] getFacePowers(float direction, double power) {
         angularVelocity = imu.getAngularVelocity().xRotationRate;
         double relativeAngle = getRelativeAngle(direction, Robot.rotation);
-        double scaler = Math.pow(FACE_CUBIC_CORRECTION * relativeAngle, 3) + FACE_LINEAR_CORRECTION * relativeAngle;
+        double scaler = Math.pow(FACE_CUBIC_CORRECTION * relativeAngle, 3) +
+                FACE_LINEAR_CORRECTION * relativeAngle;
 
         if (relativeAngle > 0.5) {
             scaler += FACE_MIN_CORRECTION;
@@ -645,7 +640,8 @@ public class Robot {
 
         if (relativeAngle != 0) {
             double momentumRelative =  angularVelocity * (relativeAngle / Math.abs(relativeAngle));
-            double exponential = Math.pow(FACE_MOMENTUM_CORRECTION, FACE_MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
+            double exponential = Math.pow(FACE_MOMENTUM_CORRECTION,
+                    FACE_MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
             double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
 
             scaler *= momentumCorrection;
@@ -655,6 +651,12 @@ public class Robot {
         double right = power *scaler;
 
         double[] powers = {left,right};
+
+        double totalPower = 2 * (Math.abs(left) + Math.abs(right));
+        if (totalPower < ZERO_POWER_THRESHOLD) {
+            powers = new double[] {0,0};
+        }
+
         return powers;
     }
 
@@ -665,6 +667,16 @@ public class Robot {
         return notMoved;
     }
 
+    public float scaleAngleRange(float angle) {
+        if (angle < -180) {
+            angle += 360;
+        }
+        if (angle > 180) {
+            angle -= 360;
+        }
+        return angle;
+    }
+
     public void record(String record) {
         TestingRecord+="\n"+record;
     }

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchControl.java

@@ -0,0 +1,58 @@
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Launch;
+import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
+import org.timecrafters.UltimateGoal.Competition.ResetRingBelt;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class LaunchControl extends CyberarmState {
+
+    private Robot robot;
+    private boolean ranLaunch;
+
+    public LaunchControl(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void start() {
+        robot.launchMotor.setPower(Robot.LAUNCH_POWER);
+
+        if (robot.ringBeltStage > 2) {
+            if (robot.ringBeltStage > 4) {
+                addParallelState(new ResetRingBelt(robot));
+            } else {
+                addParallelState(new LaunchSingle(robot));
+            }
+            ranLaunch = true;
+        }
+    }
+
+    @Override
+    public void exec() {
+        if (childrenHaveFinished()) {
+            if (!ranLaunch) {
+                if (robot.ringBeltStage <= 2) {
+                    addParallelState(new ProgressRingBelt(robot));
+                } else {
+                    addParallelState(new LaunchSingle(robot));
+                    ranLaunch = true;
+                }
+            } else {
+                setHasFinished(true);
+            }
+        }
+    }
+
+    @Override
+    public void telemetry() {
+
+        engine.telemetry.addData("Launch Control children", childrenHaveFinished());
+        for (CyberarmState state : children) {
+            if (!state.getHasFinished()) {
+                engine.telemetry.addLine("" + state.getClass());
+            }
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchSingle.java

@@ -0,0 +1,52 @@
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.UltimateGoal.Competition.UnstickRingBelt;
+
+public class LaunchSingle extends CyberarmState {
+
+    private Robot robot;
+    boolean hasCycled;
+    boolean detectedPass;
+    private int targetPos;
+    private boolean reduceConditionPrev;
+    private double reducePos;
+    private long stuckStartTime;
+
+    public LaunchSingle(Robot robot) {
+        this.robot = robot;
+    }
+
+
+    @Override
+    public void start() {
+        robot.ringBeltStage +=1;
+        robot.ringBeltMotor.setTargetPosition(robot.ringBeltMotor.getCurrentPosition() + robot.ringBeltGap);
+        robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
+    }
+
+    @Override
+    public void exec() {
+        int currentPos = robot.ringBeltMotor.getCurrentPosition();
+        if (currentPos >= targetPos) {
+            setHasFinished(true);
+        }
+
+        if (robot.beltIsStuck() && childrenHaveFinished()) {
+            long currentTime = System.currentTimeMillis();
+            if (stuckStartTime == 0) {
+                stuckStartTime = currentTime;
+            } else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) {
+                addParallelState(new UnstickRingBelt(robot));
+            }
+        } else {
+            stuckStartTime = 0;
+        }
+
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player1.java

@@ -33,18 +33,15 @@ public class Player1 extends CyberarmState {
     private boolean aPrev;
 
     //Drive to launch control
-    private DriveToCoordinates driveToLaunch;
+    private powerShotsControl powerShots;
     private boolean runNextDriveToLaunch;
     private boolean driveToLaunchInputPrev;
 
-    private double launchTolerance;
+    private double endGameX;
-    private double launchPower;
+    private double endGameY;
-    private long launchBrakeTime;
+    private float endGameRot;
 
-    private float launchAngleGoal;
+    private double refinePower;
-    private float launchAnglePower1;
-    private float launchAnglePower2;
-    private float launchAnglePower3;
 
     public Player1(Robot robot) {
         this.robot = robot;
@@ -55,15 +52,11 @@ public class Player1 extends CyberarmState {
         cardinalSnapping = robot.stateConfiguration.variable("tele","control", "cardinalSnapping").value();
         pairSnapping = robot.stateConfiguration.variable("tele","control", "pairSnapping").value();
         faceControlThreshold = robot.stateConfiguration.variable("tele","control", "faceControlT").value();
+        refinePower = robot.stateConfiguration.variable("tele","control", "refPower").value();
 
-        launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("tele","launchPosG","tolPos").value());
+        endGameX = robot.stateConfiguration.variable("tele","_endGameStart","x").value();
-        launchPower = robot.stateConfiguration.variable("tele","launchPosG","power").value();
+        endGameY = robot.stateConfiguration.variable("tele","_endGameStart","y").value();
-        launchBrakeTime = robot.stateConfiguration.variable("tele","launchPosG","brakeMS").value();
+        endGameRot = robot.stateConfiguration.variable("tele","_endGameStart", "r").value();
-
-        launchAngleGoal = robot.stateConfiguration.variable("tele","launchAngles","goal").value();
-        launchAnglePower1 = robot.stateConfiguration.variable("tele","launchAngles","p1").value();
-        launchAnglePower2 = robot.stateConfiguration.variable("tele","launchAngles","p2").value();
-        launchAnglePower3 = robot.stateConfiguration.variable("tele","launchAngles","p3").value();
     }
 
     @Override
@@ -85,9 +78,6 @@ public class Player1 extends CyberarmState {
         }
         lbPrev = lb;
 
-        if (engine.gamepad1.guide) {
-            robot.syncIfStationary();
-        }
 
         runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished());
 
@@ -123,20 +113,25 @@ public class Player1 extends CyberarmState {
         }
         aPrev = a;
 
-        runNextDriveToLaunch = (driveToLaunch == null || driveToLaunch.getHasFinished());
+        runNextDriveToLaunch = (powerShots == null || powerShots.getHasFinished());
 
         boolean driveToLaunchInput = engine.gamepad1.y && !findWobbleInput;
         if (driveToLaunchInput) {
             if (runNextDriveToLaunch && !driveToLaunchInputPrev) {
-                driveToLaunch = new DriveToCoordinates(robot, robot.launchPositionX,robot.launchPositionY,robot.launchRotation,launchTolerance,launchPower,launchBrakeTime);
+                powerShots = new powerShotsControl(robot);
-                addParallelState(driveToLaunch);
+                addParallelState(powerShots);
             }
             faceDirection = robot.getRotation();
         } else if (!runNextDriveToLaunch) {
-            driveToLaunch.setHasFinished(true);
+            powerShots.setHasFinished(true);
         }
         driveToLaunchInputPrev = driveToLaunchInput;
 
+//        if (engine.gamepad1.y) {
+//            robot.setLocalization(endGameRot,endGameX,endGameY);
+//            setHasFinished(true);
+//        }
+
         if (childrenHaveFinished()) {
             //Normal Driver Controls
 
@@ -156,8 +151,14 @@ public class Player1 extends CyberarmState {
             //so that the controller and robot can be re-synced in the event of a bad initial
             //position.
             if (engine.gamepad1.back) {
-                robot.setLocalization(rightJoystickDegrees, robot.getLocationX(), robot.getLocationY());
+                float newRotation = 0;
-                faceDirection = rightJoystickDegrees;
+
+                if (rightJoystickMagnitude != 0) {
+                    newRotation = rightJoystickDegrees;
+                }
+
+                robot.setLocalization(newRotation, robot.getLocationX(), robot.getLocationY());
+                faceDirection = newRotation;
             }
 
             //if the driver is letting go of the face joystick, the robot should maintain it's current face direction.
@@ -168,18 +169,13 @@ public class Player1 extends CyberarmState {
             }
             rightJoystickMagnitudePrevious = rightJoystickMagnitude;
 
-            //sets the launch positions to
+            if (engine.gamepad1.dpad_right) {
-            if (engine.gamepad1.dpad_up) {
+                powers = new double[]{refinePower, -refinePower, refinePower, -refinePower};
-                faceDirection = launchAngleGoal;
+                faceDirection = robot.getRotation();
-            } else if (engine.gamepad1.dpad_right) {
-                faceDirection = launchAnglePower1;
-            } else if (engine.gamepad1.dpad_down) {
-                faceDirection = launchAnglePower2;
             } else if (engine.gamepad1.dpad_left) {
-                faceDirection = launchAnglePower3;
+                powers = new double[]{-refinePower, refinePower, -refinePower, refinePower};
-            }
+                faceDirection = robot.getRotation();
-
+            } else if (leftJoystickMagnitude == 0) {
-            if (leftJoystickMagnitude == 0) {
                 double[] facePowers = robot.getFacePowers(faceDirection, 0.4);
                 powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
             } else {

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java

@@ -58,33 +58,29 @@ public class Player2 extends CyberarmState {
             robot.collectionMotor.setPower(0);
         }
 
+        if (childrenHaveFinished()) {
             //belt progression control
             boolean rb = engine.gamepad2.right_bumper;
-        if (rb && !rbPrev && childrenHaveFinished()) {
+            if (rb && !rbPrev) {
                 addParallelState(new ProgressRingBelt(robot));
             }
             rbPrev = rb;
 
-        //launch sequence control
+            //main launch sequence control
             boolean y2 = engine.gamepad2.y;
-        if (y2 && !yPrev && childrenHaveFinished()) {
+            if (y2 && !yPrev) {
                 addParallelState(new Launch(robot));
             }
             yPrev = y2;
 
 
-
+            //special launch sequence for single shots
-        //toggles the wobble arm up and down.
+            boolean x = engine.gamepad2.x;
-//        boolean b = engine.gamepad2.b;
+            if (x && !bPrev) {
-//        if (b && !bPrev) {
+                addParallelState(new LaunchControl(robot));
-//            wobbleArmUp = !wobbleArmUp;
+            }
-//            if (wobbleArmUp) {
+            bPrev = x;
-//                robot.wobbleArmMotor.setTargetPosition(550);
+        }
-//            } else {
-//                robot.wobbleArmMotor.setTargetPosition(0);
-//            }
-//        }
-//        bPrev = b;
 
         //manually control the wobble arm for when it's initialized in an unexpected position.
         double leftStickY = engine.gamepad2.left_stick_y;
@@ -142,8 +138,11 @@ public class Player2 extends CyberarmState {
             robot.ringBeltMotor.setPower(beltPowerPrev);
             robot.ringBeltMotor.setMode(runModePrev);
         }
-
         lbPrev = lb;
+
+//        if (engine.gamepad1.y) {
+//            setHasFinished(true);
+//        }
     }
 
     private void setArmMode(DcMotor.RunMode runMode) {
@@ -159,14 +158,15 @@ public class Player2 extends CyberarmState {
 //        engine.telemetry.addData("pos", robot.ringBeltMotor.getCurrentPosition());
 //        engine.telemetry.addData("target", robot.ringBeltMotor.getTargetPosition());
 
-        engine.telemetry.addData("Touch Sensor Pressed", robot.wobbleTouchSensor.isPressed());
+//        engine.telemetry.addData("ring belt stage", robot.ringBeltStage);
-        engine.telemetry.addData("  Sensor value", robot.wobbleTouchSensor.getValue());
+//        engine.telemetry.addData("Touch Sensor Pressed", robot.wobbleTouchSensor.isPressed());
-        engine.telemetry.addData("Player 2 children", childrenHaveFinished());
+//        engine.telemetry.addData("  Sensor value", robot.wobbleTouchSensor.getValue());
-        for (CyberarmState state : children) {
+//        engine.telemetry.addData("Player 2 children", childrenHaveFinished());
-            if (!state.getHasFinished()) {
+//        for (CyberarmState state : children) {
-                engine.telemetry.addLine("" + state.getClass());
+//            if (!state.getHasFinished()) {
-            }
+//                engine.telemetry.addLine("" + state.getClass());
-        }
+//            }
+//        }
     }
 
 }

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java

@@ -3,6 +3,8 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.Face;
 import org.timecrafters.UltimateGoal.Competition.Robot;
 
 @TeleOp (name = "TeleOp",group = "comp")
@@ -14,7 +16,7 @@ public class TeleOpEngine extends CyberarmEngine {
     public void init() {
         robot = new Robot(hardwareMap);
         robot.initHardware();
-        robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
+        robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
         robot.webCamServo.setPosition(0);
         super.init();
     }
@@ -22,6 +24,8 @@ public class TeleOpEngine extends CyberarmEngine {
     @Override
     public void setup() {
         addState(new TeleOpState(robot));
+
+
     }
 
     @Override

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java

@@ -25,7 +25,7 @@ public class TeleOpState extends CyberarmState {
 
     @Override
     public void exec() {
-
+//        setHasFinished(childrenHaveFinished());
     }
 
     @Override

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/powerShotsControl.java

@@ -0,0 +1,75 @@
+package org.timecrafters.UltimateGoal.Competition.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.Face;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+import java.util.ArrayList;
+
+public class powerShotsControl extends CyberarmState {
+
+    private Robot robot;
+
+    private double endGameX;
+    private double endGameY;
+    private float endGameRot;
+
+    private int nextState = 0;
+
+    private ArrayList<CyberarmState> states = new ArrayList<CyberarmState>();
+
+    public powerShotsControl(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        endGameX = robot.stateConfiguration.variable("tele","_endGameStart","x").value();
+        endGameY = robot.stateConfiguration.variable("tele","_endGameStart","y").value();
+        endGameRot = robot.stateConfiguration.variable("tele","_endGameStart", "r").value();
+
+        endGameX = robot.inchesToTicks(endGameX);
+        endGameY = robot.inchesToTicks(endGameY);
+        endGameRot = (float) robot.inchesToTicks(endGameRot);
+
+        states.add(new DriveToCoordinates(robot, "tele", "_pow1"));
+        states.add(new Face(robot,"tele","_faceZero"));
+        states.add(new LaunchControl(robot));
+        states.add(new DriveToCoordinates(robot, "tele", "_pow2"));
+        states.add(new Face(robot,"tele","_faceZero"));
+        states.add(new LaunchControl(robot));
+        states.add(new DriveToCoordinates(robot, "tele", "_pow3"));
+        states.add(new Face(robot,"tele","_faceZero"));
+        states.add(new LaunchControl(robot));
+    }
+
+    @Override
+    public void start() {
+        robot.setLocalization(endGameRot,endGameX,endGameY);
+        robot.launchMotor.setPower(Robot.LAUNCH_POWER);
+    }
+
+    @Override
+    public void exec() {
+        if (childrenHaveFinished()) {
+            if (nextState < states.size()) {
+                addParallelState(states.get(nextState));
+                nextState += 1;
+            } else {
+                robot.launchMotor.setPower(0);
+                setHasFinished(true);
+            }
+        }
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Power shots children", childrenHaveFinished());
+        for (CyberarmState state : children) {
+            if (!state.getHasFinished()) {
+                engine.telemetry.addLine("" + state.getClass());
+            }
+        }
+    }
+}