diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 31eb0db7..b16ab4e6 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotContainer; import frc.robot.RobotMap; @@ -30,6 +30,10 @@ public class Climber extends SubsystemBase { // ok private SN_DoubleSolenoid climberHookPiston; + private ShuffleboardTab tab; + private int shuffleboardWidgetWidth; + private int shuffleboardWidgetHeight; + public Climber() { climberBottomSafetySwitch = new DigitalInput(ClimberMap.BOTTOM_SAFETY_MAG_SWITCH_DIO); @@ -151,16 +155,28 @@ public boolean isClimberClosedLoopErrorAcceptable() { @Override public void periodic() { // This method will be called once per scheduler run - // SmartDashboard.putNumber("Climber Encoder Counts", getClimberEncoderCount()); - // SmartDashboard.putNumber("Climber Closed Loop Error", - // getClimberClosedLoopError()); - SmartDashboard.putBoolean("Is Climber At Bottom", isClimberAtBottom()); - SmartDashboard.putBoolean("Is Climber Locked", isClimberLocked()); - SmartDashboard.putBoolean("Is Climber Angled", isClimberAngled()); - SmartDashboard.putBoolean("Is Climber Hooked", isHookDeployed()); - // SmartDashboard.putBoolean("Is Climber Error Acceptable", - // isClimberClosedLoopErrorAcceptable()); - // SmartDashboard.putNumber("Climber Motor Speed", - // climbMotor.getMotorOutputPercent()); + tab.add("Climber Encoder Counts", getClimberEncoderCount()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + + tab.add("Climber Closed Loop Error", getClimberClosedLoopError()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + + tab.add("Is Climber At Bottom", isClimberAtBottom()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + + tab.add("Is Climber Locked", isClimberLocked()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + + tab.add("Is Climber Angled", isClimberAngled()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + + tab.add("Is Climber Hooked", isHookDeployed()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + + tab.add("Is Climber Error Acceptable", isClimberClosedLoopErrorAcceptable()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + + tab.add("Climber Motor Speed", climbMotor.getMotorOutputPercent()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 40298cf0..092897ca 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -14,7 +14,10 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX; import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotContainer; import frc.robot.RobotPreferences; @@ -31,6 +34,18 @@ public class Drivetrain extends SubsystemBase { private TalonFXConfiguration config; + // For Shuffleboard + private ShuffleboardTab tab; + private ShuffleboardLayout encoderCountsLayout; + private ShuffleboardLayout motorPercentOutputLayout; + private ShuffleboardLayout feetDrivenLayout; + private ShuffleboardLayout feetPerSecondLayout; + private ShuffleboardLayout encoderCountsPer100msLayout; + private ShuffleboardLayout closedLoopErrorLayout; + + private int shuffleboardWidgetWidth; + private int shuffleboardWidgetHeight; + public SlewRateLimiter posSlewRateLimiter; public SlewRateLimiter negSlewRateLimiter; @@ -46,6 +61,22 @@ public Drivetrain() { posSlewRateLimiter = new SlewRateLimiter(DrivetrainPrefs.drivePosSlewRateLimit.getValue()); negSlewRateLimiter = new SlewRateLimiter(DrivetrainPrefs.driveNegSlewRateLimit.getValue()); + tab = Shuffleboard.getTab("Drivetrain"); + motorPercentOutputLayout = tab.getLayout("Motor Percent Output", BuiltInLayouts.kList).withSize(4, 4) + .withPosition(0, 0); + encoderCountsLayout = tab.getLayout("Encoder Counts", BuiltInLayouts.kList).withSize(3, 3).withPosition(4, 0); + encoderCountsPer100msLayout = tab.getLayout("Encoder Counts per 100ms", BuiltInLayouts.kList).withSize(3, 3) + .withPosition(7, 0); + closedLoopErrorLayout = tab.getLayout("Closed Loop Error", BuiltInLayouts.kList).withSize(4, 3).withPosition(10, 0); + feetPerSecondLayout = tab.getLayout("Feet Per Second", BuiltInLayouts.kList).withSize(3, 3).withPosition(14, 0); + feetDrivenLayout = tab.getLayout("Feet Driven", BuiltInLayouts.kList).withSize(3, 3).withPosition(17, 0); + + // Shuffleboard Layout Width + shuffleboardWidgetWidth = 2; + + // Shuffleboard Layout Height + shuffleboardWidgetHeight = 1; + configure(); } @@ -243,53 +274,65 @@ public void setCoastMode() { public void periodic() { // This method will be called once per scheduler run - // // Encoder Counts - // SmartDashboard.putNumber("Drivetrain Left Encoder", getLeftEncoderCount()); - // SmartDashboard.putNumber("Drivetrain Right Encoder", getRightEncoderCount()); - // SmartDashboard.putNumber("Drivetrain Average Encoder", - // getAverageEncoderCount()); - - // // Motion Profile - // SmartDashboard.putBoolean("Is Drivetrain Motion Profile Finished", - // isMotionProfileFinished()); - - // // Motor Percent Output - // SmartDashboard.putNumber("Drivetrain Left Lead Motor Speed", - // leftLeadMotor.getMotorOutputPercent()); - // SmartDashboard.putNumber("Drivetrain Right Lead Motor Speed", - // rightLeadMotor.getMotorOutputPercent()); - // SmartDashboard.putNumber("Drivetrain Left Follow Motor Speed", - // leftFollowMotor.getMotorOutputPercent()); - // SmartDashboard.putNumber("Drivetrain Right Follow Motor Speed", - // rightFollowMotor.getMotorOutputPercent()); - - // // Feet Driven - // SmartDashboard.putNumber("Drivetrain Left Feet Driven", getLeftFeetDriven()); - // SmartDashboard.putNumber("Drivetrain Right Feet Driven", - // getRightFeetDriven()); - // SmartDashboard.putNumber("Drivetrain Average Feet Driven", - // getAverageFeetDriven()); - - // // Feet Per Second - // SmartDashboard.putNumber("Drivetrain Left Feet Per Second", - // getLeftFeetPerSecond()); - // SmartDashboard.putNumber("Drivetrain Right Feet Per Second", - // getRightFeetPerSecond()); - // SmartDashboard.putNumber("Drivetrain Average Feet Per Second", - // getAverageFeetPerSecond()); - - // // Encoder Counts per 100ms - // SmartDashboard.putNumber("Drivetrain Left Velocity", getLeftVelocity()); - // SmartDashboard.putNumber("Drivetrain Right Velocity", getRightVelocity()); - // SmartDashboard.putNumber("Drivetrain Average Velocity", - // getAverageVelocity()); - - // // Closed Loop Error - // SmartDashboard.putNumber("Drivetrain Left Closed Loop Error Inches", - // getLeftClosedLoopErrorInches()); - // SmartDashboard.putNumber("Drivetrain Right Closed Loop Error Inches", - // getRightClosedLoopErrorInches()); - // SmartDashboard.putNumber("Drivetrain Average Closed Loop Error Inches", - // getAverageClosedLoopErrorInches()); + // { + // Shuffleboard Layouts + // Encoder Counts + encoderCountsLayout.add("Drivetrain Left Encoder", getLeftEncoderCount()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + encoderCountsLayout.add("Drivetrain Right Encoder", getRightEncoderCount()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + encoderCountsLayout.add("Drivetrain Average Encoder", getAverageEncoderCount()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + + // Motion Profile + tab.add("Is Drivetrain Motion Profile Finished", isMotionProfileFinished()).withSize(4, 2).withPosition(0, 4); + + // Motor Percent Output + motorPercentOutputLayout.add("Drivetrain Left Lead Motor Speed", leftLeadMotor.getMotorOutputPercent()).withSize( + shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + motorPercentOutputLayout.add("Drivetrain Right Lead Motor Speed", rightLeadMotor.getMotorOutputPercent()) + .withSize(shuffleboardWidgetWidth, shuffleboardWidgetHeight); + motorPercentOutputLayout.add("Drivetrain Left Follow Motor Speed", leftFollowMotor.getMotorOutputPercent()) + .withSize(shuffleboardWidgetWidth, shuffleboardWidgetHeight); + motorPercentOutputLayout.add("Drivetrain Right Follow Motor Speed", rightFollowMotor.getMotorOutputPercent()) + .withSize(shuffleboardWidgetWidth, shuffleboardWidgetHeight); + + // Feet Driven + feetDrivenLayout.add("Drivetrain Left Feet Driven", getLeftFeetDriven()).withSize(3, + 4); + feetDrivenLayout.add("Drivetrain Right Feet Driven", getRightFeetDriven()).withSize(3, 4); + feetDrivenLayout.add("Drivetrain Average Feet Driven", getAverageFeetDriven()).withSize(3, 4); + + // Feet Per Second + feetPerSecondLayout.add("Drivetrain Left Feet Per Second", getLeftFeetPerSecond()).withSize( + shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + feetPerSecondLayout.add("Drivetrain Right Feet Per Second", getRightFeetPerSecond()).withSize( + shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + feetPerSecondLayout.add("Drivetrain Average Feet Per Second", getAverageFeetPerSecond()).withSize( + shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + + // Encoder Counts per 100ms + encoderCountsPer100msLayout.add("Drivetrain Left Velocity", getLeftVelocity()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + encoderCountsPer100msLayout.add("Drivetrain Right Velocity", getRightVelocity()).withSize( + shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + encoderCountsPer100msLayout.add("Drivetrain Average Velocity", getAverageVelocity()).withSize( + shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + + // Closed Loop Error + closedLoopErrorLayout.add("Drivetrain Left Closed Loop Error Inches", getLeftClosedLoopErrorInches()) + .withSize(3, shuffleboardWidgetHeight); + closedLoopErrorLayout.add("Drivetrain Right Closed Loop Error Inches", getRightClosedLoopErrorInches()) + .withSize(3, shuffleboardWidgetHeight); + closedLoopErrorLayout.add("Drivetrain Average Closed Loop Error Inches", getAverageClosedLoopErrorInches()) + .withSize(3, shuffleboardWidgetHeight); + + // End Shuffleboard Layout } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java index ca484e40..60fc7252 100644 --- a/src/main/java/frc/robot/subsystems/Hood.java +++ b/src/main/java/frc/robot/subsystems/Hood.java @@ -7,7 +7,8 @@ import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotMap.*; @@ -19,12 +20,21 @@ public class Hood extends SubsystemBase { private DoubleSolenoid.Value shallowAngleHoodValue = Value.kReverse; private DoubleSolenoid.Value steepAngleHoodValue = Value.kForward; + private ShuffleboardTab tab; + private int shuffleboardWidgetWidth; + private int shuffleboardWidgetHeight; + // Initializes Hood Variables public Hood() { angleHoodSolenoid = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, HoodMap.HOOD_SOLENOID_STEEP_ANGLE_PCM_A, HoodMap.HOOD_SOLENOID_SHALLOW_ANGLE_PCM_B); + + tab = Shuffleboard.getTab("Hood"); + shuffleboardWidgetHeight = 1; + shuffleboardWidgetWidth = 2; // configure is not needed since this is a solenoid + } // Method checks if solenoid is extended @@ -56,6 +66,6 @@ public void shallowHood() { @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putBoolean("Hood Solenoid", isHoodSteep()); + tab.add("Hood Solenoid", isHoodSteep()).withSize(shuffleboardWidgetHeight, shuffleboardWidgetWidth); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index e8fd7bb7..909ee4b5 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -8,13 +8,14 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.frcteam3255.components.SN_DoubleSolenoid; import com.frcteam3255.preferences.SN_DoublePreference; -// import com.revrobotics.ColorSensorV3; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +// import com.revrobotics.ColorSensorV3; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; // import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotMap; import frc.robot.RobotMap.*; @@ -25,8 +26,13 @@ public class Intake extends SubsystemBase { // Creates the Intake motors private TalonFX intakeMotor; private SN_DoubleSolenoid intakePiston; - // private ColorSensorV3 intakeColorSensorV3; - // private final I2C.Port i2cPort = I2C.Port.kMXP; + + // Shuffleboard + private ShuffleboardTab tab; + private ShuffleboardLayout intakeComponentsLayout; + + private int shuffleboardWidgetWidth; + private int shuffleboardWidgetHeight; // Initializes Intake Variables public Intake() { @@ -34,8 +40,12 @@ public Intake() { intakePiston = new SN_DoubleSolenoid(RobotMap.PRIMARY_PCM, PneumaticsModuleType.CTREPCM, IntakeMap.INTAKE_SOLENOID_PCM_A, IntakeMap.INTAKE_SOLENOID_PCM_B); - // intakeColorSensorV3 = new ColorSensorV3(i2cPort); + tab = Shuffleboard.getTab("Intake"); + intakeComponentsLayout = tab.getLayout("Intake Components", BuiltInLayouts.kList).withSize(2, 2); + + shuffleboardWidgetWidth = 2; + shuffleboardWidgetHeight = 1; configure(); } @@ -79,90 +89,18 @@ public void retractIntake() { intakePiston.setRetracted(); } - // Returns Ball color in Intake - // public int getRed() { - // return intakeColorSensorV3.getRed(); - // } - - // Returns Ball color in Intake - // public int getBlue() { - // return intakeColorSensorV3.getBlue(); - // } - - // Returns Proximity of Ball in Intake - // public int getProximity() { - // return intakeColorSensorV3.getProximity(); - // } - - public static enum ballColor { - red, blue, none; - } - - // public ballColor getBallColor() { - // if (isBallNearIntake()) { - // if (getBlue() > getRed()) { - // return ballColor.blue; - // } else { - // return ballColor.red; - // } - // } else { - // return ballColor.none; - // } - // } - - // Gets Ball Color/Status - // public String ballColorString() { - // if (getBallColor() == ballColor.blue) { - // return "blue"; - // } else if (getBallColor() == ballColor.red) { - // return "red"; - // } else if (getBallColor() == ballColor.none) { - // return "none"; - // } else { - // return "Unknown Color"; - // } - // } - - // public boolean isBallNearIntake() { - // return getProximity() > IntakePrefs.colorSensorMinProximity.getValue(); - // } - - // public boolean isBallBlue() { - // return getBallColor() == ballColor.blue; - // } - - // public boolean isBallRed() { - // return getBallColor() == ballColor.red; - // } - - public boolean isAllianceBlue() { - return DriverStation.getAlliance() == Alliance.Blue; - } - - public boolean isAllianceRed() { - return DriverStation.getAlliance() == Alliance.Red; - } - - // public boolean ballColorMatchesAlliance() { - // return (isBallBlue() && isAllianceBlue()) || (isBallRed() && - // isAllianceRed()); - // } - @Override public void periodic() { // This method will be called once per scheduler run - // SmartDashboard.putNumber("Intake Motor", getIntakeMotorCount()); - // SmartDashboard.putNumber("IntakeMotorOutputPercent", - // intakeMotor.getMotorOutputPercent()); - SmartDashboard.putBoolean("Intake Solenoid", isIntakeDeployed()); - - // SmartDashboard.putNumber("Color Sensor Blue", getBlue()); - // SmartDashboard.putNumber("Color Sensor Red", getRed()); - // SmartDashboard.putNumber("Color Sensor Prox", getProximity()); - // SmartDashboard.putBoolean("Is Ball Blue", isBallBlue()); - SmartDashboard.putBoolean("Is Alliance Blue", isAllianceBlue()); - // SmartDashboard.putBoolean("Ball Color Matches Alliance", - // ballColorMatchesAlliance()); - // SmartDashboard.putBoolean("Is Proximity", isBallNearIntake()); + + // Intake Components + intakeComponentsLayout.add("Intake Motor", getIntakeMotorCount()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + intakeComponentsLayout.add("IntakeMotorOutputPercent", intakeMotor.getMotorOutputPercent()).withSize( + shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + intakeComponentsLayout.add("Intake Solenoid", isIntakeDeployed()).withSize(shuffleboardWidgetWidth, + shuffleboardWidgetHeight); + } } \ No newline at end of file