add mathlib and smooth accelerate distance command

[3501/2015-FRC-Spark] / src / org / usfirst / frc3501 / RiceCatRobot / MathLib.java

package org.usfirst.frc3501.RiceCatRobot;\r
\r
public class MathLib {\r
\r
  /***\r
   * This method gives speed as a function of % distance covered so the speed\r
   * forms a parabola starting and ending at minSpeed when you start and end and\r
   * achieving maxSpeed exactly halfway.\r
   *\r
   * @param minSpeed\r
   *          the starting and ending speed, in range [0, 1]\r
   * @param maxSpeed\r
   *          the max speed, achieved at percentComplete = 1/2.\r
   * @param percentComplete\r
   *          should be currentDistance / targetDistance\r
   * @return the speed (motor value) to set motors to for smooth acceleration.\r
   *         Note that since velocity is a parabola, acceleration is linear. It\r
   *         may exceed the maximum value robot can accelerate without wheel\r
   *         slipping.\r
   */\r
  public static double getSpeedForLinearAccel(double minSpeed, double maxSpeed,\r
      double percentComplete) {\r
    return 4 * (minSpeed - maxSpeed) * (percentComplete - 0.5)\r
        * (percentComplete - 0.5) + maxSpeed;\r
  }\r
\r
  /***\r
   * This method gives speed as a function of % distance covered so the speed\r
   * increases linearly from minSpeed to maxSpeed and then back down again.\r
   *\r
   * @param minSpeed\r
   *          the starting and ending speed, in range [0, 1]\r
   * @param maxSpeed\r
   *          the max speed, achieved at percentComplete = 1/2.\r
   * @param percentComplete\r
   *          should be currentDistance / targetDistance\r
   * @return the speed (motor value) to set motors to.\r
   */\r
  public static double getSpeedForConstantAccel(double minSpeed,\r
      double maxSpeed, double percentComplete) {\r
    return maxSpeed + 2 * (minSpeed - maxSpeed)\r
        * Math.abs(percentComplete - 0.5);\r
  }\r
\r
}\r