diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 54b0e14..bebbcb9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.IntakeCommandGroup; +import frc.robot.commands.IntakeIndex; import frc.robot.commands.IntakeRevCommandGroup; import frc.robot.commands.RevAndShootCommand; import frc.robot.commands.RunShooter; @@ -25,9 +26,6 @@ import frc.robot.commands.SetIndex; import frc.robot.commands.SetWinch; import frc.robot.commands.ShootClose; -import frc.robot.commands.ShooterIndex; -import frc.robot.experimental.IntakeIndex; -import frc.robot.experimental.StopShooting; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.IndexSubsystem; @@ -101,10 +99,10 @@ public RobotContainer() { * Unintended side effect is this will create EVERY auton file from the RIO deploy folder. * FTP into the the RIO to delete old auton options */ autoChooser = AutoBuilder.buildAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); + // SmartDashboard.putData("Auto Chooser", autoChooser); + Shuffleboard.getTab("Auton").add("Auto Chooser", autoChooser); shooter = new ShooterSubsystemVelocity(); - index.setDefaultCommand(index.run(() -> index.SetPower(BumperStatus(1)))); /* Configure the Button Bindings */ configureBindings(); @@ -163,7 +161,7 @@ private void configureBindings() { }; - /* Use for Debugging purposes */ + /* Use for Debugging and diagnostics purposes */ public void DebugMethodSingle() { // #region Driving @@ -172,31 +170,16 @@ public void DebugMethodSingle() { // Less useful logs that we still need to see for testing. - // var driverDiagnostics = Shuffleboard.getTab("Driver Diagnostics"); - var driverDiagnostics = Shuffleboard.getTab("Driver Diagnostics"); - - driverDiagnostics.add(autoChooser); - - driverDiagnostics.addBoolean("Note Detected", () -> index.HasCargo()); + var driverDiagnostics = Shuffleboard.getTab("Diagnostics"); + // driverDiagnostics.addDouble("Arm Rot", () -> arm.GetArmPos().getValueAsDouble()); // driverDiagnostics.addDouble("Arm Rot Deg", () -> arm.GetPositionDegrees()); // arm.showArmTelemetry("Driver Diagnostics"); + // Shuffleboard.getTab("Arm").add("Arm Output", arm); // #endregion Testing } - /** - * Schizophrenic method to quickly get an int indicating driver bumper status. 1 - * if RB,-1 if LB, 0 if both or none - * - * @param port controller port, 0 is driver, 1 is operaator - * @return an int indicating driver bumper status. - */ - private double BumperStatus(int port) { - return (Controllers[port].rightBumper().getAsBoolean() ? 1 : 0) - + (Controllers[port].leftBumper().getAsBoolean() ? -1 : 0); - } - public Command getAutonomousCommand() { return autoChooser.getSelected(); } diff --git a/src/main/java/frc/robot/experimental/IntakeIndex.java b/src/main/java/frc/robot/commands/IntakeIndex.java similarity index 87% rename from src/main/java/frc/robot/experimental/IntakeIndex.java rename to src/main/java/frc/robot/commands/IntakeIndex.java index ec0d067..2c333da 100644 --- a/src/main/java/frc/robot/experimental/IntakeIndex.java +++ b/src/main/java/frc/robot/commands/IntakeIndex.java @@ -1,10 +1,8 @@ -package frc.robot.experimental; +package frc.robot.commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants; -import frc.robot.commands.SetIndex; -import frc.robot.commands.SetIntake; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.IntakeSubsystem; diff --git a/src/main/java/frc/robot/commands/IntakeRevCommandGroup.java b/src/main/java/frc/robot/commands/IntakeRevCommandGroup.java index 1b08cff..f4f11f4 100644 --- a/src/main/java/frc/robot/commands/IntakeRevCommandGroup.java +++ b/src/main/java/frc/robot/commands/IntakeRevCommandGroup.java @@ -9,7 +9,7 @@ public class IntakeRevCommandGroup extends SequentialCommandGroup { // LED to unloaded color // Intake + Index in parallel. // Note triggers the note sensor (ToF), turn off both intake and index, - // Set LED status to loaded (and shuffleboard) + // Set LED status to loaded and to dashboard SetIntake runIntake; IndexSubsystem index; diff --git a/src/main/java/frc/robot/commands/RevAndShootCommand.java b/src/main/java/frc/robot/commands/RevAndShootCommand.java index a99ee73..1e0f1f5 100644 --- a/src/main/java/frc/robot/commands/RevAndShootCommand.java +++ b/src/main/java/frc/robot/commands/RevAndShootCommand.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.Constants; -import frc.robot.experimental.IncrementIndex; +// import frc.robot.experimental.IncrementIndex; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.ShooterSubsystemVelocity; @@ -23,14 +23,14 @@ public class RevAndShootCommand extends SequentialCommandGroup { IndexSubsystem indexer; ShooterSubsystemVelocity shooter; // component Commands - private IncrementIndex outtaMyWay; + // private IncrementIndex outtaMyWay; private Command revUpShooter; private Command indexCommand; public RevAndShootCommand(IndexSubsystem indexer) { // Set up our subsystems this.indexer = indexer; - outtaMyWay = new IncrementIndex(indexer); + // outtaMyWay = new IncrementIndex(indexer); } diff --git a/src/main/java/frc/robot/commands/RevAndShootEndsCommand.java b/src/main/java/frc/robot/commands/RevAndShootEndsCommand.java index 3bfe295..a057315 100644 --- a/src/main/java/frc/robot/commands/RevAndShootEndsCommand.java +++ b/src/main/java/frc/robot/commands/RevAndShootEndsCommand.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.Constants; -import frc.robot.experimental.IncrementIndex; +// import frc.robot.experimental.IncrementIndex; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.ShooterSubsystemVelocity; @@ -23,14 +23,14 @@ public class RevAndShootEndsCommand extends SequentialCommandGroup { IndexSubsystem indexer; ShooterSubsystemVelocity shooter; // component Commands - private IncrementIndex outtaMyWay; + // private IncrementIndex outtaMyWay; private Command revUpShooter; private Command indexCommand; public RevAndShootEndsCommand(IndexSubsystem indexer) { // Set up our subsystems this.indexer = indexer; - outtaMyWay = new IncrementIndex(indexer); + // outtaMyWay = new IncrementIndex(indexer); } public RevAndShootEndsCommand(IndexSubsystem indexer, ShooterSubsystemVelocity shooter) { diff --git a/src/main/java/frc/robot/experimental/ArmPositionAndShoot.java b/src/main/java/frc/robot/experimental/ArmPositionAndShoot.java.txt similarity index 100% rename from src/main/java/frc/robot/experimental/ArmPositionAndShoot.java rename to src/main/java/frc/robot/experimental/ArmPositionAndShoot.java.txt diff --git a/src/main/java/frc/robot/experimental/IncrementIndex.java b/src/main/java/frc/robot/experimental/IncrementIndex.java.txt similarity index 100% rename from src/main/java/frc/robot/experimental/IncrementIndex.java rename to src/main/java/frc/robot/experimental/IncrementIndex.java.txt diff --git a/src/main/java/frc/robot/commands/IntakeCommandGroupOff.java b/src/main/java/frc/robot/experimental/IntakeCommandGroupOff.java.txt similarity index 90% rename from src/main/java/frc/robot/commands/IntakeCommandGroupOff.java rename to src/main/java/frc/robot/experimental/IntakeCommandGroupOff.java.txt index 091e9b0..0c89b05 100644 --- a/src/main/java/frc/robot/commands/IntakeCommandGroupOff.java +++ b/src/main/java/frc/robot/experimental/IntakeCommandGroupOff.java.txt @@ -1,7 +1,9 @@ -package frc.robot.commands; +package frc.robot.experimental; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.SetIndex; +import frc.robot.commands.SetIntake; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.IntakeSubsystem; diff --git a/src/main/java/frc/robot/experimental/KillShooterCommand.java b/src/main/java/frc/robot/experimental/KillShooterCommand.java.txt similarity index 100% rename from src/main/java/frc/robot/experimental/KillShooterCommand.java rename to src/main/java/frc/robot/experimental/KillShooterCommand.java.txt diff --git a/src/main/java/frc/robot/subsystems/MagLimitSwitch.java b/src/main/java/frc/robot/experimental/MagLimitSwitch.java.txt similarity index 81% rename from src/main/java/frc/robot/subsystems/MagLimitSwitch.java rename to src/main/java/frc/robot/experimental/MagLimitSwitch.java.txt index a250e12..765b1a5 100644 --- a/src/main/java/frc/robot/subsystems/MagLimitSwitch.java +++ b/src/main/java/frc/robot/experimental/MagLimitSwitch.java.txt @@ -1,6 +1,7 @@ -package frc.robot.subsystems; +package frc.robot.experimental; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -22,7 +23,8 @@ public void magSwitch() { } @Override public void periodic(){ - SmartDashboard.putBoolean("Mag Limit Switch", getSwitchState()); + // Shuffleboard.getTab("Sensors").add("Mag Limit Switch", getSwitchState()); + } diff --git a/src/main/java/frc/robot/experimental/SetShooter.java b/src/main/java/frc/robot/experimental/SetShooter.java.txt similarity index 100% rename from src/main/java/frc/robot/experimental/SetShooter.java rename to src/main/java/frc/robot/experimental/SetShooter.java.txt diff --git a/src/main/java/frc/robot/commands/ShooterIndex.java b/src/main/java/frc/robot/experimental/ShooterIndex.java.txt similarity index 100% rename from src/main/java/frc/robot/commands/ShooterIndex.java rename to src/main/java/frc/robot/experimental/ShooterIndex.java.txt diff --git a/src/main/java/frc/robot/experimental/StopIntakeIndex.java b/src/main/java/frc/robot/experimental/StopIntakeIndex.java.txt similarity index 100% rename from src/main/java/frc/robot/experimental/StopIntakeIndex.java rename to src/main/java/frc/robot/experimental/StopIntakeIndex.java.txt diff --git a/src/main/java/frc/robot/experimental/StopShooting.java b/src/main/java/frc/robot/experimental/StopShooting.java.txt similarity index 100% rename from src/main/java/frc/robot/experimental/StopShooting.java rename to src/main/java/frc/robot/experimental/StopShooting.java.txt diff --git a/src/main/java/frc/robot/experimental/TunerConstantsORIGINAL.java b/src/main/java/frc/robot/experimental/TunerConstantsORIGINAL.java.txt similarity index 100% rename from src/main/java/frc/robot/experimental/TunerConstantsORIGINAL.java rename to src/main/java/frc/robot/experimental/TunerConstantsORIGINAL.java.txt diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 3545ab3..e7889df 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -14,7 +14,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.GravityTypeValue; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -102,10 +102,11 @@ public ArmSubsystem() { m_arm.getConfigurator().apply(armCurrent0, 0.050); /* - * Send info about the arm to the Smart Dashboard + * Send info about the arm to the Shuffleboard * Defaults to percent output + * Only needed for diagnostics */ - // SmartDashboard.putData("Arm Output", m_arm); + // Shuffleboard.getTab("Arm").add("Arm Output", m_arm); } /* End of the class-method */ @@ -115,14 +116,12 @@ public StatusSignal getArmPos() { } public void setArmPose(double armPose) { + m_arm.setControl(m_armPose.withPosition(armPose)); + + // less effecient approach, shouldn't use this + // m_arm.setControl(new MotionMagicVoltage(armPose)); - m_arm.setControl( - - new MotionMagicVoltage(armPose)); - /* - SmartDashboard.putNumber("Arm Position", m_arm.getPosition().getValue()); - SmartDashboard.putNumber("Arm Stator", m_arm.getStatorCurrent().getValue());*/ - showArmTelemetry(); + // showArmTelemetry(); } @@ -131,11 +130,13 @@ public void setArmPose(double armPose) { // PositionVoltage armPose = new PositionVoltage(armPose); public void setArmForClimb(double power) { m_arm.setControl(new VoltageOut (power)); - showArmTelemetry(); + // showArmTelemetry(); } + /* I think this is all unused code */ + /* double targetPosition = 10; public void moveArmToPositionAndStop(double targetPosition) { @@ -149,24 +150,39 @@ public void moveArmToPositionAndStop(double targetPosition) { // Stop the arm m_arm.setControl(new PositionDutyCycle(0)); -} + } + */ /* * Currently only being called in subsystem-command; * inspite of my efforts it only appears once it's triggered at least once */ + /* public void showArmTelemetry() { - SmartDashboard.putNumber("Arm Position", m_arm.getPosition().getValue()); - SmartDashboard.putNumber("Arm Stator", m_arm.getStatorCurrent().getValue()); - SmartDashboard.putNumber("Arm Supply", m_arm.getSupplyCurrent().getValue()); - SmartDashboard.putNumber("Arm Voltage", m_arm.getMotorVoltage().getValue()); + + Shuffleboard.getTab("Arm").add("Arm Position", m_arm.getPosition().getValue()); + Shuffleboard.getTab("Arm").add("Arm Stator", m_arm.getStatorCurrent().getValue()); + Shuffleboard.getTab("Arm").add("Arm Supply", m_arm.getSupplyCurrent().getValue()); + Shuffleboard.getTab("Arm").add("Arm Voltage", m_arm.getMotorVoltage().getValue()); } + */ public void stopArm(double power) { m_arm.setControl(new VoltageOut(0)); } + @Override + public void periodic() { + /* Only needed for diagnostics */ + /* + Shuffleboard.getTab("Arm").add("Arm Position", m_arm.getPosition().getValue()); + Shuffleboard.getTab("Arm").add("Arm Stator", m_arm.getStatorCurrent().getValue()); + Shuffleboard.getTab("Arm").add("Arm Supply", m_arm.getSupplyCurrent().getValue()); + Shuffleboard.getTab("Arm").add("Arm Voltage", m_arm.getMotorVoltage().getValue()); + */ + } + } // end of ArmSubsystem method /*************************** diff --git a/src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java b/src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java index d0f63f3..65bf35f 100644 --- a/src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java @@ -1,22 +1,21 @@ package frc.robot.subsystems; -/* Subsystem class to primarily use a Time of Flight sensor from 'Playing with Fusion'. - * It will read the distance from the sensor to the 'note' and determine if the note is in a load position. - * It should return an actual distance reading to the SmartDashboard and a boolean for a method 'isNoteLoaded' - * This sensor data will change LED status and enable/disable intake and index motors - */ - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - import java.util.Optional; import com.playingwithfusion.TimeOfFlight; import com.playingwithfusion.TimeOfFlight.RangingMode; -import edu.wpi.first.hal.AllianceStationID; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; + +/* Subsystem class to primarily use a Time of Flight sensor from 'Playing with Fusion'. + * It will read the distance from the sensor to the 'note' and determine if the note is in a load position. + * It should return an actual distance reading to and a boolean for a method 'isNoteLoaded' + * This sensor data will change LED status and enable/disable intake and index motors + */ + +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; public class NoteSensorSubsystem extends SubsystemBase { @@ -40,7 +39,7 @@ public class NoteSensorSubsystem extends SubsystemBase { /* Constructor */ public NoteSensorSubsystem() { /* - * Initialize the sensor, and '.setRangingMode(RangingMode.Short)' for this + * Initialize the sensor, and '.setRangingMode(RangingMode.Short)' foDr this * usage. * * | Sample value | Time | @@ -50,8 +49,9 @@ public NoteSensorSubsystem() { * | 3 | 50 ms | * | 4 (default) | 100 ms | * | 5 | 200 ms | + *****************************/ - noteDistance.setRangingMode(RangingMode.Short, 1); + noteDistance.setRangingMode(RangingMode.Short, 4); } public double getNoteDistance() { @@ -98,8 +98,12 @@ public void periodic() { // ! Can be achieved with Shuffleboard call in // ! Constructor, per setLEDColor(); - SmartDashboard.putNumber("Note Distance", noteDistance.getRange()); - SmartDashboard.putBoolean("Note Loaded", isNoteLoaded()); + + // Only needed for diagnostics + // Shuffleboard.getTab("Note").add("Note Distance", noteDistance.getRange()); + // FIXME: This is not working as expected. Code crashes saying .add title is already in use, probably because it's being called periodically. + // Shuffleboard.getTab("Sensors").add("Note Loaded 2", isNoteLoaded()); + } } // end of class NoteSensorSubsystem \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystemVelocity.java b/src/main/java/frc/robot/subsystems/ShooterSubsystemVelocity.java index 5d5fb2d..898d74c 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystemVelocity.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystemVelocity.java @@ -10,7 +10,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -148,8 +148,8 @@ public boolean isAtSetpoint() { @Override public void periodic() { // System.out.println(m_main.getVelocity()); - SmartDashboard.putNumber("Velocity", GetVelocity()); - SmartDashboard.putBoolean("Velocity Achieved", isAtSetpoint()); + // Shuffleboard.getTab("Shooter").add("Velocity", GetVelocity()); + // Shuffleboard.getTab("Shooter").add("Velocity Achieved", isAtSetpoint()); } }