Team4388/Robot-Essentials
1
0
Fork 0
mirror of https://github.com/Team4388/Robot-Essentials.git synced 2026-06-08 16:28:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

add diagnosticStatus for SwerveModule, and add diagnoseHardwareCTRE inside Status

Browse Source
This commit is contained in:
C4llSiqn
2024-11-28 16:55:08 -07:00
parent 63f127e9b5
commit b916df256d
2 changed files with 49 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+13 -2
View File
@@ -16,12 +16,14 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.ControlRequest;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.MagnetHealthValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import com.ctre.phoenix6.hardware.CANcoder;
@@ -251,10 +253,19 @@ public class SwerveModule extends Subsystem {
return status;
}
public boolean motorsAlive() {
return this.driveMotor.isAlive() && this.angleMotor.isAlive();
}
@Override
public Status diagnosticStatus() {
Log("Diagnostic info for this swereve module has not been inplemented!"); //TODO
return new Status();
Status status = new Status();
status.diagnoseHardwareCTRE("Drive", this.driveMotor);
status.diagnoseHardwareCTRE("Angle", this.angleMotor);
status.diagnoseHardwareCTRE("Steer", this.encoder);
return status;
}
// public double getCurrent() {
src/main/java/frc4388/utility/Status.java
+36
View File
@@ -3,6 +3,12 @@ package frc4388.utility;
import java.util.ArrayList;
import java.util.List;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.controls.EmptyControl;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.hardware.TalonFX;
public class Status {
public enum ReportLevel {
INFO,
@@ -33,6 +39,36 @@ public class Status {
this.reports.add(r);
}
public void diagnoseHardwareCTRE(String deviceName, TalonFX motor) {
if (motor.isAlive()) addReport(ReportLevel.INFO, deviceName + " Motor (TalonFX) Alive?: Alive.");
else addReport(ReportLevel.ERROR, deviceName + " Motor (TalonFX) Alive?: Dead!");
}
public void diagnoseHardwareCTRE(String deviceName, CANcoder coder) {
// Because the Cancoder has no method to check its alive, we send it a empty control which it should return a zero when it gets the control.
// If its not zero, that means that most likely that it had some communication error, I.e. It actually is powered off or not connected at all.
// TODO: validate that a CANCoder can actually do `EmptyControl`s
StatusCode status = coder.setControl(new EmptyControl());
if (status.value == 0) addReport(ReportLevel.INFO, deviceName + " Cancoder Alive?: Alive.");
else addReport(ReportLevel.ERROR, deviceName + " Cancoder Alive?: Dead!");
// StatusSignal<MagnetHealthValue> -> MagnetHealthValue -> int
int coderMagHealth = coder.getMagnetHealth().getValue().value;
if (coderMagHealth == 3) addReport(ReportLevel.INFO, deviceName + " Cancoder Magnet Strength?: Ideal."); // why is 3 the 'good value'?
if (coderMagHealth == 2) addReport(ReportLevel.WARNING, deviceName + " Cancoder Magnet Strength?: Subpar.");
if (coderMagHealth == 1) addReport(ReportLevel.ERROR, deviceName + " Cancoder Magnet Strength?: Too Close or Far!");
if (coderMagHealth == 0) addReport(ReportLevel.ERROR, deviceName + " Cancoder Magnet Strength?: Unkown!");
}
public void diagnoseHardwareCTRE(String deviceName, Pigeon2 pigeon) {
// Because the Pigeon has no method to check its alive, we send it a empty control which it should return a zero when it gets the control.
// If its not zero, that means that most likely that it had some communication error, I.e. It actually is powered off or not connected at all.
// TODO: validate that a Pigeon2 can actually do `EmptyControl`s
StatusCode status = pigeon.setControl(new EmptyControl());
if (status.value == 0) addReport(ReportLevel.INFO, deviceName + " Pigeon2 Alive?: Alive.");
else addReport(ReportLevel.ERROR, deviceName + " Pigeon2 Alive?: Dead!");
}
public boolean hasReport() {
return reports.size() == 0;
}