Minor changes

team484 · May 21, 2014 · b365605 · b365605
1 parent 6b4bc77
commit b365605
Showing 1 changed file with 24 additions and 9 deletions.
diff --git a/src/org/team484/fluffy/subsystems/DriveTrain.java b/src/org/team484/fluffy/subsystems/DriveTrain.java
@@ -130,10 +130,10 @@ public void followBall() {
         double speed = 0;
         int currentLeft = leftIR.getValue();
         int currentRight = rightIR.getValue();
-        double leftIn = (MathUtils.pow(currentLeft, -1.22)*6627);
-        double rightIn = (MathUtils.pow(currentRight, -1.22)*6627);
+        double leftIn = (MathUtils.pow(currentLeft, -1.22)*6627) / 0.533333333;
+        double rightIn = (MathUtils.pow(currentRight, -1.22)*6627) / 0.533333333;
         System.out.println("Left: " + leftIn + " Right: " + rightIn);
-        if ((leftIn < 32 || rightIn < 32) && SmartDashboard.getBoolean("No Ball", true)) {
+        if ((leftIn < 64 || rightIn < 64) && SmartDashboard.getBoolean("No Ball", true)) {
             double mech = MathUtils.log(Math.abs(leftIn - rightIn)) / 20;
             if (leftIn - rightIn < 0) {
                 mech = 0 - mech;
@@ -155,17 +155,25 @@ public void followBall() {
                 speed = 0;
             }
             robotDrive.mecanumDrive_Cartesian(mech, speed, rotation, 0);
-        }
+        } /**else {
+            double vector = ds.getAnalogIn(1);
+            if (vector < 1.5) {
+                robotDrive.mecanumDrive_Cartesian(0.5, 0, rotation, 0);
+            } else if (vector > 3.5) {
+                robotDrive.mecanumDrive_Cartesian(-0.5, 0, rotation, 0);
+            }
+        }**/
     }
     public void followBallTele() {
         double rotation = -(gyro.getAngle() / 160);
         double speed = 0;
         int currentLeft = leftIR.getValue();
         int currentRight = rightIR.getValue();
-        double leftIn = (MathUtils.pow(currentLeft, -1.22)*6627);
-        double rightIn = (MathUtils.pow(currentRight, -1.22)*6627);
-        System.out.println("Left: " + leftIn + " Right: " + rightIn);
-        if ((leftIn < 38 || rightIn < 38) && SmartDashboard.getBoolean("No Ball", true)) {
+        double leftIn = (MathUtils.pow(currentLeft, -1.22)*6627) / 0.533333333;
+        double rightIn = (MathUtils.pow(currentRight, -1.22)*6627) / 0.533333333;
+        //System.out.println("Left: " + leftIn + " Right: " + rightIn);
+        System.out.println("Left: " + leftIn + " Right: " +rightIn);
+        if ((leftIn < 64 || rightIn < 64) && SmartDashboard.getBoolean("No Ball", true)) {
             double mech = MathUtils.log(Math.abs(leftIn - rightIn)) / 20;
             if (leftIn - rightIn < 0) {
                 mech = 0 - mech;
@@ -188,10 +196,16 @@ public void followBallTele() {
             }
             robotDrive.mecanumDrive_Cartesian(mech, speed, rotation, 0);
         } else {
-            robotDrive.mecanumDrive_Cartesian(0, driveStick.getY(), driveStick.getX(), 0);
+            robotDrive.mecanumDrive_Cartesian(0, driveStick.getY(), MathUtils.pow(driveStick.getX(), 3), 0);
         }
+        SmartDashboard.putNumber("Match Time", 230 - ds.getMatchTime());
     }
     public void mechanumDrive(double x, double y, double rotation, boolean autonomous, boolean mechanum) {
+        int currentLeft = leftIR.getValue();
+        int currentRight = rightIR.getValue();
+        double leftIn = (MathUtils.pow(currentLeft, -1.22)*6627) / 0.53333333;
+        double rightIn = (MathUtils.pow(currentRight, -1.22)*6627) / 0.533333333;
+        //System.out.println("Left: " + leftIn + " Right: " + rightIn);
         if (mechanum && !this.wasMech) {
             this.wasMech = true;
             gyro.reset();
@@ -221,6 +235,7 @@ public void mechanumDrive(double x, double y, double rotation, boolean autonomou
             SmartDashboard.putBoolean("Shoot", false);
             SmartDashboard.putString("Can Shoot", "Don't Shoot");
         }
+        SmartDashboard.putNumber("Match Time", 230 - ds.getMatchTime());
         SmartDashboard.putNumber("Gyro", gyro.getAngle());
         SmartDashboard.putNumber("UltraSonic", sonic.getRangeInches());
         //SmartDashboard.putNumber("voltage", ds.getBatteryVoltage());