Skip to content

Commit 95b313d

Browse files
mwitcpalekmwitcpalek
authored
Merge branch 'master' into jhh/migrating-to-yaml-syntax
2 parents aef3ebe + 53e7685 commit 95b313d

File tree

8 files changed

+151
-98
lines changed

8 files changed

+151
-98
lines changed

‎gradle.properties‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
thirdcoastVersion = 19.2.0
1+
thirdcoastVersion = 19.4.1
22
logbackVersion = 1.2.3
33
kotlinVersion = 1.3.21
44
junitVersion = 5.4.0

‎src/main/java/frc/team2767/deepspace/command/intake/IntakePositionCommand.java‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,10 @@ public IntakePositionCommand(double angle) {
1616

1717
@Override
1818
protected void initialize() {
19+
if (angle == IntakeSubsystem.kStowPositionDeg)
20+
INTAKE.setCurrentLimit(IntakeSubsystem.ShoulderILimit.STOW);
21+
else INTAKE.setCurrentLimit(IntakeSubsystem.ShoulderILimit.NORMAL);
22+
1923
INTAKE.setPosition(angle);
2024
}
2125

‎src/main/java/frc/team2767/deepspace/subsystem/BiscuitSubsystem.java‎

Lines changed: 13 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -15,11 +15,11 @@
1515
import org.slf4j.Logger;
1616
import org.slf4j.LoggerFactory;
1717
import org.strykeforce.thirdcoast.telemetry.TelemetryService;
18-
import org.strykeforce.thirdcoast.telemetry.grapher.Measure;
19-
import org.strykeforce.thirdcoast.telemetry.item.Item;
18+
import org.strykeforce.thirdcoast.telemetry.item.Measurable;
19+
import org.strykeforce.thirdcoast.telemetry.item.Measure;
2020
import org.strykeforce.thirdcoast.telemetry.item.TalonItem;
2121

22-
public class BiscuitSubsystem extends Subsystem implements Limitable, Zeroable, Item {
22+
public class BiscuitSubsystem extends Subsystem implements Limitable, Zeroable, Measurable {
2323

2424
public static final double PLACE_RIGHT = 45.0;
2525
public static final double PLACE_LEFT = -45.0;
@@ -370,6 +370,9 @@ public void setLimits(int forward, int reverse) {
370370
}
371371

372372
// --------------------------GRAPHER---------------------------------
373+
private static final String COUNTER = "COUNTER";
374+
private static final String COMPRESSION = "COMPRESSION";
375+
373376
@NotNull
374377
@Override
375378
public String getDescription() {
@@ -384,7 +387,8 @@ public int getDeviceId() {
384387
@NotNull
385388
@Override
386389
public Set<Measure> getMeasures() {
387-
return Set.of(Measure.VALUE, Measure.ANALOG_IN_RAW);
390+
return Set.of(
391+
new Measure(COUNTER, "Counter"), new Measure(COMPRESSION, "Compression Raw Analog"));
388392
}
389393

390394
@NotNull
@@ -394,18 +398,18 @@ public String getType() {
394398
}
395399

396400
@Override
397-
public int compareTo(@NotNull Item item) {
401+
public int compareTo(@NotNull Measurable item) {
398402
return 0;
399403
}
400404

401405
@NotNull
402406
@Override
403407
public DoubleSupplier measurementFor(@NotNull Measure measure) {
404-
switch (measure) {
405-
case VALUE:
408+
switch (measure.getName()) {
409+
case COUNTER:
406410
return () -> graphCount;
407-
case ANALOG_IN_RAW:
408-
return () -> getCompression();
411+
case COMPRESSION:
412+
return this::getCompression;
409413
default:
410414
return () -> 2767.0;
411415
}

‎src/main/java/frc/team2767/deepspace/subsystem/ClimbSubsystem.java‎

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -13,11 +13,11 @@
1313
import org.slf4j.Logger;
1414
import org.slf4j.LoggerFactory;
1515
import org.strykeforce.thirdcoast.telemetry.TelemetryService;
16-
import org.strykeforce.thirdcoast.telemetry.grapher.Measure;
17-
import org.strykeforce.thirdcoast.telemetry.item.Item;
16+
import org.strykeforce.thirdcoast.telemetry.item.Measurable;
17+
import org.strykeforce.thirdcoast.telemetry.item.Measure;
1818
import org.strykeforce.thirdcoast.telemetry.item.TalonItem;
1919

20-
public class ClimbSubsystem extends Subsystem implements Item {
20+
public class ClimbSubsystem extends Subsystem implements Measurable {
2121

2222
// max 843
2323
// min 139
@@ -221,10 +221,12 @@ public int getDeviceId() {
221221
return 0;
222222
}
223223

224+
private static final String STRING_POT = "STRING_POT";
225+
224226
@NotNull
225227
@Override
226228
public Set<Measure> getMeasures() {
227-
return Set.of(Measure.ANALOG_IN_RAW);
229+
return Set.of(new Measure(STRING_POT, "String Pot Raw Analog"));
228230
}
229231

230232
@NotNull
@@ -234,15 +236,15 @@ public String getType() {
234236
}
235237

236238
@Override
237-
public int compareTo(@NotNull Item item) {
239+
public int compareTo(@NotNull Measurable item) {
238240
return 0;
239241
}
240242

241243
@NotNull
242244
@Override
243245
public DoubleSupplier measurementFor(@NotNull Measure measure) {
244-
switch (measure) {
245-
case ANALOG_IN_RAW:
246+
switch (measure.getName()) {
247+
case STRING_POT:
246248
return this::getStringPotPosition;
247249
default:
248250
return () -> 2767.0;

‎src/main/java/frc/team2767/deepspace/subsystem/DriveSubsystem.java‎

Lines changed: 37 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -25,21 +25,25 @@
2525
import org.strykeforce.thirdcoast.swerve.SwerveDriveConfig;
2626
import org.strykeforce.thirdcoast.swerve.Wheel;
2727
import org.strykeforce.thirdcoast.telemetry.TelemetryService;
28-
import org.strykeforce.thirdcoast.telemetry.grapher.Measure;
29-
import org.strykeforce.thirdcoast.telemetry.item.Item;
28+
import org.strykeforce.thirdcoast.telemetry.item.Measurable;
29+
import org.strykeforce.thirdcoast.telemetry.item.Measure;
3030
import org.strykeforce.thirdcoast.telemetry.item.TalonItem;
3131

32-
public class DriveSubsystem extends Subsystem implements Item {
32+
public class DriveSubsystem extends Subsystem implements Measurable {
3333

3434
public static final double TICKS_PER_INCH = 2500;
3535
public static final double TICKS_PER_TOOTH = 107.8;
3636
private static final double DRIVE_SETPOINT_MAX = 25_000.0;
3737
private static final double ROBOT_LENGTH = 21.0;
3838
private static final double ROBOT_WIDTH = 26.0;
3939
private static final int NUM_WHEELS = 4;
40-
40+
private static final String GYRO_ANGLE = "GYRO_ANGE";
41+
private static final String YAW_ERROR = "YAW_ERROR";
42+
private static final String PATH_POSITION = "PATH_POSITION";
43+
private static final String GYRO_ERROR = "GYRO_ERROR";
44+
private static final String YAW_TARGET = "YAW_TARGET";
45+
private static final String STRAFE = "STRAFE";
4146
private static double offsetGyro;
42-
4347
private static boolean enableDriveAxisFlip = false;
4448
private static Wheel[] wheels;
4549
private final SwerveDrive swerve = configSwerve();
@@ -65,6 +69,10 @@ public void sandstormAxisFlip(boolean isCameraOriented) {
6569
setEnableDriveAxisFlip(isCameraOriented);
6670
}
6771

72+
////////////////////////////////////////////////////////////////////////////
73+
// PATHFINDER
74+
////////////////////////////////////////////////////////////////////////////
75+
6876
private void setEnableDriveAxisFlip(boolean enable) {
6977
enableDriveAxisFlip = enable;
7078
logger.debug("driving is {} oriented", enable);
@@ -91,14 +99,14 @@ public void drive(double forward, double strafe, double yaw) {
9199
}
92100
}
93101

102+
////////////////////////////////////////////////////////////////////////////
103+
// TWIST
104+
////////////////////////////////////////////////////////////////////////////
105+
94106
public void stop() {
95107
swerve.stop();
96108
}
97109

98-
////////////////////////////////////////////////////////////////////////////
99-
// PATHFINDER
100-
////////////////////////////////////////////////////////////////////////////
101-
102110
public double getAverageOutputCurrent() {
103111
double sum = 0;
104112

@@ -141,14 +149,12 @@ public void interruptPath() {
141149
pathController.interrupt();
142150
}
143151

152+
////////////////////////////////////////////////////////////////////////////
153+
144154
public void setTargetYaw(double targetYaw) {
145155
this.targetYaw = targetYaw;
146156
}
147157

148-
////////////////////////////////////////////////////////////////////////////
149-
// TWIST
150-
////////////////////////////////////////////////////////////////////////////
151-
152158
public void startTwist(double heading, int distance, double targetYaw) {
153159
logger.info("heading={} distance={} targetYaw={}", heading, distance, targetYaw);
154160
twistController = new TwistController(swerve, heading, distance, targetYaw);
@@ -180,8 +186,6 @@ public void setFieldOriented(boolean isFieldOriented) {
180186
swerve.setFieldOriented(isFieldOriented);
181187
}
182188

183-
////////////////////////////////////////////////////////////////////////////
184-
185189
public void setWheelAzimuthPosition(List<Integer> positions) {
186190
Wheel[] wheels = swerve.getWheels();
187191
for (int i = 0; i < NUM_WHEELS; i++) wheels[i].setAzimuthPosition(positions.get(i));
@@ -221,6 +225,10 @@ public void setGyroOffset(double angle) {
221225
gyro.setAngleAdjustment(adj);
222226
}
223227

228+
////////////////////////////////////////////////////////////////////////////
229+
// SWERVE CONFIG
230+
////////////////////////////////////////////////////////////////////////////
231+
224232
public void undoGyroOffset() {
225233
AHRS gyro = swerve.getGyro();
226234
double adj = gyro.getAngleAdjustment();
@@ -258,10 +266,6 @@ public SwerveDrive getSwerveDrive() {
258266
return swerve;
259267
}
260268

261-
////////////////////////////////////////////////////////////////////////////
262-
// SWERVE CONFIG
263-
////////////////////////////////////////////////////////////////////////////
264-
265269
public void setAngleOrthogonalAngle() {
266270
double[] angles = new double[] {-90.0, 0.0, 90.0, 180.0, -180.0};
267271
double[] differences = new double[5];
@@ -383,12 +387,12 @@ public int getDeviceId() {
383387
@Override
384388
public Set<Measure> getMeasures() {
385389
return Set.of(
386-
Measure.ANGLE,
387-
Measure.CLOSED_LOOP_ERROR,
388-
Measure.CLOSED_LOOP_TARGET,
389-
Measure.VALUE,
390-
Measure.COMPONENT_STRAFE,
391-
Measure.DISPLACEMENT_EXPECTED);
390+
new Measure(GYRO_ANGLE, "Gyro Angle"),
391+
new Measure(YAW_ERROR, "Yaw Error"),
392+
new Measure(PATH_POSITION, "Path Position"),
393+
new Measure(GYRO_ERROR, "Gyro Error"),
394+
new Measure(YAW_TARGET, "Yaw Target"),
395+
new Measure(STRAFE, "Strafe"));
392396
}
393397

394398
@NotNull
@@ -398,25 +402,25 @@ public String getType() {
398402
}
399403

400404
@Override
401-
public int compareTo(@NotNull Item item) {
405+
public int compareTo(@NotNull Measurable item) {
402406
return 0;
403407
}
404408

405409
@NotNull
406410
@Override
407411
public DoubleSupplier measurementFor(@NotNull Measure measure) {
408-
switch (measure) {
409-
case ANGLE:
412+
switch (measure.getName()) {
413+
case GYRO_ANGLE:
410414
return () -> Math.IEEEremainder(getGyro().getAngle(), 360);
411-
case CLOSED_LOOP_ERROR:
415+
case YAW_ERROR:
412416
return () -> yawError; // yaw error
413-
case CLOSED_LOOP_TARGET:
417+
case PATH_POSITION:
414418
return () -> (isPath ? pathController.getSetpointPos() : 0.0);
415-
case VALUE:
419+
case GYRO_ERROR:
416420
return () -> targetYaw - getGyro().getAngle();
417-
case DISPLACEMENT_EXPECTED:
421+
case YAW_TARGET:
418422
return () -> targetYaw;
419-
case COMPONENT_STRAFE:
423+
case STRAFE:
420424
return () -> graphableStrafe;
421425
default:
422426
return () -> 2767.0;

‎src/main/java/frc/team2767/deepspace/subsystem/IntakeSubsystem.java‎

Lines changed: 44 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -16,11 +16,11 @@
1616
import org.slf4j.Logger;
1717
import org.slf4j.LoggerFactory;
1818
import org.strykeforce.thirdcoast.telemetry.TelemetryService;
19-
import org.strykeforce.thirdcoast.telemetry.grapher.Measure;
20-
import org.strykeforce.thirdcoast.telemetry.item.Item;
19+
import org.strykeforce.thirdcoast.telemetry.item.Measurable;
20+
import org.strykeforce.thirdcoast.telemetry.item.Measure;
2121
import org.strykeforce.thirdcoast.telemetry.item.TalonItem;
2222

23-
public class IntakeSubsystem extends Subsystem implements Limitable, Zeroable, Item {
23+
public class IntakeSubsystem extends Subsystem implements Limitable, Zeroable, Measurable {
2424

2525
private static final int TICKS_PER_DEGREE = 90;
2626
private static final int TICKS_OFFSET = 9664;
@@ -30,6 +30,8 @@ public class IntakeSubsystem extends Subsystem implements Limitable, Zeroable, I
3030
private static final int INTAKE_SLOW_BEAM_ID = 4;
3131
private static final int STABLE_THRESH = 4;
3232
private static final String PREFS_NAME = "IntakeSubsystem/Settings/";
33+
private static final String BEAM = "BEAM";
34+
private static final String INTAKE_BEAM = "INTAKE_BEAM";
3335
public static double kStowPositionDeg;
3436
public static double kZeroPositionTicks;
3537
public static double kMiddlePositionDeg;
@@ -48,6 +50,7 @@ public class IntakeSubsystem extends Subsystem implements Limitable, Zeroable, I
4850

4951
private int currentForwardLimit;
5052
private int currentReverseLimit;
53+
private ShoulderILimit currentLimit = ShoulderILimit.NORMAL;
5154

5255
public IntakeSubsystem() {
5356

@@ -244,6 +247,26 @@ public void setPosition(double angle) {
244247
shoulder.set(ControlMode.MotionMagic, setpointTicks);
245248
}
246249

250+
public void setCurrentLimit(ShoulderILimit limit) {
251+
if (limit != currentLimit) {
252+
switch (limit) {
253+
case STOW:
254+
shoulder.configContinuousCurrentLimit(ShoulderILimit.STOW.continuous, 10);
255+
shoulder.configPeakCurrentLimit(ShoulderILimit.STOW.peak, 10);
256+
logger.info("shoulder current limit: STOW");
257+
break;
258+
case NORMAL:
259+
shoulder.configContinuousCurrentLimit(ShoulderILimit.NORMAL.continuous, 10);
260+
shoulder.configPeakCurrentLimit(ShoulderILimit.NORMAL.peak, 10);
261+
logger.info("shoulder current limit: NORMAL");
262+
break;
263+
}
264+
}
265+
currentLimit = limit;
266+
}
267+
268+
// ------------------------GRAPHER-------------------------------------------------
269+
247270
@NotNull
248271
@Override
249272
public String getDescription() {
@@ -258,7 +281,7 @@ public int getDeviceId() {
258281
@NotNull
259282
@Override
260283
public Set<Measure> getMeasures() {
261-
return Set.of(Measure.VALUE, Measure.UNKNOWN);
284+
return Set.of(new Measure(BEAM, "Beam"), new Measure(INTAKE_BEAM, "Intake Beam"));
262285
}
263286

264287
@NotNull
@@ -268,17 +291,17 @@ public String getType() {
268291
}
269292

270293
@Override
271-
public int compareTo(@NotNull Item item) {
294+
public int compareTo(@NotNull Measurable item) {
272295
return 0;
273296
}
274297

275298
@NotNull
276299
@Override
277300
public DoubleSupplier measurementFor(@NotNull Measure measure) {
278-
switch (measure) {
279-
case VALUE:
301+
switch (measure.getName()) {
302+
case BEAM:
280303
return this::graphBeam;
281-
case UNKNOWN:
304+
case INTAKE_BEAM:
282305
return this::graphIntakeBeam;
283306
default:
284307
return () -> 2767;
@@ -302,4 +325,17 @@ public boolean isBeamBroken() {
302325
public boolean isIntakeSlowBeamBroken() {
303326
return !intakeBeamBreak.get();
304327
}
328+
329+
public enum ShoulderILimit {
330+
NORMAL(10, 15),
331+
STOW(1, 2);
332+
333+
public final int continuous;
334+
public final int peak;
335+
336+
ShoulderILimit(int continuous, int peak) {
337+
this.continuous = continuous;
338+
this.peak = peak;
339+
}
340+
}
305341
}

0 commit comments

Comments
 (0)