Skip to content

Commit

Permalink
big removal of smartdashboard in subsystems
Browse files Browse the repository at this point in the history
  • Loading branch information
@stwiggy
stwiggy committed Jul 20, 2024
1 parent 5d85918 commit b4244de
Show file tree
Hide file tree
Showing 6 changed files with 117 additions and 121 deletions.
77 changes: 34 additions & 43 deletions src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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())
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

}

Expand All @@ -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;
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/org/carlmontrobotics/subsystems/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ public Arm() {



SmartDashboard.putData("Arm", this);
// SmartDashboard.putData("Arm", this);

setpoint = getCurrentArmState();
goalState = getCurrentArmState();
Expand Down Expand Up @@ -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");
Expand All @@ -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);

Expand All @@ -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())
Expand Down
50 changes: 26 additions & 24 deletions src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -220,7 +220,7 @@ public Drivetrain() {

}

SmartDashboard.putData("Field", field);
// SmartDashboard.putData("Field", field);

// for(CANSparkMax driveMotor : driveMotors)
// driveMotor.setSmartCurrentLimit(80);
Expand All @@ -247,7 +247,7 @@ public Drivetrain() {
// SmartDashboard.putNumber("chassis speeds y", 0);

// SmartDashboard.putNumber("chassis speeds theta", 0);
SmartDashboard.putData(this);
// SmartDashboard.putData(this);

}

Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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);
}
}

Expand Down Expand Up @@ -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);
}
Expand Down
57 changes: 30 additions & 27 deletions src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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() {
Expand Down Expand Up @@ -121,15 +121,17 @@ 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();
lastValidDistanceIntake = Units.metersToInches(intakeDistanceSensor.getRange()) / 1000.0;
}
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();
Expand All @@ -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();
Expand Down
Loading

0 comments on commit b4244de

Please sign in to comment.