From 70f315a35a169a1bdabe1d9d59fe6e50331efa9c Mon Sep 17 00:00:00 2001
From: Nathaniel Palme <robotnrp@gmail.com>
Date: Thu, 24 Sep 2020 20:56:48 -0500
Subject: [PATCH] Updated to Ultimate Goal

---
 .../HardwareTesting/EncoderTest.java          | 46 ++++++++++---------
 .../org/timecrafters/UltimateGoal/Robot.java  |  6 +--
 2 files changed, 27 insertions(+), 25 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java
index 9c08417..e3e7739 100644
--- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java
+++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java
@@ -20,36 +20,37 @@ public class EncoderTest extends CyberarmState {
     public void exec() {
         robot.updateLocation();
 
-        if (runTime() < 3000) {
-            robot.setDrivePower(0.5, 0.5);
+        robot.setDrivePower(-0.2 * engine.gamepad1.left_stick_y, -0.2 * engine.gamepad1.right_stick_y);
 
-            ticksLeft=robot.encoderLeft.getCurrentPosition();
-            ticksRight=robot.encoderRight.getCurrentPosition();
+        ticksLeft=robot.encoderLeft.getCurrentPosition();
+        ticksRight=robot.encoderRight.getCurrentPosition();
 
 
-
-        } else {
-            robot.encoderLeft.setPower(0.0);
-            robot.encoderRight.setPower(0.0);
-
-            double ticksExtreme;
-            if (Math.abs(ticksLeft) < Math.abs(ticksRight)) {
-                ticksExtreme = ticksLeft;
-            } else {
-                ticksExtreme = ticksRight;
-            }
-            biasLeft = ticksExtreme/ticksLeft;
-            biasRight = ticksExtreme/ticksRight;
-        }
+//        if (runTime() < 3000) {
+//
+//
+//
+//
+//        } else {
+//            robot.encoderLeft.setPower(0.0);
+//            robot.encoderRight.setPower(0.0);
+//
+//            double ticksExtreme;
+//            if (Math.abs(ticksLeft) < Math.abs(ticksRight)) {
+//                ticksExtreme = ticksLeft;
+//            } else {
+//                ticksExtreme = ticksRight;
+//            }
+//            biasLeft = ticksExtreme/ticksLeft;
+//            biasRight = ticksExtreme/ticksRight;
+//        }
 
     }
 
     @Override
     public void telemetry() {
-        engine.telemetry.addLine("Biases");
-        engine.telemetry.addData("Left", biasLeft);
-        engine.telemetry.addData("Right", biasRight);
-        engine.telemetry.addLine();
+
+        engine.telemetry.addData("controler", engine.gamepad1.left_stick_y);
         engine.telemetry.addLine("Latency Values");
         engine.telemetry.addData("Y", robot.getLocationY());
         engine.telemetry.addData("X", robot.getLocationX());
@@ -59,6 +60,7 @@ public class EncoderTest extends CyberarmState {
         engine.telemetry.addLine("Actual Values");
         engine.telemetry.addData("Left", ticksLeft);
         engine.telemetry.addData("Right", ticksRight);
+
 //        engine.telemetry.addLine("");
 //        engine.telemetry.addData("Front", robot.encoderFront);
 //        engine.telemetry.addData("Back", robot.encoderBack);
diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java
index 7b50866..72f029b 100644
--- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java
+++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java
@@ -26,7 +26,7 @@ public class Robot {
     public DcMotor encoderRight;
 
     double BIAS_LEFT = 1.0;
-    double BIAS_RIGHT = 0.6815;
+    double BIAS_RIGHT = 0.87;
 
     //Robot Localizatoin
     private double locationX;
@@ -81,8 +81,8 @@ public class Robot {
 
         double average = (encoderLeftChange+encoderRightChange)/2;
 
-        double xChange = average * (Math.sin(Math.toRadians(rotationChange)));
-        double yChange = average * (Math.cos(Math.toRadians(rotationChange)));
+        double xChange = average * (Math.sin(Math.toRadians(rotation)));
+        double yChange = average * (Math.cos(Math.toRadians(rotation)));
 
         locationX += xChange;
         locationY += yChange;