From 9a978a601f3582168cd743ef6f93eb502c8e12f3 Mon Sep 17 00:00:00 2001
From: David <david.dobervich@gmail.com>
Date: Fri, 20 Nov 2015 11:19:18 -0800
Subject: [PATCH] add mathlib and smooth accelerate distance command

---
 .../usfirst/frc3501/RiceCatRobot/MathLib.java | 45 ++++++++++++++++
 .../commands/SmoothAccelerateForDistance.java | 53 +++++++++++++++++++
 2 files changed, 98 insertions(+)
 create mode 100644 src/org/usfirst/frc3501/RiceCatRobot/MathLib.java
 create mode 100644 src/org/usfirst/frc3501/RiceCatRobot/commands/SmoothAccelerateForDistance.java

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/MathLib.java b/src/org/usfirst/frc3501/RiceCatRobot/MathLib.java
new file mode 100644
index 0000000..e5c1f93
--- /dev/null
+++ b/src/org/usfirst/frc3501/RiceCatRobot/MathLib.java
@@ -0,0 +1,45 @@
+package org.usfirst.frc3501.RiceCatRobot;
+
+public class MathLib {
+
+  /***
+   * This method gives speed as a function of % distance covered so the speed
+   * forms a parabola starting and ending at minSpeed when you start and end and
+   * achieving maxSpeed exactly halfway.
+   *
+   * @param minSpeed
+   *          the starting and ending speed, in range [0, 1]
+   * @param maxSpeed
+   *          the max speed, achieved at percentComplete = 1/2.
+   * @param percentComplete
+   *          should be currentDistance / targetDistance
+   * @return the speed (motor value) to set motors to for smooth acceleration.
+   *         Note that since velocity is a parabola, acceleration is linear. It
+   *         may exceed the maximum value robot can accelerate without wheel
+   *         slipping.
+   */
+  public static double getSpeedForLinearAccel(double minSpeed, double maxSpeed,
+      double percentComplete) {
+    return 4 * (minSpeed - maxSpeed) * (percentComplete - 0.5)
+        * (percentComplete - 0.5) + maxSpeed;
+  }
+
+  /***
+   * This method gives speed as a function of % distance covered so the speed
+   * increases linearly from minSpeed to maxSpeed and then back down again.
+   *
+   * @param minSpeed
+   *          the starting and ending speed, in range [0, 1]
+   * @param maxSpeed
+   *          the max speed, achieved at percentComplete = 1/2.
+   * @param percentComplete
+   *          should be currentDistance / targetDistance
+   * @return the speed (motor value) to set motors to.
+   */
+  public static double getSpeedForConstantAccel(double minSpeed,
+      double maxSpeed, double percentComplete) {
+    return maxSpeed + 2 * (minSpeed - maxSpeed)
+        * Math.abs(percentComplete - 0.5);
+  }
+
+}
diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/SmoothAccelerateForDistance.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/SmoothAccelerateForDistance.java
new file mode 100644
index 0000000..b774346
--- /dev/null
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/SmoothAccelerateForDistance.java
@@ -0,0 +1,53 @@
+package org.usfirst.frc3501.RiceCatRobot.commands;
+
+import org.usfirst.frc3501.RiceCatRobot.Robot;
+import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction;
+import org.usfirst.frc3501.RiceCatRobot.subsystems.DriveTrain;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command will drive the specified distance at specified speed.
+ *
+ */
+public class SmoothAccelerateForDistance extends Command {
+  private double distance; // in units of cm
+  private double speed;
+
+  public SmoothAccelerateForDistance(double distance, double speed) {
+    // Since negative values in setMotorSpeeds(...) are forwards, we reverse
+    // speed here so positive input values will move the robot forwards.
+    this.speed = -speed;
+
+    // ensure distance is positive
+    this.distance = Math.max(distance, -distance);
+  }
+
+  @Override
+  protected void initialize() {
+    Robot.driveTrain.resetEncoders();
+    Robot.driveTrain.setMotorSpeeds(speed, speed);
+  }
+
+  @Override
+  protected void execute() {
+    // nothing to do since motor speeds already set
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return Math.abs(Robot.driveTrain.getLeftDistance()) >= distance
+        && Math.abs(Robot.driveTrain.getRightDistance()) >= distance;
+  }
+
+  @Override
+  protected void end() {
+    Robot.driveTrain.stop();
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}
\ No newline at end of file
-- 
2.30.2