Skip to content

Commit 1bf7773

Browse files
dabeycorndabeycorn
committed
feat: add BooleanEntry for replacing values during tune mode
Signed-off-by: Dasun Abeykoon <[email protected]>
1 parent 353d8fb commit 1bf7773

File tree

3 files changed

+113
-71
lines changed

3 files changed

+113
-71
lines changed

‎src/main/java/org/frc6423/robot/Constants.java‎

Lines changed: 12 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -18,16 +18,24 @@
1818
* <p>When creating a new constant, make sure it is static and final
1919
*/
2020
public class Constants {
21-
/** Constants that affect robot behavior during runtime */
21+
/** Constants that affect robot bhavior during runtime */
2222
public class Flags {
23-
/** Represents how often periodic robot logic will run */
23+
/**
24+
* Represents how often periodic robot logic will run
25+
*
26+
* </p> 0.02 Seconds by default
27+
*/
2428
public static final Time PERIOD = Seconds.of(0.02);
2529

2630
/** When true, the robot will enable debug menus and log debug information */
2731
@Deprecated public static final boolean debugMode = false;
2832

29-
/** When true, tunable entries will be added to NT for subsystems */
30-
public static final boolean TUNE_MODE = true;
33+
/**
34+
* When true, tunable entries will be added to NT for subsystems
35+
*
36+
* </p> Should be false by default
37+
*/
38+
public static final boolean TUNE_MODE = false;
3139
}
3240

3341
/** Constants representing port IDs that devices are connected to */

‎src/main/java/org/frc6423/robot/subsystems/superstructure/arm/Arm.java‎

Lines changed: 51 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
import edu.wpi.first.math.MathUtil;
1818
import edu.wpi.first.math.filter.LinearFilter;
1919
import edu.wpi.first.math.geometry.Rotation2d;
20+
import edu.wpi.first.networktables.BooleanEntry;
2021
import edu.wpi.first.networktables.DoubleEntry;
2122
import edu.wpi.first.units.measure.Angle;
2223
import edu.wpi.first.units.measure.AngularAcceleration;
@@ -94,36 +95,41 @@ private ArmAngle(Angle angle) {
9495

9596
private boolean isZeroed = false;
9697

97-
private static final String gainTopic = "Tunables/elevator";
98-
private static final DoubleEntry tunableKg = NtUtils.createDoubleEntry(gainTopic + "/kG", 0.0);
99-
private static final DoubleEntry tunableKs = NtUtils.createDoubleEntry(gainTopic + "/kS", 0.0);
100-
private static final DoubleEntry tunableKv = NtUtils.createDoubleEntry(gainTopic + "/kV", 0.0);
101-
private static final DoubleEntry tunableKa = NtUtils.createDoubleEntry(gainTopic + "/kA", 0.0);
102-
private static final DoubleEntry tunableKp = NtUtils.createDoubleEntry(gainTopic + "/kP", 0.0);
103-
private static final DoubleEntry tunableKd = NtUtils.createDoubleEntry(gainTopic + "/kD", 0.0);
104-
private static final DoubleEntry tunableMaxVel =
98+
/** Setup Tunable Values */
99+
private static final String gainTopic = "Tunables/arm";
100+
101+
private static final BooleanEntry replaceGainsEntry =
102+
NtUtils.createBooleanEntry(gainTopic + "/replaceGains", false);
103+
private static final DoubleEntry KgEntry = NtUtils.createDoubleEntry(gainTopic + "/kG", 0.0);
104+
private static final DoubleEntry KsEntry = NtUtils.createDoubleEntry(gainTopic + "/kS", 0.0);
105+
private static final DoubleEntry KvEntry = NtUtils.createDoubleEntry(gainTopic + "/kV", 0.0);
106+
private static final DoubleEntry KaEntry = NtUtils.createDoubleEntry(gainTopic + "/kA", 0.0);
107+
private static final DoubleEntry KpEntry = NtUtils.createDoubleEntry(gainTopic + "/kP", 0.0);
108+
private static final DoubleEntry KdEntry = NtUtils.createDoubleEntry(gainTopic + "/kD", 0.0);
109+
private static final DoubleEntry MaxVelEntry =
105110
NtUtils.createDoubleEntry(gainTopic + "/maxVel", 0.0);
106-
private static final DoubleEntry tunableMaxAccel =
111+
private static final DoubleEntry MaxAccelEntry =
107112
NtUtils.createDoubleEntry(gainTopic + "/maxAccel", 0.0);
108113

114+
/** Unpublish and close tunables if not in Tune Mode */
109115
static {
110116
if (!Flags.TUNE_MODE) {
111-
tunableKg.unpublish();
112-
tunableKs.unpublish();
113-
tunableKv.unpublish();
114-
tunableKa.unpublish();
115-
tunableKp.unpublish();
116-
tunableKd.unpublish();
117-
tunableMaxVel.unpublish();
118-
tunableMaxAccel.unpublish();
119-
tunableKg.close();
120-
tunableKs.close();
121-
tunableKv.close();
122-
tunableKa.close();
123-
tunableKp.close();
124-
tunableKd.close();
125-
tunableMaxVel.close();
126-
tunableMaxAccel.close();
117+
KgEntry.unpublish();
118+
KsEntry.unpublish();
119+
KvEntry.unpublish();
120+
KaEntry.unpublish();
121+
KpEntry.unpublish();
122+
KdEntry.unpublish();
123+
MaxVelEntry.unpublish();
124+
MaxAccelEntry.unpublish();
125+
KgEntry.close();
126+
KsEntry.close();
127+
KvEntry.close();
128+
KaEntry.close();
129+
KpEntry.close();
130+
KdEntry.close();
131+
MaxVelEntry.close();
132+
MaxAccelEntry.close();
127133
}
128134
}
129135

@@ -157,16 +163,17 @@ public void periodic() {
157163

158164
filteredCurrent = currentFilter.calculate(hardware.getStatorCurrentAmps());
159165

160-
if (Flags.TUNE_MODE) {
166+
if (Flags.TUNE_MODE && replaceGainsEntry.getAsBoolean()) {
161167
hardware.setGains(
162-
tunableKg.get(),
163-
tunableKs.get(),
164-
tunableKv.get(),
165-
tunableKa.get(),
166-
tunableKp.get(),
167-
tunableKd.get(),
168-
tunableMaxVel.get(),
169-
tunableMaxAccel.get());
168+
KgEntry.get(),
169+
KsEntry.get(),
170+
KvEntry.get(),
171+
KaEntry.get(),
172+
KpEntry.get(),
173+
KdEntry.get(),
174+
MaxVelEntry.get(),
175+
MaxAccelEntry.get());
176+
replaceGainsEntry.set(false);
170177
}
171178
}
172179

@@ -265,6 +272,16 @@ public Command holdAngle() {
265272

266273
@Override
267274
public void close() throws Exception {
275+
/** Close Tunable Entries */
276+
KgEntry.close();
277+
KsEntry.close();
278+
KvEntry.close();
279+
KaEntry.close();
280+
KpEntry.close();
281+
KdEntry.close();
282+
MaxVelEntry.close();
283+
MaxAccelEntry.close();
284+
268285
hardware.close();
269286
}
270287
}

‎src/main/java/org/frc6423/robot/subsystems/superstructure/elevator/Elevator.java‎

Lines changed: 50 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
import edu.wpi.first.math.filter.LinearFilter;
1818
import edu.wpi.first.math.geometry.Pose3d;
1919
import edu.wpi.first.math.geometry.Rotation3d;
20+
import edu.wpi.first.networktables.BooleanEntry;
2021
import edu.wpi.first.networktables.DoubleEntry;
2122
import edu.wpi.first.units.measure.Distance;
2223
import edu.wpi.first.units.measure.LinearAcceleration;
@@ -94,36 +95,41 @@ public static enum ElevatorExtension {
9495

9596
private boolean isZeroed = false;
9697

98+
/** Setup Tunable Values */
9799
private static final String gainTopic = "Tunables/elevator";
98-
private static final DoubleEntry tunableKg = NtUtils.createDoubleEntry(gainTopic + "/kG", 0.0);
99-
private static final DoubleEntry tunableKs = NtUtils.createDoubleEntry(gainTopic + "/kS", 0.0);
100-
private static final DoubleEntry tunableKv = NtUtils.createDoubleEntry(gainTopic + "/kV", 0.0);
101-
private static final DoubleEntry tunableKa = NtUtils.createDoubleEntry(gainTopic + "/kA", 0.0);
102-
private static final DoubleEntry tunableKp = NtUtils.createDoubleEntry(gainTopic + "/kP", 0.0);
103-
private static final DoubleEntry tunableKd = NtUtils.createDoubleEntry(gainTopic + "/kD", 0.0);
104-
private static final DoubleEntry tunableMaxVel =
100+
101+
private static final BooleanEntry replaceGainsEntry =
102+
NtUtils.createBooleanEntry(gainTopic + "/replaceGains", false);
103+
private static final DoubleEntry KgEntry = NtUtils.createDoubleEntry(gainTopic + "/kG", 0.0);
104+
private static final DoubleEntry KsEntry = NtUtils.createDoubleEntry(gainTopic + "/kS", 0.0);
105+
private static final DoubleEntry KvEntry = NtUtils.createDoubleEntry(gainTopic + "/kV", 0.0);
106+
private static final DoubleEntry KaEntry = NtUtils.createDoubleEntry(gainTopic + "/kA", 0.0);
107+
private static final DoubleEntry KpEntry = NtUtils.createDoubleEntry(gainTopic + "/kP", 0.0);
108+
private static final DoubleEntry KdEntry = NtUtils.createDoubleEntry(gainTopic + "/kD", 0.0);
109+
private static final DoubleEntry MaxVelEntry =
105110
NtUtils.createDoubleEntry(gainTopic + "/maxVel", 0.0);
106-
private static final DoubleEntry tunableMaxAccel =
111+
private static final DoubleEntry MaxAccelEntry =
107112
NtUtils.createDoubleEntry(gainTopic + "/maxAccel", 0.0);
108113

114+
/** Unpublish and close tunables if not in Tune Mode */
109115
static {
110116
if (!Flags.TUNE_MODE) {
111-
tunableKg.unpublish();
112-
tunableKs.unpublish();
113-
tunableKv.unpublish();
114-
tunableKa.unpublish();
115-
tunableKp.unpublish();
116-
tunableKd.unpublish();
117-
tunableMaxVel.unpublish();
118-
tunableMaxAccel.unpublish();
119-
tunableKg.close();
120-
tunableKs.close();
121-
tunableKv.close();
122-
tunableKa.close();
123-
tunableKp.close();
124-
tunableKd.close();
125-
tunableMaxVel.close();
126-
tunableMaxAccel.close();
117+
KgEntry.unpublish();
118+
KsEntry.unpublish();
119+
KvEntry.unpublish();
120+
KaEntry.unpublish();
121+
KpEntry.unpublish();
122+
KdEntry.unpublish();
123+
MaxVelEntry.unpublish();
124+
MaxAccelEntry.unpublish();
125+
KgEntry.close();
126+
KsEntry.close();
127+
KvEntry.close();
128+
KaEntry.close();
129+
KpEntry.close();
130+
KdEntry.close();
131+
MaxVelEntry.close();
132+
MaxAccelEntry.close();
127133
}
128134
}
129135

@@ -157,16 +163,17 @@ public void periodic() {
157163

158164
filteredCurrent = currentFilter.calculate(hardware.getParentStatorCurrentAmps());
159165

160-
if (Flags.TUNE_MODE) {
166+
if (Flags.TUNE_MODE && replaceGainsEntry.getAsBoolean()) {
161167
hardware.setGains(
162-
tunableKg.get(),
163-
tunableKs.get(),
164-
tunableKv.get(),
165-
tunableKa.get(),
166-
tunableKp.get(),
167-
tunableKd.get(),
168-
tunableMaxVel.get(),
169-
tunableMaxAccel.get());
168+
KgEntry.get(),
169+
KsEntry.get(),
170+
KvEntry.get(),
171+
KaEntry.get(),
172+
KpEntry.get(),
173+
KdEntry.get(),
174+
MaxVelEntry.get(),
175+
MaxAccelEntry.get());
176+
replaceGainsEntry.set(false);
170177
}
171178
}
172179

@@ -286,6 +293,16 @@ public Command holdExtension() {
286293

287294
@Override
288295
public void close() throws Exception {
296+
/** Close Tunable Entries */
297+
KgEntry.close();
298+
KsEntry.close();
299+
KvEntry.close();
300+
KaEntry.close();
301+
KpEntry.close();
302+
KdEntry.close();
303+
MaxVelEntry.close();
304+
MaxAccelEntry.close();
305+
289306
hardware.close();
290307
}
291308
}

0 commit comments

Comments
 (0)