Permalink
Browse files

Corrected checkTolerance and fixed initTable in SoftwarePIDController

* Added a unit test for verifying the checkTolerance logic
* Updated both SoftwarePIDController and the base method in Controller
* Fixed a problem where no gains would be sent to the SmartDashboard
  • Loading branch information...
rothso committed Feb 6, 2017
1 parent 83a5de7 commit 0fc0d3a9c880cb75c55b4a45ce64c5eeddc1a7ac
@@ -256,6 +256,34 @@ public void shouldUseProportionalRateOnlyWPILib() throws InterruptedException {
assertThat(model.getActualValue() - 0.5 < 0.02).isTrue();
}
@Test
public void shouldCorrectlyOutputWithinTolerance() {
final double target = 0.5;
final double tolerance = 0.02;
// Not within tolerance should return false
model = simple();
model.setValue(0);
controller = new SoftwarePIDController(model::sourceType, model::getActualValue, model::setValue)
.withGains(0.9, 0.0, 0.0)
.withInputRange(-1.0, 1.0)
.withOutputRange(-1.0, 1.0)
.withTolerance(tolerance)
.withTarget(target);
assertThat(controller.isWithinTolerance()).isFalse();
// Within tolerance should return true
model = simple();
model.setValue(target - tolerance + 0.001);
controller = new SoftwarePIDController(model::sourceType, model::getActualValue, model::setValue)
.withGains(0.9, 0.0, 0.0)
.withInputRange(-1.0, 1.0)
.withOutputRange(-1.0, 1.0)
.withTolerance(tolerance)
.withTarget(target);
assertThat(controller.isWithinTolerance()).isTrue();
}
protected int runController(int maxNumSteps) {
int counter = 0;
while (!controller.computeOutput() && counter < maxNumSteps) {
@@ -98,7 +98,7 @@
* @return <code>true</code> if the proposed value is within the tolerance of the target, or <code>false</code> otherwise
*/
public default boolean checkTolerance(double value) {
return Math.abs(value) <= (getTarget() - getTolerance());
return Math.abs(getTarget() - value) < getTolerance();
}
/**
@@ -656,7 +656,7 @@ public Target continuous(boolean continuous) {
}
public boolean isWithinTolerance(double value) {
return Math.abs(value) <= (setpoint - tolerance);
return calculateError(value) < tolerance;
}
public double calculateError(double input) {
@@ -721,7 +721,7 @@ public synchronized void initTable(ITable subtable) {
if (this.table != null) {
this.table.removeTableListener(listener);
}
this.table = table;
this.table = subtable;
if (table != null) {
Gains gains = this.gains;
Target target = this.target;

0 comments on commit 0fc0d3a

Please sign in to comment.