Skip to content

Instantly share code, notes, and snippets.

@dzil123
Created March 10, 2019 07:16
Show Gist options
  • Save dzil123/fe2b07ad7affa4ccccca63f4ec77d5f9 to your computer and use it in GitHub Desktop.
Save dzil123/fe2b07ad7affa4ccccca63f4ec77d5f9 to your computer and use it in GitHub Desktop.
package frc.robot.util;
public class Deadband {
private double minValue;
private double m;
private double deadband;
public Deadband(double minValue, double deadband) {
this.minValue = minValue;
m = calcM(minValue, deadband);
this.deadband = deadband;
}
public double calc(double input) {
return calc(input, false);
}
public double calc(double input, boolean square) {
double output;
if (isDeadband(input)) {
output = 0;
} else if (input > 0) {
output = m * (input - deadband) + minValue;
} else {
output = m * (input + deadband) - minValue;
}
if (square) {
output = Math.copySign(output * output, output);
}
return output;
}
private static double calcM(double minValue, double deadband) {
return (1.0 - minValue) / (1 - deadband);
}
private boolean isDeadband(double input) {
return Math.abs(input) <= deadband;
}
public double getMinValue() {
return minValue;
}
public double getDeadband() {
return deadband;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment