Skip to content

Commit de6eed3

Browse files
rhauchrhauch
committed
Merge pull request #43 from rhauch/pid-gain-profiles
PIDController now supports multiple named gain profiles
2 parents 2fb8c4c + 3f6ceda commit de6eed3

File tree

2 files changed

+243
-35
lines changed

2 files changed

+243
-35
lines changed

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

Lines changed: 46 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020

2121
import org.junit.Before;
2222
import org.junit.Test;
23+
import org.strongback.control.PIDController.Gains;
2324
import org.strongback.function.DoubleBiFunction;
2425

2526
import edu.wpi.first.wpilibj.HLUsageReporting;
@@ -46,7 +47,7 @@ public double getActualValue() {
4647

4748
public void setValue(double input) {
4849
double newValue = model.applyAsDouble(actualValue, input);
49-
if ( print ) System.out.println("actual=" + actualValue + "; input=" + input + "; newValue=" + newValue);
50+
if (print) System.out.println("actual=" + actualValue + "; input=" + input + "; newValue=" + newValue);
5051
this.actualValue = newValue;
5152
}
5253
}
@@ -74,14 +75,14 @@ public void beforeEach() {
7475
@Test
7576
public void shouldUseProportionalOnly() {
7677
model = simple();
77-
//model.print = true;
78+
// model.print = true;
7879
controller = new PIDController(model::getActualValue, model::setValue).withGains(0.9, 0.0, 0.0)
7980
.inputRange(-1.0, 1.0)
8081
.outputRange(-1.0, 1.0)
8182
.tolerance(0.02)
8283
.setpoint(0.5);
8384
assertThat(runController(10)).isLessThan(5);
84-
assertThat(model.getActualValue()-0.5 < 0.02).isTrue();
85+
assertThat(model.getActualValue() - 0.5 < 0.02).isTrue();
8586
}
8687

8788
@Test
@@ -94,47 +95,71 @@ public void shouldUseProportionalAndDifferential() {
9495
.tolerance(0.02)
9596
.setpoint(0.5);
9697
assertThat(runController(10)).isLessThan(5);
97-
assertThat(model.getActualValue()-0.5 < 0.02).isTrue();
98+
assertThat(model.getActualValue() - 0.5 < 0.02).isTrue();
9899
}
99100

100-
101101
@Test
102102
public void shouldUseProportionalOnlyWithInitialValue() {
103103
model = simple();
104104
model.setValue(0.2);
105-
//model.print = true;
105+
// model.print = true;
106106
controller = new PIDController(model::getActualValue, model::setValue).withGains(0.9, 0.0, 0.0)
107107
.inputRange(-1.0, 1.0)
108108
.outputRange(-1.0, 1.0)
109109
.tolerance(0.02)
110110
.setpoint(0.5);
111111
assertThat(runController(10)).isLessThan(5);
112-
assertThat(model.getActualValue()-0.5 < 0.02).isTrue();
112+
assertThat(model.getActualValue() - 0.5 < 0.02).isTrue();
113113
}
114114

115115
@Test
116116
public void shouldUseProportionalAndDifferentialWithInitialValue() {
117117
model = simple();
118118
model.setValue(0.2);
119-
//model.print = true;
119+
// model.print = true;
120120
controller = new PIDController(model::getActualValue, model::setValue).withGains(0.7, 0.0, 0.3)
121121
.inputRange(-1.0, 1.0)
122122
.outputRange(-1.0, 1.0)
123123
.tolerance(0.02)
124124
.setpoint(0.5);
125125
assertThat(runController(10)).isLessThan(5);
126-
assertThat(model.getActualValue()-0.5 < 0.02).isTrue();
126+
assertThat(model.getActualValue() - 0.5 < 0.02).isTrue();
127+
}
128+
129+
@Test
130+
public void shouldDefineMultipleProfiles() {
131+
model = simple();
132+
model.setValue(0.2);
133+
// model.print = true;
134+
controller = new PIDController(model::getActualValue, model::setValue).withGains(0.9, 0.0, 0.0)
135+
.withProfile("two", 1.2, 0.0, 2.0)
136+
.withProfile("three", 1.3, 0.0, 3.0)
137+
.inputRange(-1.0, 1.0)
138+
.outputRange(-1.0, 1.0)
139+
.tolerance(0.02)
140+
.setpoint(0.5);
141+
assertThat(controller.getProfileNames()).containsOnly(PIDController.DEFAULT_PROFILE,"two","three");
142+
assertThat(controller.getCurrentProfile()).isEqualTo(PIDController.DEFAULT_PROFILE);
143+
assertGains(controller.getGainsForCurrentProfile(), 0.9, 0.0, 0.0, 0.0);
144+
assertGains(controller.getGainsFor("two"), 1.2, 0.0, 2.0, 0.0);
145+
assertGains(controller.getGainsFor("three"), 1.3, 0.0, 3.0, 0.0);
146+
controller.useProfile("two");
147+
assertGains(controller.getGainsForCurrentProfile(), 1.2, 0.0, 2.0, 0.0);
148+
controller.useProfile("three");
149+
assertGains(controller.getGainsForCurrentProfile(), 1.3, 0.0, 3.0, 0.0);
127150
}
128151

129152
@Test
130153
public void shouldUseProportionalOnlyWPILib() throws InterruptedException {
131-
HLUsageReporting.SetImplementation(new HLUsageReporting.Interface(){
154+
HLUsageReporting.SetImplementation(new HLUsageReporting.Interface() {
132155
@Override
133156
public void reportPIDController(int num) {
134157
}
158+
135159
@Override
136160
public void reportScheduler() {
137161
}
162+
138163
@Override
139164
public void reportSmartDashboard() {
140165
}
@@ -150,16 +175,23 @@ public void reportSmartDashboard() {
150175
wpi.enable();
151176
Thread.sleep(300);
152177
wpi.disable();
153-
assertThat(model.getActualValue()-0.5 < 0.02).isTrue();
178+
assertThat(model.getActualValue() - 0.5 < 0.02).isTrue();
154179
}
155180

156-
protected int runController( int maxNumSteps) {
181+
protected int runController(int maxNumSteps) {
157182
int counter = 0;
158-
while ( !controller.computeOutput() && counter < maxNumSteps) {
183+
while (!controller.computeOutput() && counter < maxNumSteps) {
159184
++counter;
160-
if ( print ) System.out.println(counter + " " + model.getActualValue());
185+
if (print) System.out.println(counter + " " + model.getActualValue());
161186
}
162187
return counter;
163188
}
164189

190+
protected void assertGains( Gains gains, double p, double i, double d, double f ) {
191+
assertThat(gains.getP()).isEqualTo(p);
192+
assertThat(gains.getI()).isEqualTo(i);
193+
assertThat(gains.getD()).isEqualTo(d);
194+
assertThat(gains.getFeedForward()).isEqualTo(f);
195+
}
196+
165197
}

0 commit comments

Comments
 (0)