Driver controlled Enhancements & Ring belt debugging

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

@@ -41,6 +41,11 @@ public class AutoEngine extends CyberarmEngine {
         // since we've preloaded three rings, the ring belt stage is set to account for this;
         robot.ringBeltStage = 3;
 
+        float rotation = robot.stateConfiguration.variable("system", "startPos", "direction").value();
+        double locationX = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "x").value());
+        double locationY = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "y").value());
+        robot.setLocalization(rotation,locationX,locationY);
+
         launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value());
         launchPower = robot.stateConfiguration.variable("auto","04_0","power").value();
         launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","brakeMS").value();

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo1.java

@@ -0,0 +1,164 @@
+package org.timecrafters.UltimateGoal.Competition.Demo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
+import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class Demo1 extends CyberarmState {
+    private Robot robot;
+
+    //normal drive control
+    private float leftJoystickDegrees;
+    private double leftJoystickMagnitude;
+    private float rightJoystickDegrees;
+    private double rightJoystickMagnitude;
+    private double rightJoystickMagnitudePrevious;
+
+    private double faceControlThreshold;
+    private float cardinalSnapping;
+    private float pairSnapping;
+
+    private float faceDirection = 0;
+    private double[] powers = {0,0,0,0};
+    private double drivePower = 1;
+    private boolean lbPrev;
+
+    //find wobble goal control
+    private FindWobbleGoal findWobbleGoal;
+    private boolean runNextFindWobble;
+    private boolean findWobbleInputPrev;
+
+    //Drive to launch control
+    private DriveToCoordinates driveToLaunch;
+    private boolean runNextDriveToLaunch;
+    private boolean driveToLaunchInputPrev;
+
+    private double launchTolerance;
+    private double launchPower;
+    private long launchBrakeTime;
+
+    private float launchAngleGoal;
+    private float launchAnglePower1;
+    private float launchAnglePower2;
+    private float launchAnglePower3;
+
+    public Demo1(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        cardinalSnapping = robot.stateConfiguration.variable("tele","control", "cardinalSnapping").value();
+        pairSnapping = robot.stateConfiguration.variable("tele","control", "pairSnapping").value();
+        faceControlThreshold = robot.stateConfiguration.variable("tele","control", "faceControlT").value();
+
+    }
+
+    @Override
+    public void start() {
+        faceDirection = robot.getRotation();
+    }
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+
+        boolean lb = engine.gamepad1.left_stick_button;
+        if (lb && !lbPrev) {
+            if (drivePower == 1) {
+                drivePower = 0.5;
+            } else {
+                drivePower = 1;
+            }
+        }
+        lbPrev = lb;
+
+        runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished());
+
+        boolean findWobbleInput = engine.gamepad1.dpad_up;
+        if (findWobbleInput) {
+            if (runNextFindWobble && !findWobbleInputPrev) {
+                findWobbleGoal = new FindWobbleGoal(robot, "auto", "08_0");
+                addParallelState(findWobbleGoal);
+            }
+            faceDirection = robot.getRotation();
+        } else if (!runNextFindWobble) {
+            findWobbleGoal.setHasFinished(true);
+        }
+        findWobbleInputPrev = findWobbleInput;
+
+        if (childrenHaveFinished()) {
+            //Normal Driver Controls
+
+            double leftJoystickX = engine.gamepad1.left_stick_x;
+            double leftJoystickY = engine.gamepad1.left_stick_y;
+
+            leftJoystickDegrees = robot.getRelativeAngle(90, (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)));
+            leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
+
+            double rightJoystickX = engine.gamepad1.right_stick_x;
+            double rightJoystickY = engine.gamepad1.right_stick_y;
+
+            rightJoystickDegrees = robot.getRelativeAngle(90, (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)));
+            rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
+
+            //allows the the driver to indicate which direction the robot is currently looking so
+            //so that the controller and robot can be re-synced in the event of a bad initial
+            //position.
+            if (engine.gamepad1.right_stick_button) {
+                robot.setLocalization(rightJoystickDegrees, robot.getLocationX(), robot.getLocationY());
+                faceDirection = rightJoystickDegrees;
+            }
+
+            //if the driver is letting go of the face joystick, the robot should maintain it's current face direction.
+            if (rightJoystickMagnitude > faceControlThreshold || rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) {
+
+                //if the joystick is close to one of the cardinal directions, it is set to exactly the cardinal direction
+                faceDirection = snapToCardinal(rightJoystickDegrees, cardinalSnapping, 0);
+            }
+            rightJoystickMagnitudePrevious = rightJoystickMagnitude;
+
+
+            if (leftJoystickMagnitude == 0) {
+                double[] facePowers = robot.getFacePowers(faceDirection, 0.4);
+                powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
+            } else {
+                //drives the robot in the direction of the move joystick while facing the direction
+                //of the look joystick. if the move direction is almost aligned/perpendicular to the
+                //look joystick,
+                powers = robot.getMecanumPowers(snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection), drivePower, faceDirection);
+            }
+
+            robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
+        }
+
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addLine("Controler Directions");
+        engine.telemetry.addData("Right", rightJoystickDegrees);
+        engine.telemetry.addData("Left", leftJoystickDegrees);
+
+        engine.telemetry.addData("face", faceDirection);
+
+        engine.telemetry.addData("Player 1 children", childrenHaveFinished());
+        for (CyberarmState state : children) {
+            if (!state.getHasFinished()) {
+                engine.telemetry.addLine("" + state.getClass());
+            }
+        }
+    }
+
+    private float snapToCardinal(float angle, float snapping, float offset) {
+        int o = (int) offset + 180;
+        o %= 90;
+        for (int cardinal = o-180; (cardinal <= 180+o && cardinal != angle); cardinal += 90) {
+            if (angle >= cardinal-snapping && angle <= cardinal+snapping) {
+                angle = cardinal;
+            }
+        }
+        return angle;
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo2.java

@@ -0,0 +1,142 @@
+package org.timecrafters.UltimateGoal.Competition.Demo;
+
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Launch;
+import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+
+public class Demo2 extends CyberarmState {
+    private Robot robot;
+
+    private boolean rbPrev;
+    private boolean yPrev;
+    private boolean xPrev;
+    private boolean bPrev;
+    private boolean wobbleArmUp = false;
+    private boolean wobbleGrabOpen = false;
+    private boolean aPrev;
+    private double beltPowerPrev;
+    private boolean lbPrev;
+    private boolean manualArmHold;
+
+    private boolean launchInput = false;
+
+    public Demo2(Robot robot) {
+        this.robot = robot;
+    }
+
+
+    @Override
+    public void init() {
+        robot.wobbleArmMotor.setTargetPosition(0);
+        robot.wobbleArmMotor.setPower(0.5);
+    }
+
+    @Override
+    public void start() {
+        robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
+    }
+
+    @Override
+    public void exec() {
+
+        //Collector control
+        if (childrenHaveFinished()) {
+            robot.collectionMotor.setPower(engine.gamepad1.right_trigger);
+        } else {
+            robot.collectionMotor.setPower(0);
+        }
+
+        //belt progression control
+        boolean rb = engine.gamepad1.right_bumper;
+        if (rb && !rbPrev && childrenHaveFinished()) {
+            addParallelState(new ProgressRingBelt(robot));
+        }
+        rbPrev = rb;
+
+        //launch sequence control
+        boolean y2 = engine.gamepad1.y;
+        if (y2 && !yPrev && childrenHaveFinished()) {
+            addParallelState(new Launch(robot));
+        }
+        yPrev = y2;
+
+        //toggles wobble grabber open and closed
+        boolean x = engine.gamepad1.x;
+        if (x && !xPrev) {
+            wobbleGrabOpen = !wobbleGrabOpen;
+            if (wobbleGrabOpen) {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
+            } else {
+                robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX * 0.05 );
+            }
+        }
+        xPrev = x;
+
+        //toggles the wobble arm up and down.
+        boolean b = engine.gamepad1.b;
+        if (b && !bPrev) {
+            wobbleArmUp = !wobbleArmUp;
+            if (wobbleArmUp) {
+                robot.wobbleArmMotor.setTargetPosition(550);
+            } else {
+                robot.wobbleArmMotor.setTargetPosition(0);
+            }
+        }
+        bPrev = b;
+
+        //manually toggle the launch wheel for emergencies
+        boolean a = engine.gamepad1.a;
+        if (a && !aPrev) {
+            if (robot.launchMotor.getPower() == 0) {
+                robot.launchMotor.setPower(Robot.LAUNCH_POWER);
+            } else {
+                robot.launchMotor.setPower(0);
+            }
+        }
+        aPrev = a;
+
+        //manually control the wobble arm for when it's initialized in an unexpected position.
+        if (engine.gamepad1.dpad_up) {
+            robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            robot.wobbleArmMotor.setPower(0.5);
+            manualArmHold = true;
+        } else if (engine.gamepad1.dpad_down) {
+            robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            robot.wobbleArmMotor.setPower(-0.1);
+            manualArmHold = true;
+        } else if (manualArmHold) {
+            manualArmHold = false;
+            robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
+            robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        }
+
+        //allows the driver to revers the belt in the event of a jam
+        boolean lb = engine.gamepad1.left_bumper;
+        if (lb && !lbPrev) {
+            beltPowerPrev = robot.ringBeltMotor.getPower();
+            robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER);
+        }
+
+        if (!lb && lbPrev) {
+            robot.ringBeltMotor.setPower(beltPowerPrev);
+        }
+
+        lbPrev = lb;
+
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Player 2 children", childrenHaveFinished());
+        for (CyberarmState state : children) {
+            if (!state.getHasFinished()) {
+                engine.telemetry.addLine("" + state.getClass());
+            }
+        }
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoControl.java

@@ -0,0 +1,39 @@
+package org.timecrafters.UltimateGoal.Competition.Demo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.UltimateGoal.Competition.TeleOp.Player1;
+import org.timecrafters.UltimateGoal.Competition.TeleOp.Player2;
+
+public class DemoControl extends CyberarmState {
+
+    private Robot robot;
+
+    public DemoControl(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void start() {
+        addParallelState(new Demo1(robot));
+        addParallelState(new Demo2(robot));
+    }
+
+    @Override
+    public void exec() {
+
+    }
+
+    @Override
+    public void telemetry() {
+
+        engine.telemetry.addLine("Location");
+        engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")");
+        engine.telemetry.addData("Rotation ", robot.getRotation());
+    }
+
+    private float round(double number,double unit) {
+        return (float) (Math.floor(number/unit) * unit);
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java

@@ -0,0 +1,34 @@
+package org.timecrafters.UltimateGoal.Competition.Demo;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.Competition.Robot;
+import org.timecrafters.UltimateGoal.Competition.TeleOp.TeleOpState;
+
+@TeleOp (name = "Demo")
+public class DemoEngine extends CyberarmEngine {
+
+    private Robot robot;
+
+    @Override
+    public void init() {
+        robot = new Robot(hardwareMap);
+        robot.initHardware();
+        robot.wobbleGrabServo.setPosition(0);
+        robot.webCamServo.setPosition(0);
+        super.init();
+    }
+
+    @Override
+    public void setup() {
+        addState(new DemoControl(robot));
+    }
+
+    @Override
+    public void stop() {
+        robot.deactivateVuforia();
+        robot.saveRecording();
+        super.stop();
+    }
+}

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

@@ -1,6 +1,7 @@
 package org.timecrafters.UltimateGoal.Competition;
 
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
 
@@ -29,14 +30,15 @@ public class Launch extends CyberarmState {
     public void start() {
         try {
             if (robot.stateConfiguration.action(groupName, actionName).enabled) {
-                robot.ringBeltMotor.setPower(0.5);
-                robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_BLUE);
+                robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.ringBeltMotor.setPower(0.7);
+
             } else {
                 setHasFinished(true);
             }
         } catch (NullPointerException e){
-            robot.ringBeltMotor.setPower(0.5);
-            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_BLUE);
+            robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            robot.ringBeltMotor.setPower(0.7);
         }
 
     }
@@ -74,7 +76,7 @@ public class Launch extends CyberarmState {
                 robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
             } else {
                 hasCycled = true;
-                reducePos = (int) (beltPos + (1.5 * Robot.RING_BELT_GAP));
+                reducePos = (int) (beltPos + (robot.reduceLaunchPos));
             }
         }
         detectedPass = detectingPass;

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

@@ -1,6 +1,7 @@
 package org.timecrafters.UltimateGoal.Competition;
 
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
 
@@ -15,20 +16,24 @@ public class ProgressRingBelt extends CyberarmState {
         this.robot = robot;
     }
 
+    private void prep(){
+        robot.ringBeltMotor.setTargetPosition(targetPos);
+        robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.ringBeltMotor.setPower(0.7);
+        robot.ringBeltStage += 1;
+        robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE);
+    }
+
     @Override
     public void start() {
         int currentPos = robot.ringBeltMotor.getCurrentPosition();
         if (robot.ringBeltStage < 2) {
             targetPos = currentPos + Robot.RING_BELT_GAP;
-            robot.ringBeltOn();
-            robot.ringBeltStage += 1;
-            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE);
+            prep();
         } else if (robot.ringBeltStage == 2) {
-            targetPos = currentPos + 160;
-            robot.ringBeltOn();
-            robot.ringBeltStage += 1;
+            targetPos = currentPos + 240;
+            prep();
             prepLaunch = !robot.initLauncher;
-            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE);
         } else if (robot.ringBeltStage > 2) {
             setHasFinished(true);
         }
@@ -41,8 +46,6 @@ public class ProgressRingBelt extends CyberarmState {
 
         int currentPos = robot.ringBeltMotor.getCurrentPosition();
         if (currentPos >= targetPos) {
-            robot.ringBeltMotor.setPower(0);
-            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
             if(prepLaunch) {
                 robot.launchMotor.setPower(Robot.LAUNCH_POWER);
             }

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

@@ -63,11 +63,16 @@ public class Robot {
     public DcMotor encoderBack;
 
     //Steering Constants
-    static final double FINE_CORRECTION = 0.055 ;
-    static final double LARGE_CORRECTION = 0.025;
+    static final double CUBIC_CORRECTION = 0.025;
+    static final double LINEAR_CORRECTION = 0.055;
+    static final double FACE_MIN_CORRECTION = 0.2;
+    static final double FACE_LINEAR_CORRECTION = 0.025;
     static final double MOMENTUM_CORRECTION = 1.05;
     static final double MOMENTUM_MAX_CORRECTION = 1.4;
     static final double MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(MOMENTUM_MAX_CORRECTION-1)/Math.log10(MOMENTUM_CORRECTION));
+    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));
 
     //Conversion Constants
     static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622;
@@ -89,9 +94,9 @@ public class Robot {
     static final float CAMERA_DISPLACEMENT_DIRECTION = (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT);
 
     //Robot Localization
-    public double locationX;
-    public double locationY;
-    private float rotation;
+    private static double locationX;
+    private static double locationY;
+    private static float rotation;
 
     private int encoderLeftPrevious = 0;
     private int encoderBackPrevious = 0;
@@ -108,6 +113,7 @@ public class Robot {
     public double launchPositionX;
     public double launchPositionY;
     public float launchRotation;
+    public int reduceLaunchPos;
     public static final double LAUNCH_TOLERANCE_POS = 0.5 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
     public static final double LAUNCH_TOLERANCE_FACE = 0.5;
 
@@ -125,7 +131,7 @@ public class Robot {
     public RevTouchSensor limitSwitch;
     public int ringBeltStage;
     public static final int RING_BELT_LOOP_TICKS = 2544;
-    public static final int RING_BELT_GAP = 670;
+    public static final int RING_BELT_GAP = 700;
     public static final double RING_BELT_POWER = 0.2;
     private int ringBeltPrev;
     public long beltMaxStopTime;
@@ -253,10 +259,6 @@ public class Robot {
         webCamServo = hardwareMap.servo.get("look");
         webCamServo.setDirection(Servo.Direction.REVERSE );
 
-        rotation = stateConfiguration.variable("system", "startPos", "direction").value();
-        locationX = inchesToTicks((double) stateConfiguration.variable("system", "startPos", "x").value());
-        locationY = inchesToTicks((double) stateConfiguration.variable("system", "startPos", "y").value());
-
         minCheckVelocity =stateConfiguration.variable("system", "tensorFlow", "minCheckV").value();
         minCheckDurationMs =stateConfiguration.variable("system", "tensorFlow", "minCheckMS").value();
 
@@ -274,6 +276,7 @@ public class Robot {
         launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
 
         initLauncher = stateConfiguration.action("system","initLauncher").enabled;
+        reduceLaunchPos = stateConfiguration.variable("system", "launchPos", "reducePower").value();
 
         if (initLauncher) {
             double launcherPower = 0;
@@ -399,7 +402,7 @@ public class Robot {
         sidewaysVector = encoderBackChange + (rotationChange * ticksPerDegreeSideways);
 
         double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector));
-        double direction = Math.toRadians(rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector);
+        double direction = Math.toRadians(Robot.rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector);
 
         double xChange = magnitude * (Math.sin(direction));
         double yChange = magnitude * (Math.cos(direction));
@@ -407,31 +410,31 @@ public class Robot {
         locationX += xChange;
         locationY += yChange;
 
-        rotation += rotationChange;
+        Robot.rotation += rotationChange;
 
 
-        totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange);
+//        totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange);
+
+//
+//        if (totalV < minCheckVelocity) {
+//            long timeCurrent = System.currentTimeMillis();
+//
+//            if (timeStartZeroVelocity == 0) {
+//                timeStartZeroVelocity = timeCurrent;
+//            } else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs) {
+//                syncWithVuforia();
+//            }
+//
+//        } else {
+//            timeStartZeroVelocity = 0;
+//        }
 
 
-        if (totalV < minCheckVelocity) {
-            long timeCurrent = System.currentTimeMillis();
-
-            if (timeStartZeroVelocity == 0) {
-                timeStartZeroVelocity = timeCurrent;
-            } else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs) {
-                syncWithVuforia();
-            }
-
-        } else {
-            timeStartZeroVelocity = 0;
+        if (Robot.rotation > 180) {
+            Robot.rotation -= 360;
         }
-
-
-        if (rotation > 180) {
-            rotation -= 360;
-        }
-        if (rotation < -180) {
-            rotation += 360;
+        if (Robot.rotation < -180) {
+            Robot.rotation += 360;
         }
 
     }
@@ -452,18 +455,18 @@ public class Robot {
                 //For our tournament, it makes sense to make zero degrees towards the goal.
                 //Orientation is inverted to have clockwise be positive.
                 Orientation rotation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES);
-                this.rotation = 90-rotation.thirdAngle;
+                Robot.rotation = 90-rotation.thirdAngle;
 
-                if (this.rotation > 180) {
-                    this.rotation -= -180;
+                if (Robot.rotation > 180) {
+                    Robot.rotation -= -180;
                 }
 
                 VectorF translation = lastConfirmendLocation.getTranslation();
                 double camX = -translation.get(1) / mmPerInch;
                 double camY = translation.get(0) / mmPerInch;
 
-                double displaceX = CAMERA_DISPLACEMENT_MAG * Math.sin(this.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
-                double displaceY = CAMERA_DISPLACEMENT_MAG * Math.cos(this.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
+                double displaceX = CAMERA_DISPLACEMENT_MAG * Math.sin(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
+                double displaceY = CAMERA_DISPLACEMENT_MAG * Math.cos(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
 
                 locationX = inchesToTicks(camX - displaceX);
                 locationY = inchesToTicks(camY - displaceY);
@@ -474,26 +477,28 @@ public class Robot {
     }
 
     public float getRotation() {
-        return rotation;
+        return Robot.rotation;
     }
 
     public double getLocationX() {
-        return locationX;
+        return Robot.locationX;
     }
 
     public double getLocationY() {
-        return locationY;
+        return Robot.locationY;
     }
 
-    public void resetRotation(float rotation) {
-        this.rotation = rotation;
+    public void setLocalization(float rotation, double x, double y) {
+        Robot.rotation = rotation;
+        Robot.locationX = x;
+        Robot.locationY = y;
     }
 
     //Manually set the position of the robot on the field.
     public void setCurrentPosition(float rotation, double x, double y) {
-        this.rotation = rotation;
-        locationX = x;
-        locationY = y;
+        Robot.rotation = rotation;
+        Robot.locationX = x;
+        Robot.locationY = y;
     }
 
     //returns the angle from the robot's current position to the given target position.
@@ -560,10 +565,10 @@ public class Robot {
 
         //calculating correction needed to steer the robot towards the degreesDirectionFace
         float relativeRotation =
-                getRelativeAngle(degreesDirectionFace, rotation);
+                getRelativeAngle(degreesDirectionFace, Robot.rotation);
         double turnCorrection =
-                Math.pow(LARGE_CORRECTION * relativeRotation, 3) +
-                        FINE_CORRECTION * relativeRotation;
+                Math.pow(CUBIC_CORRECTION * relativeRotation, 3) +
+                        LINEAR_CORRECTION * relativeRotation;
 
         double powerForwardRight = scalar * (q + turnCorrection);
         double powerForwardLeft = scalar * (p - turnCorrection);
@@ -604,12 +609,18 @@ public class Robot {
     //Outputs the power necessary to turn and face a provided direction
     public double[] getFacePowers(float direction, double power) {
         angularVelocity = imu.getAngularVelocity().xRotationRate;
-        double relativeAngle = getRelativeAngle(direction, rotation);
-        double scaler = Math.pow(LARGE_CORRECTION * relativeAngle, 3) + FINE_CORRECTION * relativeAngle;
+        double relativeAngle = getRelativeAngle(direction, Robot.rotation);
+        double scaler = Math.pow(CUBIC_CORRECTION * relativeAngle, 3) + FACE_LINEAR_CORRECTION * relativeAngle;
+
+        if (relativeAngle > 0.5) {
+            scaler += FACE_MIN_CORRECTION;
+        } else if (relativeAngle < -0.5) {
+            scaler -= FACE_MIN_CORRECTION;
+        }
 
         if (relativeAngle != 0) {
             double momentumRelative =  angularVelocity * (relativeAngle / Math.abs(relativeAngle));
-            double exponential = Math.pow(MOMENTUM_CORRECTION, 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;

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

@@ -1,12 +1,10 @@
 package org.timecrafters.UltimateGoal.Competition.TeleOp;
 
-import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
 
 import org.cyberarm.engine.V2.CyberarmState;
 import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
 import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
-import org.timecrafters.UltimateGoal.Competition.Launch;
-import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
 import org.timecrafters.UltimateGoal.Competition.Robot;
 
 public class Player1 extends CyberarmState {
@@ -42,6 +40,11 @@ public class Player1 extends CyberarmState {
     private double launchPower;
     private long launchBrakeTime;
 
+    private float launchAngleGoal;
+    private float launchAnglePower1;
+    private float launchAnglePower2;
+    private float launchAnglePower3;
+
     public Player1(Robot robot) {
         this.robot = robot;
     }
@@ -52,9 +55,19 @@ public class Player1 extends CyberarmState {
         pairSnapping = robot.stateConfiguration.variable("tele","control", "pairSnapping").value();
         faceControlThreshold = robot.stateConfiguration.variable("tele","control", "faceControlT").value();
 
-        launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value());
-        launchPower = robot.stateConfiguration.variable("auto","04_0","power").value();
-        launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","brakeMS").value();
+        launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("tele","launchPosG","tolPos").value());
+        launchPower = robot.stateConfiguration.variable("tele","launchPosG","power").value();
+        launchBrakeTime = robot.stateConfiguration.variable("tele","launchPosG","brakeMS").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
+    public void start() {
+        faceDirection = robot.getRotation();
     }
 
     @Override
@@ -115,6 +128,14 @@ public class Player1 extends CyberarmState {
             rightJoystickDegrees = robot.getRelativeAngle(90, (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)));
             rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
 
+            //allows the the driver to indicate which direction the robot is currently looking
+            //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());
+                faceDirection = rightJoystickDegrees;
+            }
+
             //if the driver is letting go of the face joystick, the robot should maintain it's current face direction.
             if (rightJoystickMagnitude > faceControlThreshold || rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) {
 
@@ -123,15 +144,19 @@ public class Player1 extends CyberarmState {
             }
             rightJoystickMagnitudePrevious = rightJoystickMagnitude;
 
-            //allows the the driver to indicate which direction the robot is currently looking so
-            //so that the controller and robot can be re-synced in the event of a bad initial
-            //position.
-            if (engine.gamepad1.right_stick_button) {
-                robot.resetRotation(faceDirection);
+            //sets the launch positions to
+            if (engine.gamepad1.dpad_up) {
+                faceDirection = launchAngleGoal;
+            } else if (engine.gamepad1.dpad_right) {
+                faceDirection = launchAnglePower1;
+            } else if (engine.gamepad1.dpad_down) {
+                faceDirection = launchAnglePower2;
+            } else if (engine.gamepad1.dpad_left) {
+                faceDirection = launchAnglePower3;
             }
 
             if (leftJoystickMagnitude == 0) {
-                double[] facePowers = robot.getFacePowers(faceDirection, drivePower);
+                double[] facePowers = robot.getFacePowers(faceDirection, 0.4);
                 powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
             } else {
                 //drives the robot in the direction of the move joystick while facing the direction
@@ -143,10 +168,29 @@ public class Player1 extends CyberarmState {
             robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
         }
 
+
+        double ringBeltPower = robot.ringBeltMotor.getPower();
+        if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() - robot.ringBeltMotor.getCurrentPosition()) > 10) {
+            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE );
+        } else if (ringBeltPower < 0) {
+            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED);
+        }  else {
+            if (drivePower == 1) {
+                robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
+            } else {
+                robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GOLD);
+            }
+        }
     }
 
     @Override
     public void telemetry() {
+        engine.telemetry.addLine("Controler Directions");
+        engine.telemetry.addData("Right", rightJoystickDegrees);
+        engine.telemetry.addData("Left", leftJoystickDegrees);
+
+        engine.telemetry.addData("face", faceDirection);
+
         engine.telemetry.addData("Player 1 children", childrenHaveFinished());
         for (CyberarmState state : children) {
             if (!state.getHasFinished()) {

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

@@ -19,6 +19,7 @@ public class Player2 extends CyberarmState {
     private boolean wobbleGrabOpen = false;
     private boolean aPrev;
     private double beltPowerPrev;
+    private DcMotor.RunMode runModePrev;
     private boolean lbPrev;
     private boolean manualArmHold;
 
@@ -45,7 +46,13 @@ public class Player2 extends CyberarmState {
 
         //Collector control
         if (childrenHaveFinished()) {
-            robot.collectionMotor.setPower(engine.gamepad2.right_trigger);
+            double rt = engine.gamepad2.right_trigger;
+            double lt = engine.gamepad2.left_trigger;
+            if (rt >= lt) {
+                robot.collectionMotor.setPower(rt);
+            } else {
+                robot.collectionMotor.setPower(-lt);
+            }
         } else {
             robot.collectionMotor.setPower(0);
         }
@@ -117,20 +124,27 @@ public class Player2 extends CyberarmState {
         //allows the driver to revers the belt in the event of a jam
         boolean lb = engine.gamepad2.left_bumper;
         if (lb && !lbPrev) {
+            runModePrev = robot.ringBeltMotor.getMode();
             beltPowerPrev = robot.ringBeltMotor.getPower();
+            robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
             robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER);
         }
 
         if (!lb && lbPrev) {
             robot.ringBeltMotor.setPower(beltPowerPrev);
+            robot.ringBeltMotor.setMode(runModePrev);
         }
 
         lbPrev = lb;
-
     }
 
     @Override
     public void telemetry() {
+        engine.telemetry.addLine("belt");
+        engine.telemetry.addData("power", robot.ringBeltMotor.getPower());
+        engine.telemetry.addData("pos", robot.ringBeltMotor.getCurrentPosition());
+        engine.telemetry.addData("target", robot.ringBeltMotor.getTargetPosition());
+
         engine.telemetry.addData("Player 2 children", childrenHaveFinished());
         for (CyberarmState state : children) {
             if (!state.getHasFinished()) {

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

@@ -1,6 +1,7 @@
 package org.timecrafters.UltimateGoal.Competition;
 
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
 
@@ -9,6 +10,7 @@ public class UnstickRingBelt extends CyberarmState {
     private Robot robot;
     private int targetPos;
     private double lastPower;
+    private DcMotor.RunMode lastRunMode;
 
     public UnstickRingBelt(Robot robot) {
         this.robot = robot;
@@ -17,10 +19,10 @@ public class UnstickRingBelt extends CyberarmState {
     @Override
     public void start() {
         lastPower = robot.ringBeltMotor.getPower();
+        lastRunMode = robot.ringBeltMotor.getMode();
         int currentPos = robot.ringBeltMotor.getCurrentPosition();
         targetPos = currentPos - robot.beltReverseTicks;
         robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER);
-        robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED);
     }
 
     @Override