Skip to content
This repository was archived by the owner on Jan 13, 2025. It is now read-only.

Commit 27717a5

Browse files
dejabotdejabot
committed
add support for multiple PID slots in MotorController
if PID values are set via config, support live refreshes of these values when set() is called.
1 parent 7b78a18 commit 27717a5

9 files changed

+490
-95
lines changed

‎src/main/java/com/team766/hal/LocalMotorController.java

Lines changed: 42 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,10 @@
1010
import com.team766.logging.Severity;
1111

1212
public class LocalMotorController implements MotorController {
13+
1314
private BasicMotorController motor;
1415
private ControlInputReader sensor;
16+
// TODO: add support for multiple slots
1517
private PIDController pidController;
1618

1719
private boolean inverted = false;
@@ -181,7 +183,17 @@ public double getSensorVelocity() {
181183
}
182184

183185
@Override
184-
public void set(final ControlMode mode, final double value) {
186+
public void set(final ControlMode mode, final double value, int slot, double feedForward) {
187+
if (slot != 0) {
188+
throw new UnsupportedOperationException(
189+
"Selecting PID slots not supported on LocalMotorController");
190+
}
191+
192+
if (feedForward != 0.0) {
193+
throw new UnsupportedOperationException(
194+
"Arbitrary feedForward not supported on LocalMotorController");
195+
}
196+
185197
if (this.controlMode != mode || this.leader != null) {
186198
pidController.reset();
187199
this.leader = null;
@@ -218,22 +230,38 @@ public void setNeutralMode(final NeutralMode neutralMode) {
218230
}
219231

220232
@Override
221-
public void setP(final double value) {
233+
public void setP(final double value, int slot) {
234+
if (slot != 0) {
235+
throw new UnsupportedOperationException(
236+
"Selecting PID slot not supported on LocalMotorController");
237+
}
222238
pidController.setP(value);
223239
}
224240

225241
@Override
226-
public void setI(final double value) {
242+
public void setI(final double value, int slot) {
243+
if (slot != 0) {
244+
throw new UnsupportedOperationException(
245+
"Selecting PID slot not supported on LocalMotorController");
246+
}
227247
pidController.setI(value);
228248
}
229249

230250
@Override
231-
public void setD(final double value) {
251+
public void setD(final double value, int slot) {
252+
if (slot != 0) {
253+
throw new UnsupportedOperationException(
254+
"Selecting PID slot not supported on LocalMotorController");
255+
}
232256
pidController.setD(value);
233257
}
234258

235259
@Override
236-
public void setFF(final double value) {
260+
public void setFF(final double value, int slot) {
261+
if (slot != 0) {
262+
throw new UnsupportedOperationException(
263+
"Selecting PID slot not supported on LocalMotorController");
264+
}
237265
pidController.setFF(value);
238266
}
239267

@@ -250,7 +278,11 @@ public void setSensorInverted(final boolean inverted_) {
250278
}
251279

252280
@Override
253-
public void setOutputRange(final double minOutput, final double maxOutput) {
281+
public void setOutputRange(final double minOutput, final double maxOutput, int slot) {
282+
if (slot != 0) {
283+
throw new UnsupportedOperationException(
284+
"Selecting PID slot not supported on LocalMotorController");
285+
}
254286
pidController.setMaxoutputLow(minOutput);
255287
pidController.setMaxoutputHigh(maxOutput);
256288
}
@@ -266,10 +298,10 @@ public void setCurrentLimit(final double ampsLimit) {
266298
public void restoreFactoryDefault() {
267299
this.motor.restoreFactoryDefault();
268300

269-
this.setP(0.0);
270-
this.setI(0.0);
271-
this.setD(0.0);
272-
this.setFF(0.0);
301+
this.setP(0.0, 0);
302+
this.setI(0.0, 0);
303+
this.setD(0.0, 0);
304+
this.setFF(0.0, 0);
273305
this.pidController.setMaxoutputLow(null);
274306
this.pidController.setMaxoutputHigh(null);
275307

‎src/main/java/com/team766/hal/MotorController.java

Lines changed: 50 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
44
import com.ctre.phoenix.motorcontrol.NeutralMode;
5+
import com.team766.library.ValueProvider;
56

67
/**
78
* Interface for motor controlling devices.
@@ -25,7 +26,7 @@ enum ControlMode {
2526
}
2627

2728
/**
28-
* Common interface for setting the power outputu by a motor controller.
29+
* Common interface for setting the power output by a motor controller.
2930
*
3031
* @param power The power to set. Value should be between -1.0 and 1.0.
3132
*/
@@ -44,7 +45,24 @@ enum ControlMode {
4445
*
4546
* @param value The setpoint value, as described above.
4647
*/
47-
void set(ControlMode mode, double value);
48+
default void set(ControlMode mode, double value) {
49+
set(mode, value, 0, 0.0);
50+
}
51+
52+
/**
53+
* Sets the appropriate output on the motor controller, depending on the mode.
54+
* @param mode The output mode to apply.
55+
* In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
56+
* In Current mode, output value is in amperes.
57+
* In Velocity mode, output value is in position change / 100ms.
58+
* In Position mode, output value is in encoder ticks or an analog value,
59+
* depending on the sensor.
60+
* In Follower mode, the output value is the integer device ID of the talon to
61+
* duplicate.
62+
*
63+
* @param value The setpoint value, as described above.
64+
*/
65+
void set(ControlMode mode, double value, int pidSlot, double feedForward);
4866

4967
/**
5068
* Common interface for inverting direction of a motor controller.
@@ -92,13 +110,33 @@ enum ControlMode {
92110

93111
void setNeutralMode(NeutralMode neutralMode);
94112

95-
void setP(double value);
113+
default int numPIDSlots() {
114+
return 1;
115+
}
96116

97-
void setI(double value);
117+
default void setP(ValueProvider<Double> value, int slot) {
118+
setP(value.get(), slot);
119+
}
98120

99-
void setD(double value);
121+
void setP(double value, int slot);
100122

101-
void setFF(double value);
123+
default void setI(ValueProvider<Double> value, int slot) {
124+
setI(value.get(), slot);
125+
}
126+
127+
void setI(double value, int slot);
128+
129+
default void setD(ValueProvider<Double> value, int slot) {
130+
setD(value.get(), slot);
131+
}
132+
133+
void setD(double value, int slot);
134+
135+
default void setFF(ValueProvider<Double> value, int slot) {
136+
setFF(value.get(), slot);
137+
}
138+
139+
void setFF(double value, int slot);
102140

103141
void setSelectedFeedbackSensor(FeedbackDevice feedbackDevice);
104142

@@ -112,7 +150,12 @@ enum ControlMode {
112150
*/
113151
void setSensorInverted(boolean inverted);
114152

115-
void setOutputRange(double minOutput, double maxOutput);
153+
default void setOutputRange(
154+
ValueProvider<Double> minOutput, ValueProvider<Double> maxOutput, int slot) {
155+
setOutputRange(minOutput.get(), maxOutput.get(), slot);
156+
}
157+
158+
void setOutputRange(double minOutput, double maxOutput, int slot);
116159

117160
void setCurrentLimit(double ampsLimit);
118161

‎src/main/java/com/team766/hal/MotorControllerWithSensorScale.java

Lines changed: 19 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
44
import com.ctre.phoenix.motorcontrol.NeutralMode;
55

6+
// TODO: add support for live refreshes of PID values, via PIDSlotHelper?
67
public class MotorControllerWithSensorScale implements MotorController {
78
private MotorController delegate;
89
private double scale;
@@ -12,6 +13,11 @@ public MotorControllerWithSensorScale(final MotorController delegate_, final dou
1213
this.scale = scale_;
1314
}
1415

16+
@Override
17+
public int numPIDSlots() {
18+
return delegate.numPIDSlots();
19+
}
20+
1521
@Override
1622
public double getSensorPosition() {
1723
return delegate.getSensorPosition() * scale;
@@ -23,16 +29,16 @@ public double getSensorVelocity() {
2329
}
2430

2531
@Override
26-
public void set(final ControlMode mode, final double value) {
32+
public void set(final ControlMode mode, final double value, int slot, double feedForward) {
2733
switch (mode) {
2834
case PercentOutput:
2935
delegate.set(mode, value);
3036
return;
3137
case Position:
32-
delegate.set(mode, value / scale);
38+
delegate.set(mode, value / scale, slot, feedForward);
3339
return;
3440
case Velocity:
35-
delegate.set(mode, value / scale);
41+
delegate.set(mode, value / scale, slot, feedForward);
3642
return;
3743
case Voltage:
3844
delegate.set(mode, value);
@@ -78,23 +84,23 @@ public void setNeutralMode(final NeutralMode neutralMode) {
7884
}
7985

8086
@Override
81-
public void setP(final double value) {
82-
delegate.setP(value * scale);
87+
public void setP(final double value, int slot) {
88+
delegate.setP(value * scale, slot);
8389
}
8490

8591
@Override
86-
public void setI(final double value) {
87-
delegate.setI(value * scale);
92+
public void setI(final double value, int slot) {
93+
delegate.setI(value * scale, slot);
8894
}
8995

9096
@Override
91-
public void setD(final double value) {
92-
delegate.setD(value * scale);
97+
public void setD(final double value, int slot) {
98+
delegate.setD(value * scale, slot);
9399
}
94100

95101
@Override
96-
public void setFF(final double value) {
97-
delegate.setFF(value * scale);
102+
public void setFF(final double value, int slot) {
103+
delegate.setFF(value * scale, slot);
98104
}
99105

100106
@Override
@@ -108,8 +114,8 @@ public void setSensorInverted(final boolean inverted) {
108114
}
109115

110116
@Override
111-
public void setOutputRange(final double minOutput, final double maxOutput) {
112-
delegate.setOutputRange(minOutput, maxOutput);
117+
public void setOutputRange(final double minOutput, final double maxOutput, int slot) {
118+
delegate.setOutputRange(minOutput, maxOutput, slot);
113119
}
114120

115121
@Override

‎src/main/java/com/team766/hal/PIDSlotHelper.java

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
package com.team766.hal;
2+
3+
import com.team766.library.ValueProvider;
4+
import edu.wpi.first.math.Pair;
5+
import java.util.ArrayList;
6+
7+
public class PIDSlotHelper {
8+
private final ArrayList<ValueProvider<Double>> pGains;
9+
private final ArrayList<ValueProvider<Double>> iGains;
10+
private final ArrayList<ValueProvider<Double>> dGains;
11+
private final ArrayList<ValueProvider<Double>> ffGains;
12+
private final ArrayList<Pair<ValueProvider<Double>, ValueProvider<Double>>> outputMaxes;
13+
14+
public PIDSlotHelper(int numSlots) {
15+
int num = numSlots;
16+
pGains = new ArrayList<ValueProvider<Double>>(num);
17+
iGains = new ArrayList<ValueProvider<Double>>(num);
18+
dGains = new ArrayList<ValueProvider<Double>>(num);
19+
ffGains = new ArrayList<ValueProvider<Double>>(num);
20+
outputMaxes = new ArrayList<Pair<ValueProvider<Double>, ValueProvider<Double>>>(num);
21+
}
22+
23+
public ValueProvider<Double> getP(int slot) {
24+
return pGains.get(slot);
25+
}
26+
27+
public void setP(ValueProvider<Double> value, int slot) {
28+
pGains.set(slot, value);
29+
}
30+
31+
public ValueProvider<Double> getI(int slot) {
32+
return iGains.get(slot);
33+
}
34+
35+
public void setI(ValueProvider<Double> value, int slot) {
36+
iGains.set(slot, value);
37+
}
38+
39+
public ValueProvider<Double> getD(int slot) {
40+
return dGains.get(slot);
41+
}
42+
43+
public void setD(ValueProvider<Double> value, int slot) {
44+
dGains.set(slot, value);
45+
}
46+
47+
public ValueProvider<Double> getFF(int slot) {
48+
return ffGains.get(slot);
49+
}
50+
51+
public void setFF(ValueProvider<Double> value, int slot) {
52+
ffGains.set(slot, value);
53+
}
54+
55+
public Pair<ValueProvider<Double>, ValueProvider<Double>> getOutputRange(int slot) {
56+
return outputMaxes.get(slot);
57+
}
58+
59+
public void setOutputRange(
60+
ValueProvider<Double> minValue, ValueProvider<Double> maxValue, int slot) {
61+
outputMaxes.set(
62+
slot, new Pair<ValueProvider<Double>, ValueProvider<Double>>(maxValue, maxValue));
63+
}
64+
65+
public void refreshPIDForSlot(MotorController motor, int slot) {
66+
if ((pGains.get(slot) != null) && pGains.get(slot).hasValue())
67+
motor.setP(pGains.get(slot).get(), slot);
68+
if ((iGains.get(slot) != null) && iGains.get(slot).hasValue())
69+
motor.setI(pGains.get(slot).get(), slot);
70+
if ((dGains.get(slot) != null) && dGains.get(slot).hasValue())
71+
motor.setD(pGains.get(slot).get(), slot);
72+
if ((ffGains.get(slot) != null) && ffGains.get(slot).hasValue())
73+
motor.setFF(pGains.get(slot).get(), slot);
74+
if (outputMaxes.get(slot) != null
75+
&& (outputMaxes.get(slot).getFirst() != null)
76+
&& (outputMaxes.get(slot).getSecond() != null)
77+
&& outputMaxes.get(slot).getFirst().hasValue()
78+
&& outputMaxes.get(slot).getSecond().hasValue()) {
79+
motor.setOutputRange(
80+
outputMaxes.get(slot).getFirst().get(),
81+
outputMaxes.get(slot).getSecond().get(),
82+
slot);
83+
}
84+
}
85+
}

0 commit comments

Comments
 (0)