diff --git a/src/main/java/frc/robot/commands/shooter/AutoAim.java b/src/main/java/frc/robot/commands/shooter/AutoAim.java index 95fe879..8f0ca28 100644 --- a/src/main/java/frc/robot/commands/shooter/AutoAim.java +++ b/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 { diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 4942fce..fe91513 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -41,9 +41,7 @@ public ElevatorSubsystem() { _leftMotor.getConfigurator().apply(softLimits); - _heightController.setTolerance(0.01); - - SmartDashboard.putData("ELEVATOR PID", _heightController); + _heightController.setTolerance(0.02); } @Override @@ -51,7 +49,6 @@ 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()); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 38feaff..82ea570 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/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()); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 8990a78..81dbc4f 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 8447512..9995ccc 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/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 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); diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 77d63b2..145faeb 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/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 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; } } diff --git a/src/main/java/frc/robot/utils/UtilFuncs.java b/src/main/java/frc/robot/utils/UtilFuncs.java index 8553aae..a059f30 100644 --- a/src/main/java/frc/robot/utils/UtilFuncs.java +++ b/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;