challenge-bot.com Git - 3501/2017steamworks/blob - src/org/usfirst/frc/team3501/robot/MathLib.java

   1 package org.usfirst.frc.team3501.robot;
   2
   3 public class MathLib {
   4
   5   /***
   6    * This method gives speed as a function of % distance covered so the speed
   7    * forms a parabola starting and ending at minSpeed when you start and end and
   8    * achieving maxSpeed exactly halfway.
   9    *
  10    * @param minSpeed
  11    *          the starting and ending speed, in range [0, 1]
  12    * @param maxSpeed
  13    *          the max speed, achieved at percentComplete = 1/2.
  14    * @param percentComplete
  15    *          should be currentDistance / targetDistance
  16    * @return the speed (motor value) to set motors to for smooth acceleration.
  17    *         Note that since velocity is a parabola, acceleration is linear. It
  18    *         may exceed the maximum value robot can accelerate without wheel
  19    *         slipping.
  20    */
  21   public static double getSpeedForLinearAccel(double minSpeed, double maxSpeed,
  22       double percentComplete) {
  23     return 4 * (minSpeed - maxSpeed) * (percentComplete - 0.5)
  24         * (percentComplete - 0.5) + maxSpeed;
  25   }
  26
  27   /***
  28    * This method gives speed as a function of % distance covered so the speed
  29    * increases linearly from minSpeed to maxSpeed and then back down again.
  30    *
  31    * @param minSpeed
  32    *          the starting and ending speed, in range [0, 1]
  33    * @param maxSpeed
  34    *          the max speed, achieved at percentComplete = 1/2.
  35    * @param percentComplete
  36    *          should be currentDistance / targetDistance
  37    * @return the speed (motor value) to set motors to.
  38    */
  39   public static double getSpeedForConstantAccel(double minSpeed,
  40       double maxSpeed, double percentComplete) {
  41     return maxSpeed
  42         + 2 * (minSpeed - maxSpeed) * Math.abs(percentComplete - 0.5);
  43   }
  44
  45   /***
  46    * Restricts an input value to the range [low, high]. If value > high it will
  47    * be set to high. If value < low it will be set to low.
  48    *
  49    * This method is used for defensive programming for inputs to motors to
  50    * restrict them to valid ranges.
  51    *
  52    * @param value
  53    *          the value to restrict.
  54    * @param low
  55    *          the smallest acceptable value.
  56    * @param high
  57    *          the largest acceptable value.
  58    * @return returns value restricted to be within the range [low, high].
  59    */
  60   public static double restrictToRange(double value, double low, double high) {
  61     value = Math.max(value, low);
  62     value = Math.min(value, high);
  63     return value;
  64   }
  65
  66   /***
  67    * Returns true if val is in the range [low, high]
  68    *
  69    * @param val
  70    *          value to test
  71    * @param low
  72    *          low end of range
  73    * @param high
  74    *          high end of range
  75    * @return boolean return true if val is in the range [low, high]
  76    */
  77   public static boolean inRange(double val, double low, double high) {
  78     return (val <= high) && (val >= low);
  79   }
  80
  81   public static double limitValue(double val) {
  82     return limitValue(val, 1.0);
  83   }
  84
  85   public static double limitValue(double val, double max) {
  86     if (val > max) {
  87       return max;
  88     } else if (val < -max) {
  89       return -max;
  90     } else {
  91       return val;
  92     }
  93   }
  94
  95   public static double limitValue(double val, double min, double max) {
  96     if (val > max) {
  97       return max;
  98     } else if (val < min) {
  99       return min;
 100     } else {
 101       return val;
 102     }
 103   }
 104
 105   public static double calcLeftTankDrive(double x, double y) {
 106     return limitValue(y + x);
 107   }
 108
 109   public static double calcRightTankDrive(double x, double y) {
 110     return limitValue(y - x);
 111   }
 112 }