Merge branch 'main' of https://github.com/Team334/R2024

src/main/java/frc/robot/commands/shooter/AutoAim.java

@@ -186,8 +186,6 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    System.out.println("AIMING");
-
     double currentSwerveHeading = _swerve.getHeading().getDegrees();
 
     if (!_overrideDesired) {
@@ -206,8 +204,6 @@ public void execute() {
 
     if (_reachedSwerveHeading) rotationVelocity = 0;
 
-    SmartDashboard.putNumber("Y", _desiredSwerveHeading);
-
     // if (_reachedSwerveHeading && _reachedShooterAngle) {
     //   _leds.setColor(Constants.LEDColors.GREEN);
     // } else {

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

@@ -41,17 +41,14 @@ public ElevatorSubsystem() {
 
     _leftMotor.getConfigurator().apply(softLimits);
 
-    _heightController.setTolerance(0.01);
-
-    SmartDashboard.putData("ELEVATOR PID", _heightController);
+    _heightController.setTolerance(0.02);
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
     // harry chen code maybe fix
 
-    SmartDashboard.putNumber("ELEVATOR HEIGHT FROM GROUND", getHeight() + Physical.ELEVATOR_LOWEST_HEIGHT);
     SmartDashboard.putNumber("ELEVATOR HEIGHT METERS", getHeight());
   }
 

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -73,24 +73,6 @@ public IntakeSubsystem() {
     _actuatorMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
   }
 
-  // /** 
-  //  * Creates a safety if the intake is moving against a note.
-  //  * 
-  //  * @return True if the intake is moving against a note, else False.
-  //  */
-
-  // // TODO: why not working?
-  // public boolean noteSafety() {
-  //   SmartDashboard.putNumber("FEED OUTPUT", Math.abs(_feedMotor.get()));
-  //   SmartDashboard.putNumber("FEED VEL", Math.abs(_feedEncoder.getVelocity()));
-
-  //   if (Math.abs(_feedMotor.get()) > 0 && Math.abs(_feedEncoder.getVelocity()) < 2) {
-  //     return true;
-  //   }
-
-  //   return false;
-  // }
-
   /**
    * Toggle reverse soft limit.
    */
@@ -109,8 +91,6 @@ public void resetActuatorEncoder() {
    * Returns true if the actuator is at the last desired state.
    */
   public boolean atDesiredActuatorState() {
-    if (_actuatorController.atSetpoint()) System.out.println("s" + _actuatorController.getSetpoint());
-
     return _actuatorController.atSetpoint();
   }
 
@@ -197,11 +177,6 @@ public void feed(FeedMode feedMode) {
   public void periodic() {
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("ACTUATOR ENCODER", getActuator());
-    SmartDashboard.putData("ACTUATOR PID", _actuatorController);
-    SmartDashboard.putNumber("ACTUATOR OUT", _actuatorMotor.get());
-
-    // SmartDashboard.putBoolean("NOTE SAFETY", noteSafety());
-
-    // if (noteSafety()) { feed(FeedMode.NONE); }
+    SmartDashboard.putNumber("ACTUATOR OUTPUT", _actuatorMotor.get());
   }
 }

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -68,10 +68,8 @@ public ShooterSubsystem() {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    SmartDashboard.putNumber("SHOOTER ANGLE ENCODER", _angleMotor.getPosition().getValueAsDouble());
     SmartDashboard.putNumber("SHOOTER ANGLE", getAngle());
-
-    // SmartDashboard.putNumber("SHOOTER PERCENT OUTPUT", _leftMotor.get());
+    SmartDashboard.putNumber("SHOOTER PERCENT OUTPUT", _leftMotor.get());
   }
 
   /** Returns true if the shooter is at the last desired angle setpoint. */
@@ -120,7 +118,6 @@ public void setShooterState(ShooterState state) {
         if (UtilFuncs.ShootFast()) { 
           spinShooter(Speeds.SHOOTER_FAST_SPIN_SPEED); 
         } else {
-          System.out.println("FAR");
           spinShooter(Speeds.SHOOTER_SLOW_SPIN_SPEED);
         }
         break;

src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java

@@ -151,26 +151,19 @@ public void initSendable(SendableBuilder builder) {
         _frontRight.displayInfo(builder);
         _backRight.displayInfo(builder);
         _backLeft.displayInfo(builder);
-        builder.addDoubleProperty("Robot Angle", () -> getHeading().getDegrees(), null);
-        builder.addDoubleProperty("Swerve Speed", () -> Constants.Speeds.SWERVE_DRIVE_SLOW_COEFF, null);
+        builder.addBooleanProperty("Swerve FAST", () -> _drivingState == DrivingSpeeds.FAST, null);
       }
     });
-
-    SmartDashboard.putData("Swerve/Built-in Accelerometer", new BuiltInAccelerometer());
   }
 
   @Override
   public void periodic() {
     publisher.set(states);
 
-    SmartDashboard.putNumber("Gyro 180/-180", getHeading().getDegrees());
-
+    SmartDashboard.putNumber("Gyro RAW", getHeadingRaw().getDegrees());
     SmartDashboard.putBoolean("Field Oriented", fieldOriented);
-    SmartDashboard.putNumber("CAN Utilization %", RobotController.getCANStatus().percentBusUtilization * 100.0);
-    SmartDashboard.putNumber("Voltage", RobotController.getBatteryVoltage());
-    SmartDashboard.putNumber("CPU Temperature", RobotController.getCPUTemp());
-    SmartDashboard.putBoolean("RSL", RobotController.getRSLState());
     SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime());
+    SmartDashboard.putNumber("Speaker Distance", speakerDistance());
 
     // Update the bot's pose
     _estimator.update(getHeadingRaw(), new SwerveModulePosition[]{
@@ -180,11 +173,11 @@ public void periodic() {
       _backLeft.getPosition()
     });
 
-    SmartDashboard.putBoolean("IN RANGE", _visionSubsystem.isValid());
-
     Optional<Pose2d> visionBotpose = _visionSubsystem.getBotpose();
+
+    SmartDashboard.putBoolean("VISION VALID", visionBotpose.isPresent());
+
     if (visionBotpose.isPresent()) {
-      SmartDashboard.putNumber("X LL", visionBotpose.get().getX());
       _estimator.addVisionMeasurement(visionBotpose.get(), _visionSubsystem.getLatency());
     }
 
@@ -338,15 +331,11 @@ public double[] speakerSetpoints() {
 
     Pose3d speakerPose = UtilFuncs.GetAlliance() == Alliance.Red ? FieldConstants.SPEAKER_POSE_RED : FieldConstants.SPEAKER_POSE_BLUE;
 
-    SmartDashboard.putBoolean("IS RED", UtilFuncs.GetAlliance() == Alliance.Red);
-
     Translation2d speakerTranslation = new Translation2d(speakerPose.getX(), speakerPose.getY());
     Translation2d botTranslation = getPose().getTranslation();
 
     Translation2d distanceVec = speakerTranslation.minus(botTranslation);
 
-    SmartDashboard.putNumber("DISTANCE", distanceVec.getNorm());
-
     double elevatorHeight = Physical.ELEVATOR_MAX_SHOOT_HEIGHT + (distanceVec.getNorm() * Presets.ELEVATOR_HEIGHT_RATE); // TODO: get values and test
 
     xSpeakerAngle = MathUtil.inputModulus(distanceVec.getAngle().getDegrees(), -180, 180);

src/main/java/frc/robot/subsystems/VisionSubsystem.java

@@ -38,17 +38,6 @@ public VisionSubsystem() {
 
   @Override
   public void periodic() {
-    SmartDashboard.putBoolean("VISISBLE", isApriltagVisible());
-    // This method will be called once per scheduler run
-
-    // SmartDashboard.putNumber("retrieved botpose",
-    // getBotpose().getTranslation().getX());
-
-    // System.out.println(isApriltagVisible(6));
-
-    // _field.setRobotPose(getBotpose());
-
-    // SmartDashboard.putData("Limelight Field", _field);
   }
 
   /**
@@ -82,9 +71,6 @@ public Optional<Pose2d> getBotpose() {
     int tags = (int) botpose_array[7];
     double distance = botpose_array[9];
 
-    SmartDashboard.putNumber("TAGS", tags);
-    SmartDashboard.putNumber("DISTANCE", distance);
-
     if (tags < 2) return Optional.empty();
     if (distance > FieldConstants.TAG_DISTANCE_THRESHOLD) return Optional.empty();
 
@@ -110,7 +96,6 @@ public boolean isValid() {
     for (JsonNode tag : tags) {
       double distance = ((ArrayNode) tag.get("t6t_cs")).get(2).asDouble();
       if (distance <= FieldConstants.TAG_DISTANCE_THRESHOLD) {
-        SmartDashboard.putNumber("TAG DISTANCE", distance);
         return true;
       }
     }

src/main/java/frc/robot/utils/UtilFuncs.java

@@ -25,7 +25,6 @@ public static void ShootFast(DoubleSupplier distance) {
    * Whether to shoot fast based on distance of bot from speaker shot point.
    */
   public static boolean ShootFast() {
-    SmartDashboard.putNumber("DISTANCE SHOOTING", _distanceSupplier.getAsDouble());
     if (_distanceSupplier.getAsDouble() > FieldConstants.SHOOTER_SLOW_THRESHOLD) return true;
 
     return false;