Skip to content

Commit f976d6f

Browse files
ds12ads12a
committed
builds but untested
1 parent d6bf8e5 commit f976d6f

File tree

12 files changed

+184
-25
lines changed

12 files changed

+184
-25
lines changed

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 24 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -40,10 +40,12 @@
4040
import frc.robot.commands.elevator.SetElevatorPositionCommand;
4141
import frc.robot.commands.elevator.ZeroElevatorCommand;
4242
import frc.robot.commands.robotState.AutoReefCycleCommand;
43+
import frc.robot.commands.robotState.DepthChargeHealthCheckCommand;
4344
import frc.robot.commands.robotState.FloorAlgaeCommand;
4445
import frc.robot.commands.robotState.ForceProcessorCommand;
4546
import frc.robot.commands.robotState.HPAlgaeCommand;
4647
import frc.robot.commands.robotState.InterruptAutoCommand;
48+
import frc.robot.commands.robotState.LockWheelsCommand;
4749
import frc.robot.commands.robotState.ReefCycleCommand;
4850
import frc.robot.commands.robotState.ScoreAlgaeCommand;
4951
import frc.robot.commands.robotState.SetScoreSideCommand;
@@ -54,7 +56,6 @@
5456
import frc.robot.commands.robotState.ToggleAlgaeHeightCommand;
5557
import frc.robot.commands.robotState.ToggleAutoPlacingCommand;
5658
import frc.robot.commands.robotState.ToggleGetAlgaeCommand;
57-
import frc.robot.commands.robotState.lockwheelscommand;
5859
import frc.robot.commands.robotState.setScoreSideLeftCommand;
5960
import frc.robot.commands.vision.SetVisionUpdatesCommand;
6061
import frc.robot.constants.BiscuitConstants;
@@ -71,13 +72,11 @@
7172
import frc.robot.subsystems.climb.ClimbIO;
7273
import frc.robot.subsystems.climb.ClimbIOServoFX;
7374
import frc.robot.subsystems.climb.ClimbSubsystem;
74-
import frc.robot.subsystems.coral.CoralIO;
7575
import frc.robot.subsystems.coral.CoralIOFX;
7676
import frc.robot.subsystems.coral.CoralSubsystem;
7777
import frc.robot.subsystems.drive.DriveSubsystem;
7878
import frc.robot.subsystems.drive.Swerve;
7979
import frc.robot.subsystems.drive.SwerveFXS;
80-
import frc.robot.subsystems.elevator.ElevatorIO;
8180
import frc.robot.subsystems.elevator.ElevatorIOFX;
8281
import frc.robot.subsystems.elevator.ElevatorSubsystem;
8382
import frc.robot.subsystems.elevator.ElevatorSubsystem.ElevatorStates;
@@ -110,14 +109,14 @@ public class RobotContainer {
110109
private final ClimbIO climbIO;
111110
private final ClimbSubsystem climbSubsystem;
112111

113-
private final CoralIO coralIO;
112+
private final CoralIOFX coralIO;
114113
private final CoralSubsystem coralSubsystem;
115114

116115
private Swerve protoSwerve;
117116
private SwerveFXS swerve;
118117
private final DriveSubsystem driveSubsystem;
119118

120-
private final ElevatorIO elevatorIO;
119+
private final ElevatorIOFX elevatorIO;
121120
private final ElevatorSubsystem elevatorSubsystem;
122121

123122
private final FunnelIOFXS funnelIO;
@@ -602,7 +601,7 @@ private void configurePitDashboard() {
602601
.withSize(1, 1);
603602

604603
Shuffleboard.getTab("Pit")
605-
.add("Lock Wheels", new lockwheelscommand(driveSubsystem))
604+
.add("Lock Wheels", new LockWheelsCommand(driveSubsystem))
606605
.withPosition(3, 1)
607606
.withSize(1, 1);
608607

@@ -615,6 +614,25 @@ private void configurePitDashboard() {
615614
.add("Stop Azimuths", new StopAllAxisCommand(robotStateSubsystem).ignoringDisable(true))
616615
.withPosition(5, 1)
617616
.withSize(1, 1);
617+
618+
Shuffleboard.getTab("Pit")
619+
.add(
620+
"Health Check",
621+
new DepthChargeHealthCheckCommand(
622+
swerve,
623+
driveSubsystem,
624+
funnelIO,
625+
funnelSubsystem,
626+
coralIO,
627+
coralSubsystem,
628+
algaeIO,
629+
algaeSubsystem,
630+
elevatorIO,
631+
elevatorSubsystem,
632+
biscuitIO,
633+
biscuitSubsystem))
634+
.withPosition(6, 1)
635+
.withSize(1, 1);
618636
}
619637

620638
public void configTestDash() {

‎src/main/java/frc/robot/commands/robotState/DepthChargeHealthCheckCommand.java‎

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
package frc.robot.commands.robotState;
2+
3+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
4+
import frc.robot.subsystems.algae.AlgaeIOFX;
5+
import frc.robot.subsystems.algae.AlgaeSubsystem;
6+
import frc.robot.subsystems.biscuit.BiscuitIOFXS;
7+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
8+
import frc.robot.subsystems.coral.CoralIOFX;
9+
import frc.robot.subsystems.coral.CoralSubsystem;
10+
import frc.robot.subsystems.drive.DriveSubsystem;
11+
import frc.robot.subsystems.drive.SwerveFXS;
12+
import frc.robot.subsystems.elevator.ElevatorIOFX;
13+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
14+
import frc.robot.subsystems.funnel.FunnelIOFXS;
15+
import frc.robot.subsystems.funnel.FunnelSubsystem;
16+
import java.util.List;
17+
import org.strykeforce.healthcheck.IOHealthCheckCommand;
18+
19+
public class DepthChargeHealthCheckCommand extends SequentialCommandGroup {
20+
private SwerveFXS swerve;
21+
private DriveSubsystem driveSubsystem;
22+
private FunnelIOFXS funnelIOFXS;
23+
private FunnelSubsystem funnelSubsystem;
24+
private CoralIOFX coralIOFXS;
25+
private CoralSubsystem coralSubsystem;
26+
private AlgaeIOFX algaeIOFXS;
27+
private AlgaeSubsystem algaeSubsystem;
28+
private ElevatorIOFX elevatorIOFX;
29+
private ElevatorSubsystem elevatorSubsystem;
30+
private BiscuitIOFXS biscuitIOFXS;
31+
private BiscuitSubsystem biscuitSubsystem;
32+
33+
public DepthChargeHealthCheckCommand(
34+
SwerveFXS swerve,
35+
DriveSubsystem driveSubsystem,
36+
FunnelIOFXS funnelIOFXS,
37+
FunnelSubsystem funnelSubsystem,
38+
CoralIOFX coralIOFXS,
39+
CoralSubsystem coralSubsystem,
40+
AlgaeIOFX algaeIOFXS,
41+
AlgaeSubsystem algaeSubsystem,
42+
ElevatorIOFX elevatorIOFX,
43+
ElevatorSubsystem elevatorSubsystem,
44+
BiscuitIOFXS biscuitIOFXS,
45+
BiscuitSubsystem biscuitSubsystem) {
46+
47+
this.swerve = swerve;
48+
this.driveSubsystem = driveSubsystem;
49+
this.funnelIOFXS = funnelIOFXS;
50+
this.funnelSubsystem = funnelSubsystem;
51+
this.coralIOFXS = coralIOFXS;
52+
this.coralSubsystem = coralSubsystem;
53+
this.algaeIOFXS = algaeIOFXS;
54+
this.algaeSubsystem = algaeSubsystem;
55+
this.elevatorIOFX = elevatorIOFX;
56+
this.elevatorSubsystem = elevatorSubsystem;
57+
this.biscuitIOFXS = biscuitIOFXS;
58+
this.biscuitSubsystem = biscuitSubsystem;
59+
60+
addCommands(
61+
new ResetCaseHealthCheckCommand(),
62+
new IOHealthCheckCommand(
63+
List.of(
64+
driveSubsystem,
65+
funnelSubsystem,
66+
coralSubsystem,
67+
algaeSubsystem,
68+
elevatorSubsystem,
69+
biscuitSubsystem),
70+
swerve,
71+
funnelIOFXS,
72+
coralIOFXS,
73+
algaeIOFXS,
74+
elevatorIOFX,
75+
biscuitIOFXS),
76+
new LockWheelsCommand(driveSubsystem));
77+
}
78+
}

‎src/main/java/frc/robot/commands/robotState/ResetCaseHealthCheckCommand.java‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
package frc.robot.commands.robotState;
2+
3+
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import org.strykeforce.healthcheck.internal.ResetCaseNum;
5+
6+
public class ResetCaseHealthCheckCommand extends InstantCommand {
7+
public ResetCaseHealthCheckCommand() {}
8+
9+
@Override
10+
public void initialize() {
11+
new ResetCaseNum().resetCaseId();
12+
}
13+
}

‎src/main/java/frc/robot/commands/robotState/lockwheelscommand.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,10 +3,10 @@
33
import edu.wpi.first.wpilibj2.command.InstantCommand;
44
import frc.robot.subsystems.drive.DriveSubsystem;
55

6-
public class lockwheelscommand extends InstantCommand {
6+
public class LockWheelsCommand extends InstantCommand {
77
private DriveSubsystem driveSubsystem;
88

9-
public lockwheelscommand(DriveSubsystem driveSubsystem) {
9+
public LockWheelsCommand(DriveSubsystem driveSubsystem) {
1010
this.driveSubsystem = driveSubsystem;
1111
}
1212

‎src/main/java/frc/robot/constants/ElevatorConstants.java‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,6 @@ public class ElevatorConstants {
4141
public static final Angle kHealthCheck = Rotations.of(24);
4242
// Idle
4343

44-
4544
// Algae removal
4645
public static final Angle kL2AlgaeSetpoint = Rotations.of(6.308);
4746
public static final Angle kL3AlgaeSetpoint = Rotations.of(17.513);

‎src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java‎

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -13,11 +13,20 @@
1313
import frc.robot.constants.AlgaeConstants;
1414
import org.slf4j.Logger;
1515
import org.slf4j.LoggerFactory;
16+
import org.strykeforce.healthcheck.Checkable;
17+
import org.strykeforce.healthcheck.HealthCheck;
18+
import org.strykeforce.healthcheck.Timed;
1619
import org.strykeforce.telemetry.TelemetryService;
1720

18-
public class AlgaeIOFX implements AlgaeIO {
21+
public class AlgaeIOFX implements AlgaeIO, Checkable {
1922
private Logger logger;
23+
24+
@HealthCheck
25+
@Timed(
26+
percentOutput = {-0.1, -1},
27+
duration = 2)
2028
private TalonFXS talonFXS;
29+
2130
private AlgaeIOInputs inputs;
2231

2332
private TalonFXSConfigurator configurator;
@@ -63,4 +72,9 @@ public void setPct(double pct) {
6372
public void registerWith(TelemetryService telemetryService) {
6473
telemetryService.register(talonFXS, true);
6574
}
75+
76+
@Override
77+
public String getName() {
78+
return "Algae";
79+
}
6680
}

‎src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFXS.java‎

Lines changed: 25 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
package frc.robot.subsystems.biscuit;
22

3+
import static edu.wpi.first.units.Units.Rotations;
4+
35
import com.ctre.phoenix6.BaseStatusSignal;
46
import com.ctre.phoenix6.StatusSignal;
57
import com.ctre.phoenix6.configs.TalonFXSConfiguration;
@@ -17,11 +19,21 @@
1719
import frc.robot.subsystems.biscuit.BiscuitIO.BiscuitIOInputs;
1820
import org.slf4j.Logger;
1921
import org.slf4j.LoggerFactory;
22+
import org.strykeforce.healthcheck.AfterHealthCheck;
23+
import org.strykeforce.healthcheck.BeforeHealthCheck;
24+
import org.strykeforce.healthcheck.Checkable;
25+
import org.strykeforce.healthcheck.HealthCheck;
26+
import org.strykeforce.healthcheck.Position;
2027
import org.strykeforce.telemetry.TelemetryService;
2128

22-
public class BiscuitIOFXS implements BiscuitIO {
29+
public class BiscuitIOFXS implements BiscuitIO, Checkable {
2330

2431
private Logger logger;
32+
33+
@HealthCheck
34+
@Position(
35+
percentOutput = {-0.1, 0.1},
36+
encoderChange = 40)
2537
private TalonFXS talon;
2638

2739
private final Angle sensorInitial;
@@ -34,7 +46,7 @@ public class BiscuitIOFXS implements BiscuitIO {
3446
private boolean fwdLimitSwitchOpen;
3547
private Angle offset;
3648
private Alert rangeAlert = new Alert("Biscuit overextended! Shuting down!", AlertType.kError);
37-
private Boolean lastHadAlgae = false;
49+
private boolean lastHadAlgae = false;
3850

3951
TalonFXSConfigurator configurator;
4052
private MotionMagicDutyCycle positionRequest =
@@ -113,4 +125,15 @@ public boolean zero() {
113125
}
114126
return didZero;
115127
}
128+
129+
@Override
130+
public String getName() {
131+
return "Biscuit";
132+
}
133+
134+
@BeforeHealthCheck
135+
@AfterHealthCheck
136+
public void healthCheckPos() {
137+
setPosition(Rotations.of(0), false);
138+
}
116139
}

‎src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@ public class BiscuitSubsystem extends MeasurableSubsystem {
2222
private BiscuitIO io;
2323
private BiscuitIOInputsAutoLogged inputs = new BiscuitIOInputsAutoLogged();
2424
private Angle setPoint = Rotations.of(0);
25-
private Boolean hasZeroed = false;
25+
private boolean hasZeroed = false;
2626

2727
public BiscuitSubsystem(BiscuitIO io) {
2828
this.logger = LoggerFactory.getLogger(this.getClass());

‎src/main/java/frc/robot/subsystems/coral/CoralIOFX.java‎

Lines changed: 14 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,11 +15,19 @@
1515
import frc.robot.constants.CoralConstants;
1616
import org.slf4j.Logger;
1717
import org.slf4j.LoggerFactory;
18+
import org.strykeforce.healthcheck.Checkable;
19+
import org.strykeforce.healthcheck.HealthCheck;
20+
import org.strykeforce.healthcheck.Timed;
1821
import org.strykeforce.telemetry.TelemetryService;
1922

20-
public class CoralIOFX implements CoralIO {
23+
public class CoralIOFX implements CoralIO, Checkable {
2124
// private objects
2225
private Logger logger;
26+
27+
@HealthCheck
28+
@Timed(
29+
percentOutput = {0.1, 1},
30+
duration = 2)
2331
private TalonFXS talonFx;
2432

2533
// FX Access objects
@@ -84,4 +92,9 @@ public void updateInputs(CoralIOInputs inputs) {
8492
public void registerWith(TelemetryService telemetryService) {
8593
telemetryService.register(talonFx, true);
8694
}
95+
96+
@Override
97+
public String getName() {
98+
return "Coral";
99+
}
87100
}

‎src/main/java/frc/robot/subsystems/drive/SwerveFXS.java‎

Lines changed: 2 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
import org.strykeforce.gyro.SF_AHRS;
2626
import org.strykeforce.gyro.SF_PIGEON2;
2727
import org.strykeforce.healthcheck.Checkable;
28+
import org.strykeforce.healthcheck.HealthCheck;
2829
import org.strykeforce.swerve.FXSwerveModule;
2930
import org.strykeforce.swerve.OdometryStrategy;
3031
import org.strykeforce.swerve.PoseEstimatorOdometryStrategy;
@@ -34,12 +35,8 @@
3435
import org.strykeforce.swerve.V6TalonSwerveModule.ClosedLoopUnits;
3536
import org.strykeforce.telemetry.TelemetryService;
3637

37-
38-
import org.strykeforce.healthcheck.Checkable;
39-
import org.strykeforce.healthcheck.HealthCheck;
40-
4138
public class SwerveFXS implements SwerveIO, Checkable {
42-
@HealthCheck private final SwerveDrive swerveDrive;
39+
@HealthCheck private final SwerveDrive swerveDrive;
4340

4441
// Grapher stuff
4542
private PoseEstimatorOdometryStrategy odometryStrategy;

0 commit comments

Comments
 (0)