From: Harel Dor <hareldor@gmail.com>
Date: Wed, 23 Mar 2016 00:58:35 +0000 (-0700)
Subject: Implement team 254's 2014 driving code
X-Git-Url: http://challenge-bot.com/repos/?p=3501%2Fstronghold-2016;a=commitdiff_plain;h=d9475515518368b284e5c5fb4e2463c8c082f62d

Implement team 254's 2014 driving code
---

diff --git a/src/org/usfirst/frc/team3501/robot/CheesyDriveHelper.java b/src/org/usfirst/frc/team3501/robot/CheesyDriveHelper.java
new file mode 100644
index 00000000..73b73966
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/CheesyDriveHelper.java
@@ -0,0 +1,127 @@
+package org.usfirst.frc.team3501.robot;
+
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+
+import edu.wpi.first.wpilibj.DriverStation;
+
+/**
+ * Copied/modified from Team 254's 2014 codebase. Thanks so much guys!
+ * 
+ * CheesyDriveHelper implements the calculations used in CheesyDrive, sending
+ * power to the motors.
+ */
+public class CheesyDriveHelper {
+
+  private DriveTrain drive;
+  double oldWheel, quickStopAccumulator;
+  private double throttleDeadband = 0.02;
+  private double wheelDeadband = 0.02;
+
+  public CheesyDriveHelper(DriveTrain drive) {
+    this.drive = drive;
+  }
+
+  public void cheesyDrive(double throttle, double wheel, boolean isHighGear) {
+    if (DriverStation.getInstance().isAutonomous()) {
+      return;
+    }
+
+    double wheelNonLinearity;
+
+    wheel = handleDeadband(wheel, wheelDeadband);
+    throttle = handleDeadband(throttle, throttleDeadband);
+
+    double negInertia = wheel - oldWheel;
+    oldWheel = wheel;
+
+    if (isHighGear) {
+      wheelNonLinearity = 0.6;
+      // Apply a sin function that's scaled to make it feel better.
+      wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel)
+          / Math.sin(Math.PI / 2.0 * wheelNonLinearity);
+      wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel)
+          / Math.sin(Math.PI / 2.0 * wheelNonLinearity);
+    } else {
+      wheelNonLinearity = 0.5;
+      // Apply a sin function that's scaled to make it feel better.
+      wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel)
+          / Math.sin(Math.PI / 2.0 * wheelNonLinearity);
+      wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel)
+          / Math.sin(Math.PI / 2.0 * wheelNonLinearity);
+      wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel)
+          / Math.sin(Math.PI / 2.0 * wheelNonLinearity);
+    }
+
+    double leftPwm, rightPwm, overPower;
+    double sensitivity;
+
+    double angularPower;
+    double linearPower;
+
+    // Negative inertia!
+    double negInertiaAccumulator = 0.0;
+    double negInertiaScalar;
+    if (isHighGear) {
+      negInertiaScalar = 5.0;
+      sensitivity = .75;
+    } else {
+      if (wheel * negInertia > 0) {
+        negInertiaScalar = 2.5;
+      } else {
+        if (Math.abs(wheel) > 0.65) {
+          negInertiaScalar = 5.0;
+        } else {
+          negInertiaScalar = 3.0;
+        }
+      }
+      sensitivity = .75;
+    }
+    double negInertiaPower = negInertia * negInertiaScalar;
+    negInertiaAccumulator += negInertiaPower;
+
+    wheel = wheel + negInertiaAccumulator;
+    if (negInertiaAccumulator > 1) {
+      negInertiaAccumulator -= 1;
+    } else if (negInertiaAccumulator < -1) {
+      negInertiaAccumulator += 1;
+    } else {
+      negInertiaAccumulator = 0;
+    }
+    linearPower = throttle;
+
+    overPower = 0.0;
+    angularPower = Math.abs(throttle) * wheel * sensitivity
+        - quickStopAccumulator;
+    if (quickStopAccumulator > 1) {
+      quickStopAccumulator -= 1;
+    } else if (quickStopAccumulator < -1) {
+      quickStopAccumulator += 1;
+    } else {
+      quickStopAccumulator = 0.0;
+    }
+
+    rightPwm = leftPwm = linearPower;
+    leftPwm += angularPower;
+    rightPwm -= angularPower;
+
+    if (leftPwm > 1.0) {
+      rightPwm -= overPower * (leftPwm - 1.0);
+      leftPwm = 1.0;
+    } else if (rightPwm > 1.0) {
+      leftPwm -= overPower * (rightPwm - 1.0);
+      rightPwm = 1.0;
+    } else if (leftPwm < -1.0) {
+      rightPwm += overPower * (-1.0 - leftPwm);
+      leftPwm = -1.0;
+    } else if (rightPwm < -1.0) {
+      leftPwm += overPower * (-1.0 - rightPwm);
+      rightPwm = -1.0;
+    }
+    drive.setMotorSpeeds(leftPwm, rightPwm);
+
+  }
+
+  public double handleDeadband(double val, double deadband) {
+    return (Math.abs(val) > Math.abs(deadband)) ? val : 0.0;
+  }
+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index a0134a43..1079d501 100644
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -1,5 +1,6 @@
 package org.usfirst.frc.team3501.robot.subsystems;
 
+import org.usfirst.frc.team3501.robot.CheesyDriveHelper;
 import org.usfirst.frc.team3501.robot.Constants;
 import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
 
@@ -24,6 +25,8 @@ public class DriveTrain extends PIDSubsystem {
 
   private DoubleSolenoid leftGearPiston, rightGearPiston;
 
+  private CheesyDriveHelper cheese;
+
   // Drivetrain specific constants that relate to the inches per pulse value for
   // the encoders
 
@@ -57,6 +60,8 @@ public class DriveTrain extends PIDSubsystem {
         Constants.DriveTrain.RIGHT_SHIFT_MODULE,
         Constants.DriveTrain.RIGHT_SHIFT_FORWARD,
         Constants.DriveTrain.RIGHT_SHIFT_REVERSE);
+
+    cheese = new CheesyDriveHelper(this);
   }
 
   @Override
@@ -196,13 +201,14 @@ public class DriveTrain extends PIDSubsystem {
   public void joystickDrive(double left, double right) {
     // Handle flipping of the "front" of the robot
     double k = (isFlipped() ? -1 : 1);
-
-    robotDrive.tankDrive(-left * k, -right * k);
+    cheese.cheesyDrive(-left * k, -right,
+        getGearPistonValue() == Constants.DriveTrain.HIGH_GEAR);
   }
 
   public void setMotorSpeeds(double left, double right) {
     double k = (isFlipped() ? -1 : 1);
-    robotDrive.tankDrive(-left * k, -right * k);
+    cheese.cheesyDrive(-left * k, -right,
+        getGearPistonValue() == Constants.DriveTrain.HIGH_GEAR);
   }
 
   /**