reducing clutter, testing dashboard

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 21352cc94ad2716e1c012b35e1a546c9f4b7c37c..93fb78bc82718bcede281ba1a3314af4891fa8fc 100644 (file)
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -121,6 +121,7 @@ public class Robot extends LoggedRobot {
         CommandScheduler.getInstance().run();
 
         robotContainer.logComponents();
+        robotContainer.periodic();
     }
 
     /**

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d8b1922b36dfec0a27799ac85042d7464682d936..50743b448145a330c7870eece98fb7a30da62ae5 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -87,6 +87,8 @@ public class RobotContainer {
     // display the current robot id on smartdashboard
     SmartDashboard.putString("RobotID", robotId.toString());
 
+    SmartDashboard.putNumber("Match Time", 0.0);
+
     // Filling the SendableChooser on SmartDashboard
     // autoChooserInit();
 
@@ -307,4 +309,14 @@ public class RobotContainer {
         // Subsystem Pose3ds
         });
   }
+
+  /** Updates SmartDashboard values that need to be refreshed every loop */
+  public void periodic() {
+    // update match timer
+    double matchTime = DriverStation.getMatchTime();
+    if (matchTime > 0) {
+      SmartDashboard.putNumber("Match Time", matchTime);
+    }
+    SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString());
+  }
 }

diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
index ed873afce3861617149cfdc8edb381e501de8e06..727a983c16ab80a6506bf314b8107c9e0ed1ef75 100644 (file)
--- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
+++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
@@ -42,7 +42,6 @@ public class DefaultDriveCommand extends Command {
         trenchAssistPid.setIntegratorRange(-1, 1);
 
         SmartDashboard.putNumber("0 degrees snap location", 0);
-        SmartDashboard.putNumber("", 0);
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java
index f4bfdee22300249467f3dab37049bf8ed9da6d07..786a382b1aa4d56627c6eb11958571c6e5e6d6d4 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java
@@ -182,6 +182,9 @@ public class Intake extends SubsystemBase implements IntakeIO{
         Logger.recordOutput("Intake/Setpoint", setpointInches);
         robotExtension.setLength(inchExtension);
 
+        SmartDashboard.putNumber("Intake Extension (in)", inchExtension);
+        SmartDashboard.putBoolean("Intake Extended", inchExtension > 1.0);
+
         if(calibrating){
             leftMotor.set(-0.1);
             rightMotor.set(-0.1);

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index 7a8b54bb3118dffcce8f5e9f2a69956033b61465..cb77ac2879a0fc7285b5721193ff669a43632e13 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -24,6 +24,8 @@ import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
 import frc.robot.constants.FieldConstants;
@@ -111,6 +113,8 @@ public class Drivetrain extends SubsystemBase {
 
     private Rotation2d rawGyroRotation = new Rotation2d();
 
+    private final Field2d field = new Field2d();
+
     /**
      * Creates a new Swerve Style Drivetrain.
      */
@@ -171,6 +175,8 @@ public class Drivetrain extends SubsystemBase {
                 });
 
         // PPLibTelemetry.enableCompetitionMode();
+
+        SmartDashboard.putData("Field", field);
     }
 
     public void close() {
@@ -210,6 +216,8 @@ public class Drivetrain extends SubsystemBase {
         }
         Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses());
         updateOdometryVision();
+
+        field.setRobotPose(getPose());
     }
 
     // DRIVE

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 5e751e2e5409a74c54fe4d3c09b631d96f04c49d..230a38f5766b612b1af27ca998c25d13f31b7414 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -81,7 +81,13 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         Logger.recordOutput("Shooter/realVelocity", shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER);
         Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed);
 
-        SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOON" : "lost");
+        // SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOON" : "lost");
+        double actualWheelVelocity = shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER;
+        SmartDashboard.putNumber("Shooter Speed Error (mps)", shooterTargetSpeed - actualWheelVelocity);
+
+        boolean autoWon = HubActive.wonAuto();
+        SmartDashboard.putBoolean("Auto Won", autoWon);
+        SmartDashboard.putString("Auto Result", autoWon ? "WON" : "LOST");
     }
 
     /**

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 19d212dedbba57027bcb0de8f6c7c610c8f1554a..550f6c8c69ef1f11b1ce1d80539c88acabbf0fc6 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -107,6 +108,14 @@ public class Turret extends SubsystemBase implements TurretIO{
                SmartDashboard.putData("Start turret calibration", new InstantCommand(()-> calibrate()));
                SmartDashboard.putData("Stop turret calibration", new InstantCommand(()-> stopCalibrating()));
 
+               SendableChooser<InstantCommand> turretTestChooser = new SendableChooser<>();
+               turretTestChooser.setDefaultOption("Turn to 0", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0)));
+               turretTestChooser.addOption("Turn to -90", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0)));
+               turretTestChooser.addOption("Turn to 90", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0)));
+               turretTestChooser.addOption("Turn to 200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0)));
+               turretTestChooser.addOption("Turn to -200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0)));
+               SmartDashboard.putData("Turret Test Positions", turretTestChooser);
+
                double leftPosition = encoderLeft.getAbsolutePosition().getValueAsDouble();
                double leftAbs = wrapUnit(leftPosition - TurretConstants.LEFT_ENCODER_OFFSET);
 
@@ -126,12 +135,6 @@ public class Turret extends SubsystemBase implements TurretIO{
                //motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO);
 
                motor.setPosition(0.0);
-
-               SmartDashboard.putData("Turn to 0", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0);}));
-               SmartDashboard.putData("Turn to -90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0);}));
-               SmartDashboard.putData("Turn to 90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0);}));
-               SmartDashboard.putData("Turn to 200", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0);}));
-               SmartDashboard.putData("Turn to -200", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0);}));
        }
 
        /* ---------------- Public API ---------------- */