diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index f3c2cdce..55a14898 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team2412.robot.Robot; import java.util.Map; public class IntakeSubsystem extends SubsystemBase { @@ -48,83 +49,17 @@ public class IntakeSubsystem extends SubsystemBase { private final DigitalInput feederSensor; // Shuffleboard - // speed - private final GenericEntry setIntakeInSpeedEntry = - Shuffleboard.getTab("Intake") - .addPersistent("Intake in speed - ", INTAKE_IN_SPEED) - .withSize(2, 1) - .withProperties(Map.of("Min", -1, "Max", 1)) - .getEntry(); - - private final GenericEntry setIndexInSpeedEntry = - Shuffleboard.getTab("Intake") - .add("Index in speed - ", INDEX_UPPER_IN_SPEED) - .withSize(1, 1) - .getEntry(); - - private final GenericEntry setFeederInSpeedEntry = - Shuffleboard.getTab("Intake") - .add("Feeder in speed - ", FEEDER_IN_SPEED) - .withSize(1, 1) - .getEntry(); - - // temperature - private final GenericEntry intakeMotorFrontTemp = - Shuffleboard.getTab("Intake") - .add("Front Intake temp", 0) - .withSize(1, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); - - private final GenericEntry intakeMotorBackTemp = - Shuffleboard.getTab("Intake") - .add("Back Intake temp", 0) - .withSize(1, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); - - private final GenericEntry intakeMotorLeftTemp = - Shuffleboard.getTab("Intake") - .add("Left Intake temp", 0) - .withSize(1, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); - private final GenericEntry intakeMotorRightTemp = - Shuffleboard.getTab("Intake") - .add("Right Intake temp", 0) - .withSize(1, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); - - private final GenericEntry indexMotorUpperTemp = - Shuffleboard.getTab("Intake") - .add("Upper Index temp", 0) - .withSize(1, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); + private final ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Intake"); + // speed + private GenericEntry setIntakeInSpeedEntry; - private final GenericEntry ingestMotorTemp = - Shuffleboard.getTab("Intake") - .add("Ingest temp", 0) - .withSize(1, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); + private GenericEntry setIndexInSpeedEntry; - private final GenericEntry feederMotorTemp = - Shuffleboard.getTab("Intake") - .add("Feeder temp", 0) - .withSize(1, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); + private GenericEntry setFeederInSpeedEntry; // sensor override - private final GenericEntry sensorOverride = - Shuffleboard.getTab("Intake") - .add("Override Sensors", false) - .withSize(1, 1) - .withWidget(BuiltInWidgets.kToggleSwitch) - .getEntry(); + private GenericEntry sensorOverride; public IntakeSubsystem() { intakeMotorFront = new CANSparkMax(INTAKE_MOTOR_FRONT, MotorType.kBrushless); @@ -142,9 +77,7 @@ public IntakeSubsystem() { resetMotors(); - ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Intake"); - shuffleboardTab.addBoolean("Index Sensor - ", this::indexSensorHasNote).withSize(1, 1); - shuffleboardTab.addBoolean("Feeder Sensor - ", this::feederSensorHasNote).withSize(1, 1); + initShuffleboard(); } private void configureMotor(CANSparkBase motor, int currentLimit, boolean invert) { @@ -245,17 +178,61 @@ public boolean getSensorOverride() { return sensorOverride.getBoolean(false); } - @Override - public void periodic() { - intakeMotorFrontTemp.setDouble(intakeMotorFront.getMotorTemperature()); - intakeMotorBackTemp.setDouble(intakeMotorBack.getMotorTemperature()); - intakeMotorRightTemp.setDouble(intakeMotorRight.getMotorTemperature()); - intakeMotorLeftTemp.setDouble(intakeMotorLeft.getMotorTemperature()); + // logging - ingestMotorTemp.setDouble(ingestMotor.getMotorTemperature()); + public void initShuffleboard() { + if (Robot.isDebugMode()) { + shuffleboardTab + .addDouble("Front Intake Motor Temp", () -> intakeMotorFront.getMotorTemperature()) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kTextView); + shuffleboardTab + .addDouble("Back Intake Motor Temp", () -> intakeMotorBack.getMotorTemperature()) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kTextView); + shuffleboardTab + .addDouble("Left Intake Motor Temp", () -> intakeMotorLeft.getMotorTemperature()) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kTextView); + shuffleboardTab + .addDouble("Right Intake Motor Temp", () -> intakeMotorRight.getMotorTemperature()) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kTextView); + shuffleboardTab + .addDouble("Ingest Motor Temp", () -> ingestMotor.getMotorTemperature()) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kTextView); + shuffleboardTab + .addDouble("Index Motor Temp", () -> indexMotorUpper.getMotorTemperature()) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kTextView); + shuffleboardTab + .addDouble("Feeder Motor Temp", () -> feederMotor.getMotorTemperature()) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kTextView); + } - indexMotorUpperTemp.setDouble(indexMotorUpper.getMotorTemperature()); + shuffleboardTab.addBoolean("Index Sensor - ", this::indexSensorHasNote).withSize(1, 1); + shuffleboardTab.addBoolean("Feeder Sensor - ", this::feederSensorHasNote).withSize(1, 1); - feederMotorTemp.setDouble(feederMotor.getMotorTemperature()); + setIntakeInSpeedEntry = + shuffleboardTab + .addPersistent("Intake in speed - ", INTAKE_IN_SPEED) + .withSize(2, 1) + .withProperties(Map.of("Min", -1, "Max", 1)) + .getEntry(); + + setIndexInSpeedEntry = + shuffleboardTab.add("Index in speed - ", INDEX_UPPER_IN_SPEED).withSize(1, 1).getEntry(); + + setFeederInSpeedEntry = + shuffleboardTab.add("Feeder in speed - ", FEEDER_IN_SPEED).withSize(1, 1).getEntry(); + + sensorOverride = + Shuffleboard.getTab("Intake") + .add("Override Sensors", false) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kToggleSwitch) + .getEntry(); } } diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 524568eb..398ea99f 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.team2412.robot.Hardware; +import frc.team2412.robot.Robot; import frc.team2412.robot.util.SparkPIDWidget; import java.util.Map; @@ -90,10 +91,6 @@ public class LauncherSubsystem extends SubsystemBase { private GenericEntry launcherAngleSpeedEntry; - private GenericEntry launcherTopFlywheelTemp; - - private GenericEntry launcherBottomFlyWheelTemp; - private GenericEntry launcherIsAtSpeed; private GenericEntry launcherAngleManual; @@ -269,13 +266,22 @@ public void manualSetpoint(double setpoint) { } private void initShuffleboard() { - launcherBottomFlyWheelTemp = - Shuffleboard.getTab("Launcher") - .add("bottom Flywheel temp", 0) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kTextView) - .withPosition(0, 3) - .getEntry(); + if (Robot.isDebugMode()) { + Shuffleboard.getTab("Launcher") + .add(new SparkPIDWidget(launcherAngleOnePIDController, "launcherAnglePID")) + .withPosition(2, 0); + Shuffleboard.getTab("Launcher") + .add(new SparkPIDWidget(launcherTopPIDController, "launcherTopPID")) + .withPosition(0, 0); + Shuffleboard.getTab("Launcher") + .add(new SparkPIDWidget(launcherBottomPIDController, "launcherBottomPID")) + .withPosition(1, 0); + + Shuffleboard.getTab("Launcher") + .addDouble("Bottom FlyWheel Temp", () -> launcherBottomMotor.getMotorTemperature()); + Shuffleboard.getTab("Launcher") + .addDouble("Top FlyWheel Temp", () -> launcherTopMotor.getMotorTemperature()); + } launcherIsAtSpeed = Shuffleboard.getTab("Launcher") @@ -284,13 +290,6 @@ private void initShuffleboard() { .withWidget(BuiltInWidgets.kBooleanBox) .withPosition(0, 2) .getEntry(); - launcherTopFlywheelTemp = - Shuffleboard.getTab("Launcher") - .add("top Flywheel temp", 0) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kTextView) - .withPosition(2, 3) - .getEntry(); launcherAngleSpeedEntry = Shuffleboard.getTab("Launcher") .add("Launcher angle Speed", 0) @@ -320,23 +319,13 @@ private void initShuffleboard() { .withProperties(Map.of("Min", -MAX_FREE_SPEED_RPM, "Max", MAX_FREE_SPEED_RPM)) .withPosition(5, 0) .getEntry(); + launcherAngleManual = Shuffleboard.getTab("Launcher") .add("Launcher manual increase", 0) .withSize(1, 1) .withWidget(BuiltInWidgets.kTextView) .getEntry(); - Shuffleboard.getTab("Launcher") - .add(new SparkPIDWidget(launcherAngleOnePIDController, "launcherAnglePID")) - .withPosition(2, 0); - // Shuffleboard.getTab("Launcher") - // .add(new SparkPIDWidget(launcherAngleTwoPIDController, "launcherAngleTwoPIDController")); - Shuffleboard.getTab("Launcher") - .add(new SparkPIDWidget(launcherTopPIDController, "launcherTopPID")) - .withPosition(0, 0); - Shuffleboard.getTab("Launcher") - .add(new SparkPIDWidget(launcherBottomPIDController, "launcherBottomPID")) - .withPosition(1, 0); } @Override @@ -344,8 +333,6 @@ public void periodic() { launcherAngleEntry.setDouble(getAngle()); launcherSpeedEntry.setDouble(getLauncherSpeed()); launcherAngleSpeedEntry.setDouble(getAngleSpeed()); - launcherTopFlywheelTemp.setDouble(launcherTopMotor.getMotorTemperature()); - launcherBottomFlyWheelTemp.setDouble(launcherTopMotor.getMotorTemperature()); launcherIsAtSpeed.setBoolean(isAtSpeed()); launcherAngleManual.setDouble(manualAngleSetpoint); }