Documentation and Mecanum Debugging

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/Break.java

@@ -0,0 +1,43 @@
+package org.timecrafters.UltimateGoal.Calibration;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class Break extends CyberarmState {
+
+    public DcMotor driveBackLeft;
+    public DcMotor driveFrontLeft;
+    public DcMotor driveBackRight;
+    public DcMotor driveFrontRight;
+
+    @Override
+    public void init() {
+        driveFrontLeft = engine.hardwareMap.dcMotor.get("driveFrontLeft");
+        driveFrontRight = engine.hardwareMap.dcMotor.get("driveFrontRight");
+        driveBackLeft = engine.hardwareMap.dcMotor.get("driveBackLeft");
+        driveBackRight = engine.hardwareMap.dcMotor.get("driveBackRight");
+
+        driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
+        driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        driveFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        driveFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        driveBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        driveBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    @Override
+    public void start() {
+
+    }
+
+    @Override
+    public void exec() {
+        driveFrontLeft.setPower(0);
+        driveFrontRight.setPower(0);
+        driveBackLeft.setPower(0);
+        driveBackRight.setPower(0);
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java

@@ -0,0 +1,26 @@
+package org.timecrafters.UltimateGoal.Calibration;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.UltimateGoal.HardwareTesting.MecanumFunctionTest;
+import org.timecrafters.UltimateGoal.Robot;
+
+@TeleOp (name = "Calibration", group = "test")
+public class CalibrationEngine extends CyberarmEngine {
+    private Robot robot;
+
+    @Override
+    public void init() {
+        robot = new Robot(hardwareMap);
+        robot.initHardware();
+        super.init();
+    }
+
+
+    @Override
+    public void setup() {
+        addState(new MechanumBiasCalibrator(robot));
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java

@@ -9,7 +9,7 @@ import java.util.ArrayList;
 public class MechanumBiasCalibrator extends CyberarmState {
 
     private Robot robot;
-    private ArrayList<double[]> Powers;
+    private ArrayList<double[]> Powers = new ArrayList<>();
     private double BiasFR;
     private double BiasFL;
     private double BiasBR;
@@ -26,11 +26,17 @@ public class MechanumBiasCalibrator extends CyberarmState {
         robot.updateLocation();
 
         if (engine.gamepad1.x) {
-            double[] mecanumPowers = robot.getMecanumPowers(0, 1, 0);
+            double[] mecanumPowers = robot.getMecanumPowers(-45, 0.5, 0);
 
-            //TODO: add motors when they exist.
+            
+            robot.driveFrontLeft.setPower(mecanumPowers[0]);
+            robot.driveFrontRight.setPower(mecanumPowers[1]);
+            robot.driveBackLeft.setPower(mecanumPowers[2]);
+            robot.driveBackRight.setPower(mecanumPowers[3]);
 
             Powers.add(mecanumPowers);
+        } else {
+            robot.setDrivePower(0,0,0,0 );
         }
 
         if (engine.gamepad1.y && !hasCalculated) {
@@ -42,10 +48,10 @@ public class MechanumBiasCalibrator extends CyberarmState {
             double sumBL = 0;
 
             for (double[] powers : Powers) {
-                sumFR+= powers[0];
+                sumFL+= Math.abs(powers[0]);
-                sumFL+= powers[1];
+                sumFR+= Math.abs(powers[1]);
-                sumBR+= powers[2];
+                sumBL+= Math.abs(powers[2]);
-                sumBL+= powers[3];
+                sumBR+= Math.abs(powers[3]);
             }
 
             int length = Powers.size();
@@ -54,9 +60,24 @@ public class MechanumBiasCalibrator extends CyberarmState {
             BiasBR = sumBR / length;
             BiasBL = sumBL / length;
 
+            double max = Math.max(Math.max(BiasFL,BiasFR),Math.max(BiasBL,BiasBR));
+
+            BiasFL = BiasFL /max;
+            BiasFR = BiasFR /max;
+            BiasBL = BiasBL /max;
+            BiasBR = BiasBR /max;
+
         } else if (!engine.gamepad1.y) {
             hasCalculated = false;
         }
 
     }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("FrontLeft", BiasFL);
+        engine.telemetry.addData("FrontRight", BiasFR);
+        engine.telemetry.addData("BackLeft", BiasBL);
+        engine.telemetry.addData("BackRight", BiasBR);
+    }
 }

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java

@@ -0,0 +1,34 @@
+package org.timecrafters.UltimateGoal.Calibration;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Robot;
+
+import java.util.ArrayList;
+
+public class PerformanceTest extends CyberarmState {
+
+    private Robot robot;
+    private ArrayList<Float> angles = new ArrayList<Float>();
+
+
+    public PerformanceTest(Robot robot) {
+        this.robot = robot;
+    }
+
+
+    @Override
+    public void exec() {
+        robot.updateLocation();
+        robot.record();
+
+        if (engine.gamepad1.x) {
+            double[] powers = robot.getMecanumPowers(0, 1, 0);
+            robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
+        } else {
+            robot.setDrivePower(0,0,0,0);
+        }
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java

@@ -0,0 +1,72 @@
+package org.timecrafters.UltimateGoal.Calibration;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Robot;
+
+import java.util.ArrayList;
+
+public class StalPowerCalibrator extends CyberarmState {
+
+    private Robot robot;
+    private double[] Powers;
+    private DcMotor[] Motors;
+    private int currentMotor = 0;
+    private double currentPower = 0.001;
+
+
+
+    public StalPowerCalibrator(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
+        Powers = new double[] {0,0,0,0};
+
+        for (DcMotor motor : Motors) {
+            motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        }
+    }
+
+    @Override
+    public void exec() {
+
+        if (currentMotor <= 3) {
+
+
+
+            if (Math.abs(Motors[currentMotor].getCurrentPosition()) < 10 && currentPower < 1) {
+                currentPower += 0.001;
+                Motors[currentMotor].setPower(currentPower);
+                sleep(20);
+            } else {
+                Powers[currentMotor] = currentPower;
+                Motors[currentMotor].setPower(0);
+                currentPower = 0.001;
+                currentMotor += 1;
+                sleep(1000);
+                for (DcMotor motor : Motors) {
+                    motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                    motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                }
+            }
+        }
+
+    }
+
+    @Override
+    public void telemetry() {
+
+        engine.telemetry.addData("current Power", currentPower);
+        engine.telemetry.addData("current Motor", currentMotor);
+
+        engine.telemetry.addData("FrontLeft", Powers[0]);
+        engine.telemetry.addData("FrontRight", Powers[1]);
+        engine.telemetry.addData("BackLeft", Powers[2]);
+        engine.telemetry.addData("BackRight", Powers[3]);
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlerTest.java

@@ -0,0 +1,66 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.UltimateGoal.Robot;
+
+
+public class ControlerTest extends CyberarmState {
+
+    private float leftJoystickDegrees;
+    private double leftJoystickMagnitude;
+    private float rightJoystickDegrees;
+    private double rightJoystickMagnitude;
+    private double leftJoystickX;
+    private double leftJoystickY;
+    private double rightJoystickX;
+    private double rightJoystickY;
+
+
+
+
+    @Override
+    public void exec() {
+
+        leftJoystickX = engine.gamepad1.left_stick_x;
+        leftJoystickY = engine.gamepad1.left_stick_y;
+
+        leftJoystickDegrees = (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY));
+        leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
+
+        rightJoystickX = engine.gamepad1.left_stick_x;
+        rightJoystickY = engine.gamepad1.left_stick_y;
+
+        rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
+        rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
+
+
+
+    }
+
+    @Override
+    public void telemetry() {
+
+        engine.telemetry.addLine("Left Joystick");
+        engine.telemetry.addData("Pos", "("+leftJoystickX+","+leftJoystickY+")");
+        engine.telemetry.addData("Angle", leftJoystickDegrees);
+        engine.telemetry.addData("Mag", leftJoystickMagnitude);
+
+        engine.telemetry.addLine("Right Joystick");
+        engine.telemetry.addData("Pos", "("+rightJoystickX+","+rightJoystickY+")");
+        engine.telemetry.addData("Angle", leftJoystickDegrees);
+        engine.telemetry.addData("Mag", leftJoystickMagnitude);
+
+        engine.telemetry.addLine("Buttons");
+        engine.telemetry.addData("a", engine.gamepad1.a);
+        engine.telemetry.addData("b", engine.gamepad1.b);
+        engine.telemetry.addData("x", engine.gamepad1.x);
+        engine.telemetry.addData("y", engine.gamepad1.y);
+
+        engine.telemetry.addLine("Top");
+        engine.telemetry.addData("Left Bump", engine.gamepad1.left_bumper);
+        engine.telemetry.addData("Right Bump", engine.gamepad1.right_bumper);
+        engine.telemetry.addData("Left Trigger", engine.gamepad1.left_trigger);
+        engine.telemetry.addData("Right Trigger", engine.gamepad1.right_trigger);
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java

@@ -12,10 +12,11 @@ public class EncoderTest extends CyberarmState {
         this.robot = robot;
     }
 
+
     @Override
     public void exec() {
 
-        robot.setDrivePower(1, 1,1,1);
+        robot.setDrivePower(0.1, 0.1,0.1,0.1);
 
     }
 }

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java

@@ -11,10 +11,10 @@ public class MecanumFunctionTest extends CyberarmState {
     private double leftJoystickMagnitude;
     private float rightJoystickDegrees;
     private double rightJoystickMagnitude;
-    private double powerFrontLeft;
+    private double powerFrontLeft=0;
-    private double powerFrontRight;
+    private double powerFrontRight=0;
-    private double powerBackLeft;
+    private double powerBackLeft=0;
-    private double powerBackRight;
+    private double powerBackRight=0;
 
     private static double TURN_POWER_SCALE = 0.5;
 
@@ -32,23 +32,25 @@ public class MecanumFunctionTest extends CyberarmState {
         leftJoystickDegrees = (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY));
         leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
 
-        double rightJoystickX = engine.gamepad1.left_stick_x;
+        double rightJoystickX = engine.gamepad1.right_stick_x;
-        double rightJoystickY = engine.gamepad1.left_stick_y;
+        double rightJoystickY = engine.gamepad1.right_stick_y;
 
         rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
         rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
-
+//
         powerFrontLeft = 0;
         powerFrontRight = 0;
         powerBackLeft = 0;
         powerBackRight = 0;
 
         if (rightJoystickMagnitude == 0) {
-            double[] powers = robot.getMecanumPowers(leftJoystickDegrees, leftJoystickMagnitude, leftJoystickDegrees);
+            if (leftJoystickMagnitude !=0) {
-            powerFrontLeft = powers[0];
+                double[] powers = robot.getMecanumPowers(leftJoystickDegrees, leftJoystickMagnitude, leftJoystickDegrees);
-            powerFrontRight = powers[1];
+                powerFrontLeft = powers[0];
-             powerBackLeft = powers[2];
+                powerFrontRight = powers[1];
-            powerBackRight = powers[3];
+                powerBackLeft = powers[2];
+                powerBackRight = powers[3];
+            }
         } else {
             if (leftJoystickMagnitude == 0) {
                 double[] powers =  robot.getFacePowers(rightJoystickDegrees, rightJoystickMagnitude);
@@ -65,6 +67,8 @@ public class MecanumFunctionTest extends CyberarmState {
             }
         }
 
+//        robot.record(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight);
+
         robot.setDrivePower(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight);
 
     }
@@ -77,11 +81,8 @@ public class MecanumFunctionTest extends CyberarmState {
         engine.telemetry.addData("Mag", leftJoystickMagnitude);
 
         engine.telemetry.addLine("Right Joystick");
-        engine.telemetry.addData("Angle", leftJoystickDegrees);
+        engine.telemetry.addData("Angle", rightJoystickDegrees);
-        engine.telemetry.addData("Mag", leftJoystickMagnitude);
+        engine.telemetry.addData("Mag", rightJoystickMagnitude);
 
-//        engine.telemetry.addLine("");
-//        engine.telemetry.addData("Front", robot.encoderFront);
-//        engine.telemetry.addData("Back", robot.encoderBack);
     }
 }

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java

@@ -5,7 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.timecrafters.UltimateGoal.Robot;
 
-@TeleOp (name = "Mecanum test", group = "test")
+@TeleOp (name = "Encoder test", group = "test")
 public class TestingEngine extends CyberarmEngine {
 
     private Robot robot;
@@ -21,11 +21,12 @@ public class TestingEngine extends CyberarmEngine {
 
     @Override
     public void setup() {
-        addState(new EncoderTest(robot));
+        addState(new MecanumFunctionTest(robot));
     }
 
-    @Override
+//    @Override
-    public void stop() {
+//    public void stop() {
-        robot.deactivateVuforia();
+//        robot.saveRecording();
-    }
+//        super.stop();
+//    }
 }

TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java

@@ -37,7 +37,7 @@ public class DriveToPosition extends CyberarmState {
     @Override
     public void exec() {
         robot.updateLocation();
-        robot.record();
+//        robot.record();
 
         double distanceRemaining = Math.hypot(targetX - robot.getLocationX(), targetY - robot.getLocationY());
 

TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java

@@ -48,7 +48,7 @@ public class IMUDrive extends CyberarmState {
 
         int ticksTraveled = Math.abs( robot.driveFrontRight.getCurrentPosition()-tickStart);
         if (ticksTraveled > tickTarget) {
-            robot.setDrivePower(0,0);
+//            robot.setDrivePower(0,0);
             sleep(finishDelay);
             setHasFinished(true);
         } else {
@@ -62,7 +62,7 @@ public class IMUDrive extends CyberarmState {
 
             double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower)));
 
-            robot.setDrivePower(powerAdjust*leftPower, powerAdjust*rightPower);
+//            robot.setDrivePower(powerAdjust*leftPower, powerAdjust*rightPower);
         }
     }
 

TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java

@@ -54,10 +54,10 @@ public class IMUTurn extends CyberarmState {
             turnDirection = OptimalDirection;
         }
 
-        robot.setDrivePower(power * turnDirection,-power * turnDirection);
+//        robot.setDrivePower(power * turnDirection,-power * turnDirection);
 
         if (robot.getRotation() > angleTarget - angleAllowance && robot.getRotation() < angleTarget + angleAllowance ) {
-            robot.setDrivePower(0,0);
+//            robot.setDrivePower(0,0);
             setHasFinished(true);
         }
 

TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java

@@ -24,7 +24,7 @@ public class VuforiaNavTesting extends CyberarmState {
         }
         
         robot.updateLocation();
-        robot.record();
+//        robot.record();
 
         double joystickX = engine.gamepad1.right_stick_x;
         double joystickY = engine.gamepad1.right_stick_y;

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

@@ -57,6 +57,9 @@ public class   Robot {
     static final double BIAS_BACK_LEFT = 1;
     static final double BIAS_BACK_RIGHT = 1;
 
+    static final double FINE_CORRECTION = 0.01;
+    static final double LARGE_CORRECTION = 0.01 ;
+
     //Conversion Constants
     static final double ENCODER_CIRCUMFERENCE = Math.PI * 4;
     static final int COUNTS_PER_REVOLUTION = 8192;
@@ -78,7 +81,8 @@ public class   Robot {
     public double visionX;
     public double visionY;
     public float rawAngle;
-    private String TestingRecord = "X,Y,Angle";
+//    private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight";
+    private String TestingRecord = "Rotation";
 
     public double traveledLeft;
     public double traveledRight;
@@ -101,7 +105,7 @@ public class   Robot {
 
 
     public void initHardware() {
-        webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
+//        webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
 
         imu  = hardwareMap.get(BNO055IMU.class, "imu");
 
@@ -110,15 +114,13 @@ public class   Robot {
         driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft");
         driveBackRight = hardwareMap.dcMotor.get("driveBackRight");
 
-        driveFrontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        driveFrontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        driveFrontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        driveFrontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
         driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
-        
+        driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
+
         driveFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         driveFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        driveBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        driveBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
 
@@ -129,7 +131,7 @@ public class   Robot {
 
         imu.initialize(parameters);
 
-        initVuforia();
+//        initVuforia();
 
         rotation = stateConfiguration.variable("system", "startPos", "direction").value();
         locationX = stateConfiguration.variable("system", "startPos", "x").value();
@@ -197,6 +199,7 @@ public class   Robot {
         tfObjectDetector.loadModelFromAsset("UltimateGoal.tflite", "Quad", "Single");
     }
 
+    //run this in every exec to track the robot's location.
     public void updateLocation(){
         // orientation is inverted to have clockwise be positive.
         float imuAngle = -imu.getAngularOrientation().firstAngle;
@@ -286,12 +289,14 @@ public class   Robot {
         return locationY;
     }
 
+    //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;
     }
 
+    //returns the angle from the robot's current position to the given target position.
     public float getAngleToPosition (double x, double y) {
         double differenceX = x- getLocationX();
         double differenceY = y- getLocationY();
@@ -301,6 +306,7 @@ public class   Robot {
 
     }
 
+    //Unit conversion
     public double ticksToInches(double ticks) {
         return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION);
     }
@@ -310,6 +316,9 @@ public class   Robot {
 
     }
 
+    //Returns the angle between two angles, with positive angles indicating that the reference is
+    //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;
 
@@ -326,13 +335,15 @@ public class   Robot {
     //Drive Functions
     public void setDrivePower(double powerFrontLeft, double powerFrontRight, double powerBackLeft, double powerBackRight){
         driveFrontLeft.setPower(powerFrontLeft * BIAS_FRONT_LEFT);
-        driveFrontRight.setPower(powerFrontLeft * BIAS_FRONT_RIGHT);
+        driveFrontRight.setPower(powerFrontRight * BIAS_FRONT_RIGHT);
         driveBackLeft.setPower(powerBackLeft * BIAS_BACK_LEFT);
-        driveBackLeft.setPower(powerBackRight * BIAS_BACK_RIGHT);
+        driveBackRight.setPower(powerBackRight * BIAS_BACK_RIGHT);
     }
 
+    //returns an array of the powers necessary to execute the provided motion. The order of the motors
+    //is ForwardLeft, ForwardRight, BackLeft, BackRight
     public double[] getMecanumPowers(float degreesDirectionMotion, double scalar, float degreesDirectionFace) {
-        double rad = Math.toRadians(degreesDirectionMotion);
+        double rad = Math.toRadians(getRelativeAngle(degreesDirectionFace,degreesDirectionMotion));
         double y = scalar * Math.cos(rad);
         double x = scalar * Math.sin(rad);
 
@@ -341,12 +352,12 @@ public class   Robot {
         double q = y - x;
 
         float relativeRotation =  getRelativeAngle(degreesDirectionFace, rotation);
-        double turnCorrection = Math.pow(0.03 * relativeRotation, 3) + 0.02 * relativeRotation;
+        double turnCorrection = Math.pow(LARGE_CORRECTION * relativeRotation, 3) + FINE_CORRECTION * relativeRotation;
 
-        double powerForwardRight = q - turnCorrection;
+        double powerForwardRight = q + turnCorrection;
-        double powerForwardLeft = p + turnCorrection;
+        double powerForwardLeft = p - turnCorrection;
-        double powerBackRight = p - turnCorrection;
+        double powerBackRight = p + turnCorrection;
-        double powerBackLeft = q + turnCorrection;
+        double powerBackLeft = q - turnCorrection;
 
 
         // The "extreme" is the power value that is furthest from zero. When this values exceed the
@@ -369,14 +380,19 @@ public class   Robot {
         return powers;
     }
 
+    //
     public double[] getFacePowers(float direction, double power) {
-        double left = power;
+        double relativeAngle = getRelativeAngle(direction, rotation);
-        double right = -power;
+        double scaler = Math.pow(LARGE_CORRECTION * relativeAngle, 3) + FINE_CORRECTION * relativeAngle;
 
-        if (getRelativeAngle(direction, rotation) > 0) {
+        double left = -power * scaler;
-            left *= -1;
+        double right = power *scaler;
-            right *= -1;
+
-        }
+
+//        if (relativeAngle > 0) {
+//            left *= -1;
+//            right *= -1;
+//        }
         double[] powers = {left,right};
         return powers;
     }
@@ -406,8 +422,12 @@ public class   Robot {
     }
 
     //Data Recording
+//    public void record(double frontLeft, double frontRight, double backLeft, double backRight) {
+//        TestingRecord+="\n"+frontLeft+","+frontRight+","+backLeft+","+backRight;
+//    }
+
     public void record() {
-        TestingRecord+="\n"+locationX+","+locationY+","+rotation;
+        TestingRecord+="\n"+rotation;
     }
 
     public void saveRecording() {