Skip to content

Commit 85ce9ac

Browse files
rhauchrhauch
committed
Merge pull request #53 from rhauch/wpilib-2016
Upgraded to the 2016 WPILib version
2 parents 2ecc402 + 74e954e commit 85ce9ac

File tree

6 files changed

+424
-95
lines changed

6 files changed

+424
-95
lines changed

‎strongback-tests/src/org/strongback/control/SoftwarePIDControllerTest.java‎

Lines changed: 142 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -21,9 +21,11 @@
2121
import org.junit.Before;
2222
import org.junit.Test;
2323
import org.strongback.control.PIDController.Gains;
24+
import org.strongback.control.SoftwarePIDController.SourceType;
2425
import org.strongback.function.DoubleBiFunction;
2526

26-
import edu.wpi.first.wpilibj.HLUsageReporting;
27+
import edu.wpi.first.wpilibj.PIDSource;
28+
import edu.wpi.first.wpilibj.PIDSourceType;
2729

2830
/**
2931
* @author Randall Hauch
@@ -33,14 +35,20 @@ public class SoftwarePIDControllerTest {
3335

3436
private static class SystemModel {
3537
protected final DoubleBiFunction model;
38+
protected final SourceType sourceType;
3639
protected boolean print = false;
3740

38-
public SystemModel(DoubleBiFunction model) {
41+
public SystemModel(SourceType sourceType, DoubleBiFunction model) {
3942
this.model = model;
43+
this.sourceType = sourceType;
4044
}
4145

4246
protected double actualValue = 0;
4347

48+
public SourceType sourceType() {
49+
return sourceType;
50+
}
51+
4452
public double getActualValue() {
4553
return actualValue;
4654
}
@@ -53,11 +61,19 @@ public void setValue(double input) {
5361
}
5462

5563
public static SystemModel simple() {
56-
return new SystemModel((actual, newValue) -> actual + newValue);
64+
return simple(SourceType.DISTANCE);
65+
}
66+
67+
public static SystemModel simple(SourceType type) {
68+
return new SystemModel(type, (actual, newValue) -> actual + newValue);
5769
}
5870

5971
public static SystemModel linear(double factor) {
60-
return new SystemModel((actual, newValue) -> {
72+
return linear(SourceType.DISTANCE, factor);
73+
}
74+
75+
public static SystemModel linear(SourceType type, double factor) {
76+
return new SystemModel(type, (actual, newValue) -> {
6177
return factor * (newValue - actual) + actual;
6278
});
6379
}
@@ -76,11 +92,16 @@ public void beforeEach() {
7692
public void shouldUseProportionalOnly() {
7793
model = simple();
7894
// model.print = true;
79-
controller = new SoftwarePIDController(model::getActualValue, model::setValue).withGains(0.9, 0.0, 0.0)
80-
.withInputRange(-1.0, 1.0)
81-
.withOutputRange(-1.0, 1.0)
82-
.withTolerance(0.02)
83-
.withTarget(0.5);
95+
controller = new SoftwarePIDController(model::sourceType, model::getActualValue, model::setValue)
96+
.withGains(0.9,
97+
0.0,
98+
0.0)
99+
.withInputRange(-1.0,
100+
1.0)
101+
.withOutputRange(-1.0,
102+
1.0)
103+
.withTolerance(0.02)
104+
.withTarget(0.5);
84105
assertThat(runController(10)).isLessThan(5);
85106
assertThat(model.getActualValue() - 0.5 < 0.02).isTrue();
86107
}
@@ -89,11 +110,16 @@ public void shouldUseProportionalOnly() {
89110
public void shouldUseProportionalAndDifferential() {
90111
model = simple();
91112
// model.print = true;
92-
controller = new SoftwarePIDController(model::getActualValue, model::setValue).withGains(0.7, 0.0, 0.3)
93-
.withInputRange(-1.0, 1.0)
94-
.withOutputRange(-1.0, 1.0)
95-
.withTolerance(0.02)
96-
.withTarget(0.5);
113+
controller = new SoftwarePIDController(model::sourceType, model::getActualValue, model::setValue)
114+
.withGains(0.7,
115+
0.0,
116+
0.3)
117+
.withInputRange(-1.0,
118+
1.0)
119+
.withOutputRange(-1.0,
120+
1.0)
121+
.withTolerance(0.02)
122+
.withTarget(0.5);
97123
assertThat(runController(10)).isLessThan(5);
98124
assertThat(model.getActualValue() - 0.5 < 0.02).isTrue();
99125
}
@@ -103,11 +129,16 @@ public void shouldUseProportionalOnlyWithInitialValue() {
103129
model = simple();
104130
model.setValue(0.2);
105131
// model.print = true;
106-
controller = new SoftwarePIDController(model::getActualValue, model::setValue).withGains(0.9, 0.0, 0.0)
107-
.withInputRange(-1.0, 1.0)
108-
.withOutputRange(-1.0, 1.0)
109-
.withTolerance(0.02)
110-
.withTarget(0.5);
132+
controller = new SoftwarePIDController(model::sourceType, model::getActualValue, model::setValue)
133+
.withGains(0.9,
134+
0.0,
135+
0.0)
136+
.withInputRange(-1.0,
137+
1.0)
138+
.withOutputRange(-1.0,
139+
1.0)
140+
.withTolerance(0.02)
141+
.withTarget(0.5);
111142
assertThat(runController(10)).isLessThan(5);
112143
assertThat(model.getActualValue() - 0.5 < 0.02).isTrue();
113144
}
@@ -117,11 +148,16 @@ public void shouldUseProportionalAndDifferentialWithInitialValue() {
117148
model = simple();
118149
model.setValue(0.2);
119150
// model.print = true;
120-
controller = new SoftwarePIDController(model::getActualValue, model::setValue).withGains(0.7, 0.0, 0.3)
121-
.withInputRange(-1.0, 1.0)
122-
.withOutputRange(-1.0, 1.0)
123-
.withTolerance(0.02)
124-
.withTarget(0.5);
151+
controller = new SoftwarePIDController(model::sourceType, model::getActualValue, model::setValue)
152+
.withGains(0.7,
153+
0.0,
154+
0.3)
155+
.withInputRange(-1.0,
156+
1.0)
157+
.withOutputRange(-1.0,
158+
1.0)
159+
.withTolerance(0.02)
160+
.withTarget(0.5);
125161
assertThat(runController(10)).isLessThan(5);
126162
assertThat(model.getActualValue() - 0.5 < 0.02).isTrue();
127163
}
@@ -131,13 +167,24 @@ public void shouldDefineMultipleProfiles() {
131167
model = simple();
132168
model.setValue(0.2);
133169
// model.print = true;
134-
controller = new SoftwarePIDController(model::getActualValue, model::setValue).withGains(0.9, 0.0, 0.0)
135-
.withProfile(1, 1.2, 0.0, 2.0)
136-
.withProfile(3, 1.3, 0.0, 3.0)
137-
.withInputRange(-1.0, 1.0)
138-
.withOutputRange(-1.0, 1.0)
139-
.withTolerance(0.02)
140-
.withTarget(0.5);
170+
controller = new SoftwarePIDController(model::sourceType, model::getActualValue, model::setValue)
171+
.withGains(0.9,
172+
0.0,
173+
0.0)
174+
.withProfile(1,
175+
1.2,
176+
0.0,
177+
2.0)
178+
.withProfile(3,
179+
1.3,
180+
0.0,
181+
3.0)
182+
.withInputRange(-1.0,
183+
1.0)
184+
.withOutputRange(-1.0,
185+
1.0)
186+
.withTolerance(0.02)
187+
.withTarget(0.5);
141188
assertThat(controller.getProfiles()).containsOnly(SoftwarePIDController.DEFAULT_PROFILE, 1, 3);
142189
assertThat(controller.getCurrentProfile()).isEqualTo(SoftwarePIDController.DEFAULT_PROFILE);
143190
assertGains(controller.getGainsForCurrentProfile(), 0.9, 0.0, 0.0, 0.0);
@@ -154,35 +201,51 @@ public void shouldFailToUseNonExistantProfile() {
154201
model = simple();
155202
model.setValue(0.2);
156203
// model.print = true;
157-
controller = new SoftwarePIDController(model::getActualValue, model::setValue).withGains(0.9, 0.0, 0.0)
158-
.withProfile(1, 1.2, 0.0, 2.0)
159-
.withProfile(3, 1.3, 0.0, 3.0)
160-
.withInputRange(-1.0, 1.0)
161-
.withOutputRange(-1.0, 1.0)
162-
.withTolerance(0.02)
163-
.withTarget(0.5);
204+
controller = new SoftwarePIDController(model::sourceType, model::getActualValue, model::setValue)
205+
.withGains(0.9,
206+
0.0,
207+
0.0)
208+
.withProfile(1,
209+
1.2,
210+
0.0,
211+
2.0)
212+
.withProfile(3,
213+
1.3,
214+
0.0,
215+
3.0)
216+
.withInputRange(-1.0,
217+
1.0)
218+
.withOutputRange(-1.0,
219+
1.0)
220+
.withTolerance(0.02)
221+
.withTarget(0.5);
164222
controller.useProfile(44);
165223
}
166224

167225
@Test
168-
public void shouldUseProportionalOnlyWPILib() throws InterruptedException {
169-
HLUsageReporting.SetImplementation(new HLUsageReporting.Interface() {
170-
@Override
171-
public void reportPIDController(int num) {
172-
}
173-
174-
@Override
175-
public void reportScheduler() {
176-
}
177-
178-
@Override
179-
public void reportSmartDashboard() {
180-
}
181-
});
182-
model = simple();
226+
public void shouldUseProportionalDistanceOnlyWPILib() throws InterruptedException {
227+
TestableRobotState.resetMatchTime();
228+
model = simple(SourceType.DISTANCE);
229+
// model.print = true;
183230
model.setValue(0.30);
231+
wpi = new edu.wpi.first.wpilibj.PIDController(0.9, 0.0, 0.0, sourceFor(model), model::setValue);
232+
wpi.setSetpoint(0.5);
233+
wpi.setAbsoluteTolerance(0.02);
234+
wpi.setInputRange(-1.0, 1.0);
235+
wpi.setOutputRange(-1.0, 1.0);
236+
wpi.enable();
237+
Thread.sleep(300);
238+
wpi.disable();
239+
assertThat(model.getActualValue() - 0.5 < 0.02).isTrue();
240+
}
241+
242+
@Test
243+
public void shouldUseProportionalRateOnlyWPILib() throws InterruptedException {
244+
TestableRobotState.resetMatchTime();
245+
model = simple(SourceType.RATE);
184246
// model.print = true;
185-
wpi = new edu.wpi.first.wpilibj.PIDController(0.9, 0.0, 0.0, model::getActualValue, model::setValue);
247+
model.setValue(0.30);
248+
wpi = new edu.wpi.first.wpilibj.PIDController(0.9, 0.0, 0.0, sourceFor(model), model::setValue);
186249
wpi.setSetpoint(0.5);
187250
wpi.setAbsoluteTolerance(0.02);
188251
wpi.setInputRange(-1.0, 1.0);
@@ -209,4 +272,29 @@ protected void assertGains(Gains gains, double p, double i, double d, double f)
209272
assertThat(gains.getFeedForward()).isEqualTo(f);
210273
}
211274

275+
protected static PIDSource sourceFor(SystemModel model) {
276+
return sourceFor(model, PIDSourceType.kRate);
277+
}
278+
279+
protected static PIDSource sourceFor(SystemModel model, PIDSourceType initialSourceType) {
280+
return new PIDSource() {
281+
private PIDSourceType sourceType = initialSourceType;
282+
283+
@Override
284+
public PIDSourceType getPIDSourceType() {
285+
return sourceType;
286+
}
287+
288+
@Override
289+
public void setPIDSourceType(PIDSourceType pidSource) {
290+
this.sourceType = pidSource;
291+
}
292+
293+
@Override
294+
public double pidGet() {
295+
return model.getActualValue();
296+
}
297+
};
298+
}
299+
212300
}

0 commit comments

Comments
 (0)