From b4244de7fb06421f0eabad2144f1c6d642b2e862 Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Sat, 20 Jul 2024 00:22:45 -0700 Subject: [PATCH] big removal of smartdashboard in subsystems --- .../commands/AlignToApriltag.java | 77 ++++++++----------- .../commands/AutoMATICALLYGetNote.java | 6 +- .../org/carlmontrobotics/subsystems/Arm.java | 8 +- .../subsystems/Drivetrain.java | 50 ++++++------ .../subsystems/IntakeShooter.java | 57 +++++++------- .../subsystems/Limelight.java | 40 +++++----- 6 files changed, 117 insertions(+), 121 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java index 57b0a242..bac2cc1f 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java +++ b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java @@ -23,12 +23,8 @@ public class AlignToApriltag extends Command { private Limelight limelight; public final PIDController rotationPID = new PIDController( - SmartDashboard.getNumber("apriltag align kp", - thetaPIDController[0]), - SmartDashboard.getNumber("apriltag align ki", - thetaPIDController[1]), - SmartDashboard.getNumber("apriltag align kd", - thetaPIDController[2])); + thetaPIDController[0], thetaPIDController[1], + thetaPIDController[2]); public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) { this.limelight = limelight; @@ -42,49 +38,44 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) { .getRotateAngleRadMT2())); rotationPID.setSetpoint(MathUtil.inputModulus( targetAngle.getDegrees(), -180, 180)); - rotationPID.setTolerance( - SmartDashboard.getNumber( - "apriltag align pos tolerance", - positionTolerance[2]), - SmartDashboard.getNumber( - "apriltag align vel tolerance", - velocityTolerance[2])); + rotationPID.setTolerance(positionTolerance[2], + velocityTolerance[2]); SendableRegistry.addChild(this, rotationPID); addRequirements(drivetrain); } @Override public void execute() { - double kp = SmartDashboard.getNumber("apriltag align kp", - rotationPID.getP()); - double ki = SmartDashboard.getNumber("apriltag align ki", - rotationPID.getI()); - double kd = SmartDashboard.getNumber("apriltag align kd", - rotationPID.getD()); - - if (kp != rotationPID.getP()) - rotationPID.setP(kp); - if (ki != rotationPID.getI()) - rotationPID.setI(ki); - if (kd != rotationPID.getD()) - rotationPID.setD(kd); - - double posTolerance = SmartDashboard.getNumber( - "apriltag align pos tolerance", - rotationPID.getPositionTolerance()); - double velTolerance = SmartDashboard.getNumber( - "apriltag align vel tolerance", - rotationPID.getVelocityTolerance()); - - if (posTolerance != rotationPID.getPositionTolerance() - || velTolerance != rotationPID - .getVelocityTolerance()) - rotationPID.setTolerance(posTolerance, velTolerance); - - SmartDashboard.putNumber("apriltag align pos error (rad)", - rotationPID.getPositionError()); - SmartDashboard.putNumber("apriltag align vel error (rad/s)", - rotationPID.getVelocityError()); + // double kp = SmartDashboard.getNumber("apriltag align kp", + // rotationPID.getP()); + // double ki = SmartDashboard.getNumber("apriltag align ki", + // rotationPID.getI()); + // double kd = SmartDashboard.getNumber("apriltag align kd", + // rotationPID.getD()); + + // if (kp != rotationPID.getP()) + // rotationPID.setP(kp); + // if (ki != rotationPID.getI()) + // rotationPID.setI(ki); + // if (kd != rotationPID.getD()) + // rotationPID.setD(kd); + + // double posTolerance = SmartDashboard.getNumber( + // "apriltag align pos tolerance", + // rotationPID.getPositionTolerance()); + // double velTolerance = SmartDashboard.getNumber( + // "apriltag align vel tolerance", + // rotationPID.getVelocityTolerance()); + + // if (posTolerance != rotationPID.getPositionTolerance() + // || velTolerance != rotationPID + // .getVelocityTolerance()) + // rotationPID.setTolerance(posTolerance, velTolerance); + + // SmartDashboard.putNumber("apriltag align pos error (rad)", + // rotationPID.getPositionError()); + // SmartDashboard.putNumber("apriltag align vel error (rad/s)", + // rotationPID.getVelocityError()); Rotation2d targetAngle = Rotation2d .fromDegrees(drivetrain.getHeading()) diff --git a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java index dbeb601a..e019a042 100644 --- a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java +++ b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java @@ -44,7 +44,7 @@ public void initialize() { // timer.start(); // new Intake(intake).finallyDo(()->{this.end(false);}); dt.setFieldOriented(false); - SmartDashboard.putNumber("turning speed multiplier", 3); + // SmartDashboard.putNumber("turning speed multiplier", 3); } @@ -62,12 +62,12 @@ public void execute() { forwardDistErrMeters = Math.max( forwardDistErrMeters - * SmartDashboard.getNumber("forward speed multiplier", 1.5), + * 1.5, MIN_MOVEMENT_METERSPSEC); if (LimelightHelpers.getTV(INTAKE_LL_NAME)) { dt.drive(forwardDistErrMeters, strafeDistErrMeters, angleErrRad - * SmartDashboard.getNumber("turning speed multiplier", 3)); + * 3); } // double forwardSpeed = 0; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 37719b8b..084993ea 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -130,7 +130,7 @@ public Arm() { - SmartDashboard.putData("Arm", this); + // SmartDashboard.putData("Arm", this); setpoint = getCurrentArmState(); goalState = getCurrentArmState(); @@ -166,7 +166,7 @@ public Arm() { TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints( (MAX_FF_VEL_RAD_P_S), (MAX_FF_ACCEL_RAD_P_S)); armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS); - SmartDashboard.putBoolean("arm is at pos", false); + // SmartDashboard.putBoolean("arm is at pos", false); if (RobotBase.isSimulation()) { rotationsSim = new SimDeviceSim("CANDutyCycle:CANSparkMax", ARM_MOTOR_PORT_MASTER).getDouble("position"); @@ -182,7 +182,7 @@ public void setBooleanDrive(boolean climb) { @Override public void periodic() { - SmartDashboard.putNumber("arm angle", getArmPos()); // for limelight testing + // SmartDashboard.putNumber("arm angle", getArmPos()); // for limelight testing babyMode = SmartDashboard.getBoolean("babymode", false); @@ -191,7 +191,7 @@ public void periodic() { // ^ worst case scenario // armFeed.maxAchievableVelocity(12, 0, MAX_FF_ACCEL_RAD_P_S) - SmartDashboard.putData(this); + // SmartDashboard.putData(this); // armMotorMaster.setSmartCurrentLimit(50); // armMotorFollower.setSmartCurrentLimit(50); if (DriverStation.isDisabled()) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index a22228e9..b3b940a8 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -112,13 +112,13 @@ public class Drivetrain extends SubsystemBase { private double lastSetX = 0, lastSetY = 0, lastSetTheta = 0; public Drivetrain() { - SmartDashboard.putNumber("Pose Estimator set x (m)", lastSetX); - SmartDashboard.putNumber("Pose Estimator set y (m)", lastSetY); - SmartDashboard.putNumber("Pose Estimator set rotation (deg)", - lastSetTheta); + // SmartDashboard.putNumber("Pose Estimator set x (m)", lastSetX); + // SmartDashboard.putNumber("Pose Estimator set y (m)", lastSetY); + // SmartDashboard.putNumber("Pose Estimator set rotation (deg)", + // lastSetTheta); - SmartDashboard.putNumber("pose estimator std dev x", STD_DEV_X_METERS); - SmartDashboard.putNumber("pose estimator std dev y", STD_DEV_Y_METERS); + // SmartDashboard.putNumber("pose estimator std dev x", STD_DEV_X_METERS); + // SmartDashboard.putNumber("pose estimator std dev y", STD_DEV_Y_METERS); // Calibrate Gyro { @@ -220,7 +220,7 @@ public Drivetrain() { } - SmartDashboard.putData("Field", field); + // SmartDashboard.putData("Field", field); // for(CANSparkMax driveMotor : driveMotors) // driveMotor.setSmartCurrentLimit(80); @@ -247,7 +247,7 @@ public Drivetrain() { // SmartDashboard.putNumber("chassis speeds y", 0); // SmartDashboard.putNumber("chassis speeds theta", 0); - SmartDashboard.putData(this); + // SmartDashboard.putData(this); } @@ -324,19 +324,22 @@ public void periodic() { updateMT2PoseEstimator(); - double currSetX = - SmartDashboard.getNumber("Pose Estimator set x (m)", lastSetX); - double currSetY = - SmartDashboard.getNumber("Pose Estimator set y (m)", lastSetY); - double currSetTheta = SmartDashboard - .getNumber("Pose Estimator set rotation (deg)", lastSetTheta); - - if (lastSetX != currSetX || lastSetY != currSetY - || lastSetTheta != currSetTheta) { - setPose(new Pose2d(currSetX, currSetY, - Rotation2d.fromDegrees(currSetTheta))); - } + // double currSetX = + // SmartDashboard.getNumber("Pose Estimator set x (m)", lastSetX); + // double currSetY = + // SmartDashboard.getNumber("Pose Estimator set y (m)", lastSetY); + // double currSetTheta = SmartDashboard + // .getNumber("Pose Estimator set rotation (deg)", lastSetTheta); + + // if (lastSetX != currSetX || lastSetY != currSetY + // || lastSetTheta != currSetTheta) { + // setPose(new Pose2d(currSetX, currSetY, + // Rotation2d.fromDegrees(currSetTheta))); + // } + setPose(new Pose2d(getPose().getTranslation().getX(), + getPose().getTranslation().getY(), + Rotation2d.fromDegrees(getHeading()))); // // // SmartDashboard.putNumber("Pitch", gyro.getPitch()); // // // SmartDashboard.putNumber("Roll", gyro.getRoll()); @@ -876,9 +879,9 @@ private void sysIdSetup() { m_revs_vel[i] = mutable(RotationsPerSecond.of(0)); } - SmartDashboard.putNumber("Desired Angle", 0); + // SmartDashboard.putNumber("Desired Angle", 0); - SmartDashboard.putNumber("kS", 0); + // SmartDashboard.putNumber("kS", 0); } } @@ -1073,8 +1076,7 @@ public void updateMT2PoseEstimator() { if (!rejectVisionUpdate) { poseEstimator .setVisionMeasurementStdDevs( - VecBuilder.fill(SmartDashboard.getNumber("pose estimator std dev x", STD_DEV_X_METERS), - SmartDashboard.getNumber("pose estimator std dev y", STD_DEV_Y_METERS), + VecBuilder.fill(STD_DEV_X_METERS, STD_DEV_Y_METERS, STD_DEV_HEADING_RADS)); poseEstimator.addVisionMeasurement(visionPoseEstimate.pose, visionPoseEstimate.timestampSeconds); } diff --git a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java index ce3f5c1d..33f204b7 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java +++ b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java @@ -59,7 +59,7 @@ public IntakeShooter() { pidControllerIntake.setP(kP[INTAKE]); pidControllerIntake.setI(kI[INTAKE]); pidControllerIntake.setD(kD[INTAKE]); - SmartDashboard.putData("Intake Shooter", this); + // SmartDashboard.putData("Intake Shooter", this); // SmartDashboard.putNumber("Intake Ks", kS[INTAKE]); // SmartDashboard.putNumber("Intake Kv", kV[INTAKE]); intakeEncoder.setAverageDepth(4); @@ -72,7 +72,7 @@ public IntakeShooter() { outtakeMotorVortex.setSmartCurrentLimit(60); // SmartDashboard.putNumber("Intake target RPM", 0); // SmartDashboard.putNumber("Vortex volts", 0); - SmartDashboard.putNumber("Goal RPM Outtake", 0); + // SmartDashboard.putNumber("Goal RPM Outtake", 0); } public boolean intakeIsOverTemp() { @@ -121,7 +121,8 @@ public boolean outtakeDetectsNote() { public void updateValues() { if (intakeDistanceSensor.isRangeValid()) { if (lastValidDistanceIntake != Double.POSITIVE_INFINITY) { - SmartDashboard.putNumber("Time between valid ranges intake", intakeTOFTimer.get()); + // SmartDashboard.putNumber("Time between valid ranges intake", + // intakeTOFTimer.get()); intakeTOFTimer.reset(); } else intakeTOFTimer.start(); @@ -129,7 +130,8 @@ public void updateValues() { } if (outtakeDistanceSensor.isRangeValid()) { if (lastValidDistanceOuttake != Double.POSITIVE_INFINITY) { - SmartDashboard.putNumber("Time between valid ranges outtake", outtakeTOFTimer.get()); + // SmartDashboard.putNumber("Time between valid ranges outtake", + // outtakeTOFTimer.get()); outtakeTOFTimer.reset(); } else outtakeTOFTimer.start(); @@ -146,36 +148,37 @@ public void periodic() { // if (newKS != intakeFeedforward.ks || newKV != intakeFeedforward.kv) { // intakeFeedforward = new SimpleMotorFeedforward(newKS, newKV); // } - SmartDashboard.putBoolean("instake ds sees", intakeDetectsNote()); - SmartDashboard.putBoolean("outtake ds sees", outtakeDetectsNote()); - SmartDashboard.putNumber("intake sample rate", intakeDistanceSensor.getSampleTime()); - SmartDashboard.putData("intake distanace sensor", intakeDistanceSensor); - SmartDashboard.putBoolean("intake ds range valid", intakeDistanceSensor.isRangeValid()); - SmartDashboard.putData("outtake distanace sensor", outtakeDistanceSensor); - SmartDashboard.putBoolean("outtake ds range valid", outtakeDistanceSensor.isRangeValid()); + // SmartDashboard.putBoolean("instake ds sees", intakeDetectsNote()); + // SmartDashboard.putBoolean("outtake ds sees", outtakeDetectsNote()); + // SmartDashboard.putNumber("intake sample rate", intakeDistanceSensor.getSampleTime()); + // SmartDashboard.putData("intake distanace sensor", intakeDistanceSensor); + // SmartDashboard.putBoolean("intake ds range valid", intakeDistanceSensor.isRangeValid()); + // SmartDashboard.putData("outtake distanace sensor", outtakeDistanceSensor); + // SmartDashboard.putBoolean("outtake ds range valid", + // outtakeDistanceSensor.isRangeValid()); SmartDashboard.putNumber("Intake Vel", intakeEncoder.getVelocity()); - TimeOfFlight.RangingMode rangingModeIntake = intakeDistanceSensor.getRangingMode(); - if (rangingModeIntake == TimeOfFlight.RangingMode.Long) - SmartDashboard.putString("intake ds ranging mode", "long"); - else if (rangingModeIntake == TimeOfFlight.RangingMode.Medium) - SmartDashboard.putString("intake ds ranging mode", "medium"); - else - SmartDashboard.putString("intake ds ranging mode", "short"); - - TimeOfFlight.RangingMode rangingModeOuttake = outtakeDistanceSensor.getRangingMode(); - if (rangingModeOuttake == TimeOfFlight.RangingMode.Long) - SmartDashboard.putString("outake ds ranging mode", "long"); - else if (rangingModeOuttake == TimeOfFlight.RangingMode.Medium) - SmartDashboard.putString("outake ds ranging mode", "medium"); - else - SmartDashboard.putString("outake ds ranging mode", "short"); + // TimeOfFlight.RangingMode rangingModeIntake = intakeDistanceSensor.getRangingMode(); + // if (rangingModeIntake == TimeOfFlight.RangingMode.Long) + // SmartDashboard.putString("intake ds ranging mode", "long"); + // else if (rangingModeIntake == TimeOfFlight.RangingMode.Medium) + // SmartDashboard.putString("intake ds ranging mode", "medium"); + // else + // SmartDashboard.putString("intake ds ranging mode", "short"); + + // TimeOfFlight.RangingMode rangingModeOuttake = outtakeDistanceSensor.getRangingMode(); + // if (rangingModeOuttake == TimeOfFlight.RangingMode.Long) + // SmartDashboard.putString("outake ds ranging mode", "long"); + // else if (rangingModeOuttake == TimeOfFlight.RangingMode.Medium) + // SmartDashboard.putString("outake ds ranging mode", "medium"); + // else + // SmartDashboard.putString("outake ds ranging mode", "short"); // intakeMotor.set(0.1); // outakeMotor.set(SmartDashboard.getNumber("intake volts", 0)); // count++; // setMaxOutake(); - SmartDashboard.putNumber("Intake amps", intakeMotor.getOutputCurrent()); + // SmartDashboard.putNumber("Intake amps", intakeMotor.getOutputCurrent()); if (intakeIsOverTemp()) { turnOffIntakeMotor(); diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index 89f38752..092b9799 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -32,21 +32,21 @@ public Limelight(Drivetrain drivetrain) { // ASSUMING SHOOTING AT 4000 RPM // changing speed multipliers for auto intaking note - SmartDashboard.putNumber("forward speed multiplier", 1.5); - SmartDashboard.putNumber("strafe speed multiplier", 1.5); - SmartDashboard.putNumber("rotational speed multiplier", 2); + // SmartDashboard.putNumber("forward speed multiplier", 1.5); + // SmartDashboard.putNumber("strafe speed multiplier", 1.5); + // SmartDashboard.putNumber("rotational speed multiplier", 2); // tuning apriltag alignment pid and tolerances - SmartDashboard.putNumber("rotation to align", getRotateAngleRadMT2()); + // SmartDashboard.putNumber("rotation to align", getRotateAngleRadMT2()); - SmartDashboard.putNumber("apriltag align kp", thetaPIDController[0]); - SmartDashboard.putNumber("apriltag align ki", thetaPIDController[1]); - SmartDashboard.putNumber("apriltag align kd", thetaPIDController[2]); + // SmartDashboard.putNumber("apriltag align kp", thetaPIDController[0]); + // SmartDashboard.putNumber("apriltag align ki", thetaPIDController[1]); + // SmartDashboard.putNumber("apriltag align kd", thetaPIDController[2]); - SmartDashboard.putNumber("apriltag align pos tolerance", - positionTolerance[2]); - SmartDashboard.putNumber("apriltag align vel tolerance", - velocityTolerance[2]); + // SmartDashboard.putNumber("apriltag align pos tolerance", + // positionTolerance[2]); + // SmartDashboard.putNumber("apriltag align vel tolerance", + // velocityTolerance[2]); } @@ -54,17 +54,17 @@ public Limelight(Drivetrain drivetrain) { public void periodic() { // intake limelight testing - SmartDashboard.putBoolean("see note", - LimelightHelpers.getTV(INTAKE_LL_NAME)); - SmartDashboard.putNumber("distance to note", getDistanceToNoteMeters()); - SmartDashboard.putNumber("intake tx", - LimelightHelpers.getTX(INTAKE_LL_NAME)); + // SmartDashboard.putBoolean("see note", + // LimelightHelpers.getTV(INTAKE_LL_NAME)); + // SmartDashboard.putNumber("distance to note", getDistanceToNoteMeters()); + // SmartDashboard.putNumber("intake tx", + // LimelightHelpers.getTX(INTAKE_LL_NAME)); // shooter limelight testing - SmartDashboard.putNumber("distance to speaker (meters)", - getDistanceToSpeakerMetersMT2()); - SmartDashboard.putNumber("optimized arm angle", - getOptimizedArmAngleRadsMT2()); + // SmartDashboard.putNumber("distance to speaker (meters)", + // getDistanceToSpeakerMetersMT2()); + // SmartDashboard.putNumber("optimized arm angle", + // getOptimizedArmAngleRadsMT2()); } public double getTXDeg(String limelightName) {