Implement team 254's 2014 driving code

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / CheesyDriveHelper.java

diff --git a/src/org/usfirst/frc/team3501/robot/CheesyDriveHelper.java b/src/org/usfirst/frc/team3501/robot/CheesyDriveHelper.java
new file mode 100644 (file)
index 0000000..73b7396
--- /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