From b1b7cc15bd656d32260b653fa8aff7e95820696a Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Sun, 21 Apr 2024 20:28:55 -0700 Subject: [PATCH 1/4] Add distance sensor sim support. --- build.gradle | 2 +- simgui.json | 9 +++++++++ .../org/carlmontrobotics/subsystems/IntakeShooter.java | 5 +++-- 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/build.gradle b/build.gradle index 0243b91a..a819c82b 100644 --- a/build.gradle +++ b/build.gradle @@ -79,7 +79,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - implementation "com.github.deepbluerobotics:lib199:0e5932a4219f3439ef2be3981a9189baf777f61e" + implementation "com.github.deepbluerobotics:lib199:67457c0d0880e4f484a1b5eedc094768114da9da" } test { diff --git a/simgui.json b/simgui.json index 9b3270e4..1f1f0726 100644 --- a/simgui.json +++ b/simgui.json @@ -16,6 +16,11 @@ "open": true } }, + "PlayingWithFusionTimeOfFlight[11]": { + "header": { + "open": true + } + }, "SparkMax[15]": { "header": { "open": true @@ -38,6 +43,7 @@ "/Shuffleboard/arm SysID/quasistatic forward": "Command", "/SmartDashboard/Arm": "Subsystem", "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/Intake Shooter": "Subsystem", "/SmartDashboard/SendableChooser[0]": "String Chooser", "/SmartDashboard/moveClimber": "Command" }, @@ -63,6 +69,9 @@ "NetworkTables": { "transitory": { "SmartDashboard": { + "Arm": { + "open": true + }, "BL": { "open": true }, diff --git a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java index 8ad3c0c7..b62b7911 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java +++ b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java @@ -4,6 +4,7 @@ import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.SensorFactory; import com.playingwithfusion.TimeOfFlight; import com.revrobotics.CANSparkBase; @@ -38,8 +39,8 @@ public class IntakeShooter extends SubsystemBase { private double goalOutakeRPM = outakeEncoder.getVelocity(); - private TimeOfFlight intakeDistanceSensor = new TimeOfFlight(INTAKE_DISTANCE_SENSOR_PORT); - private TimeOfFlight OutakeDistanceSensor = new TimeOfFlight(OUTAKE_DISTANCE_SENSOR_PORT); + private TimeOfFlight intakeDistanceSensor = SensorFactory.createPlayingWithFusionTimeOfFlight(INTAKE_DISTANCE_SENSOR_PORT); + private TimeOfFlight OutakeDistanceSensor = SensorFactory.createPlayingWithFusionTimeOfFlight(OUTAKE_DISTANCE_SENSOR_PORT); public IntakeShooter() { // Figure out which ones to set inverted From dd2475f34e747c0bbe467bd5a4f244720dfccbf6 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Thu, 20 Jun 2024 15:20:58 -0700 Subject: [PATCH 2/4] Add untested example of using distance sensor sims. --- .../subsystems/IntakeShooter.java | 41 +++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java index 8d17ed9e..f6ca1a64 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java +++ b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java @@ -15,12 +15,15 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.SimEnum; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; import edu.wpi.first.util.datalog.DataLogEntry; import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -57,6 +60,11 @@ public class IntakeShooter extends SubsystemBase { private double lastValidDistanceIntake = Double.POSITIVE_INFINITY; private double lastValidDistanceOuttake = Double.POSITIVE_INFINITY; + private SimDouble intakeDistanceSensorRangeSim, + outtakeDistanceSensorRangeSim; + private SimEnum intakeDistanceSensorStatusSim, + outtakeDistanceSensorStatusSim; + public IntakeShooter() { // Figure out which ones to set inverted intakeMotor.setInverted(INTAKE_MOTOR_INVERSION); @@ -81,6 +89,22 @@ public IntakeShooter() { outtakeMotorVortex.setSmartCurrentLimit(60); // SmartDashboard.putNumber("Intake target RPM", 0); // SmartDashboard.putNumber("Vortex volts", 0); + + SimDeviceSim intakeDistanceSensorSim = new SimDeviceSim( + "CANAIn:PlayingWithFusionTimeOfFlight[%d]-rangeVoltsIsMM" + .formatted(INTAKE_DISTANCE_SENSOR_PORT)); + intakeDistanceSensorRangeSim = + intakeDistanceSensorSim.getDouble("voltage"); + intakeDistanceSensorStatusSim = + intakeDistanceSensorSim.getEnum("status"); + + SimDeviceSim outtakeDistanceSensorSim = new SimDeviceSim( + "CANAIn:PlayingWithFusionTimeOfFlight[%d]-rangeVoltsIsMM" + .formatted(OUTAKE_DISTANCE_SENSOR_PORT)); + outtakeDistanceSensorRangeSim = + outtakeDistanceSensorSim.getDouble("voltage"); + outtakeDistanceSensorStatusSim = + outtakeDistanceSensorSim.getEnum("status"); } public boolean intakeIsOverTemp() { @@ -293,6 +317,23 @@ public void initSendable(SendableBuilder sendableBuilder) { sendableBuilder.addDoubleProperty("Period time", this::countPeridoic, null); } + + public void simulationPeriodic() { + if (intakeMotor.get() > 0.0) { + intakeDistanceSensorRangeSim.set( + Units.inchesToMeters(DETECT_DISTANCE_INCHES / 2) * 1000); + outtakeDistanceSensorRangeSim.set( + Units.inchesToMeters(DETECT_DISTANCE_INCHES / 2) * 1000); + } else if (intakeMotor.get() < 0.0 || outtakeMotorVortex.get() > 0.0) { + intakeDistanceSensorRangeSim.set( + Units.inchesToMeters(DETECT_DISTANCE_INCHES * 2) * 1000); + outtakeDistanceSensorRangeSim.set( + Units.inchesToMeters(DETECT_DISTANCE_INCHES * 2) * 1000); + } + intakeDistanceSensorStatusSim.set(TimeOfFlight.Status.Valid.ordinal()); + outtakeDistanceSensorStatusSim.set(TimeOfFlight.Status.Valid.ordinal()); + } + /* * public double calculateRPMAtDistance() { * From 26e07681d1255210c41f20dc23374b75e38986ad Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Thu, 20 Jun 2024 16:19:58 -0700 Subject: [PATCH 3/4] Revert unrelated formatting changes. --- .../java/org/carlmontrobotics/subsystems/IntakeShooter.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java index f6ca1a64..e1a336db 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java +++ b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java @@ -268,13 +268,13 @@ public void turnOffIntakeMotor() { public void setRPMOuttake(double rpm) { pidControllerOutake.setReference(rpm, CANSparkBase.ControlType.kVelocity, 0, - outtakeFeedforward.calculate(rpm / 60.0)); + outtakeFeedforward.calculate(rpm / 60.0)); } public void setRPMIntake(double rpm) { pidControllerIntake.setReference(rpm, CANSparkBase.ControlType.kVelocity, 0, - intakeFeedforward.calculate(rpm / 60.0)); + intakeFeedforward.calculate(rpm / 60.0)); } public double getOuttakeRPM() { From 292a7ca044936d64452efc5bb961f344eabdbd9e Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Thu, 20 Jun 2024 16:29:24 -0700 Subject: [PATCH 4/4] Try again to revert unrelated formatting. --- .../subsystems/IntakeShooter.java | 112 ++++++++---------- 1 file changed, 47 insertions(+), 65 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java index e1a336db..0dfd9d2f 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java +++ b/src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java @@ -28,27 +28,23 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeShooter extends SubsystemBase { - private final CANSparkMax intakeMotor = - MotorControllerFactory.createSparkMax(INTAKE_PORT, MotorConfig.NEO); + private final CANSparkMax intakeMotor = MotorControllerFactory.createSparkMax(INTAKE_PORT, MotorConfig.NEO); // private final CANSparkMax outakeMotor = // MotorControllerFactory.createSparkMax(10, MotorConfig.NEO_550); private final CANSparkFlex outtakeMotorVortex = new CANSparkFlex(OUTTAKE_PORT, MotorType.kBrushless); - private final RelativeEncoder outtakeEncoder = - outtakeMotorVortex.getEncoder(); + private final RelativeEncoder outtakeEncoder = outtakeMotorVortex.getEncoder(); private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder(); - private final SparkPIDController pidControllerOutake = - outtakeMotorVortex.getPIDController(); - private final SparkPIDController pidControllerIntake = - intakeMotor.getPIDController(); + private final SparkPIDController pidControllerOutake = outtakeMotorVortex.getPIDController(); + private final SparkPIDController pidControllerIntake = intakeMotor.getPIDController(); private Timer timer = new Timer(); private Timer intakeTOFTimer = new Timer(); private Timer outtakeTOFTimer = new Timer(); private int count = 0; - private SimpleMotorFeedforward intakeFeedforward = - new SimpleMotorFeedforward(kS[INTAKE], kV[INTAKE], kA[INTAKE]); - private final SimpleMotorFeedforward outtakeFeedforward = - new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], kA[OUTTAKE]); + private SimpleMotorFeedforward intakeFeedforward = new SimpleMotorFeedforward(kS[INTAKE], kV[INTAKE], + kA[INTAKE]); + private final SimpleMotorFeedforward outtakeFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], + kA[OUTTAKE]); private double goalOutakeRPM = outtakeEncoder.getVelocity(); @@ -83,8 +79,7 @@ public IntakeShooter() { // SmartDashboard.putNumber("intake volts", 0); // SmartDashboard.putNumber("Vortex volts", 0); // setMaxOutakeOverload(1); - intakeDistanceSensor.setRangingMode(RangingMode.Short, 24);// 24 ms is the minimum sample - // time acc to docs + intakeDistanceSensor.setRangingMode(RangingMode.Short, 24);// 24 ms is the minimum sample time acc to docs outtakeDistanceSensor.setRangingMode(RangingMode.Short, 24); outtakeMotorVortex.setSmartCurrentLimit(60); // SmartDashboard.putNumber("Intake target RPM", 0); @@ -108,8 +103,7 @@ public IntakeShooter() { } public boolean intakeIsOverTemp() { - return intakeMotor - .getMotorTemperature() >= MotorConfig.NEO.temperatureLimitCelsius; + return intakeMotor.getMotorTemperature() >= MotorConfig.NEO.temperatureLimitCelsius; } // --------------------------------------------------------------------------------------------------- @@ -156,25 +150,19 @@ 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; + 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(); - lastValidDistanceOuttake = - Units.metersToInches(outtakeDistanceSensor.getRange()) - / 1000.0; + lastValidDistanceOuttake = Units.metersToInches(outtakeDistanceSensor.getRange()) / 1000.0; } } @@ -189,18 +177,13 @@ public void periodic() { // } SmartDashboard.putBoolean("instake ds sees", intakeDetectsNote()); SmartDashboard.putBoolean("outtake ds sees", outtakeDetectsNote()); - SmartDashboard.putNumber("intake sample rate", - intakeDistanceSensor.getSampleTime()); + 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("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(); + TimeOfFlight.RangingMode rangingModeIntake = intakeDistanceSensor.getRangingMode(); if (rangingModeIntake == TimeOfFlight.RangingMode.Long) SmartDashboard.putString("intake ds ranging mode", "long"); else if (rangingModeIntake == TimeOfFlight.RangingMode.Medium) @@ -208,8 +191,7 @@ else if (rangingModeIntake == TimeOfFlight.RangingMode.Medium) else SmartDashboard.putString("intake ds ranging mode", "short"); - TimeOfFlight.RangingMode rangingModeOuttake = - outtakeDistanceSensor.getRangingMode(); + TimeOfFlight.RangingMode rangingModeOuttake = outtakeDistanceSensor.getRangingMode(); if (rangingModeOuttake == TimeOfFlight.RangingMode.Long) SmartDashboard.putString("outake ds ranging mode", "long"); else if (rangingModeOuttake == TimeOfFlight.RangingMode.Medium) @@ -227,8 +209,7 @@ else if (rangingModeOuttake == TimeOfFlight.RangingMode.Medium) if (intakeIsOverTemp()) { turnOffIntakeMotor(); stopIntake(); - System.err.println( - "INTAKE IS OVER TEMP!!!!\nBIG BAD\nOOPSY WOOPSY\nTURN IT OFF"); + System.err.println("INTAKE IS OVER TEMP!!!!\nBIG BAD\nOOPSY WOOPSY\nTURN IT OFF"); } } @@ -266,15 +247,13 @@ public void turnOffIntakeMotor() { } public void setRPMOuttake(double rpm) { - pidControllerOutake.setReference(rpm, - CANSparkBase.ControlType.kVelocity, 0, - outtakeFeedforward.calculate(rpm / 60.0)); + pidControllerOutake.setReference(rpm, CANSparkBase.ControlType.kVelocity, 0, + outtakeFeedforward.calculate(rpm / 60.0)); } public void setRPMIntake(double rpm) { - pidControllerIntake.setReference(rpm, - CANSparkBase.ControlType.kVelocity, 0, - intakeFeedforward.calculate(rpm / 60.0)); + pidControllerIntake.setReference(rpm, CANSparkBase.ControlType.kVelocity, 0, + intakeFeedforward.calculate(rpm / 60.0)); } public double getOuttakeRPM() { @@ -302,20 +281,13 @@ public double getVortexRPM() { @Override public void initSendable(SendableBuilder sendableBuilder) { super.initSendable(sendableBuilder); - sendableBuilder.addDoubleProperty("Outtake Velocity", - this::getOuttakeRPM, null); - sendableBuilder.addDoubleProperty("Intake velocity", this::getIntakeRPM, - null); - sendableBuilder.addDoubleProperty("Outake distance sensor", - this::getGamePieceDistanceOuttake, null); - sendableBuilder.addDoubleProperty("Intake distance sensor", - this::getGamePieceDistanceIntake, null); - sendableBuilder.addBooleanProperty("Outake distance sensor length", - this::outtakeDetectsNote, null); - sendableBuilder.addBooleanProperty("Intake distance sensor length", - this::intakeDetectsNote, null); - sendableBuilder.addDoubleProperty("Period time", this::countPeridoic, - null); + sendableBuilder.addDoubleProperty("Outtake Velocity", this::getOuttakeRPM, null); + sendableBuilder.addDoubleProperty("Intake velocity", this::getIntakeRPM, null); + sendableBuilder.addDoubleProperty("Outake distance sensor", this::getGamePieceDistanceOuttake, null); + sendableBuilder.addDoubleProperty("Intake distance sensor", this::getGamePieceDistanceIntake, null); + sendableBuilder.addBooleanProperty("Outake distance sensor length", this::outtakeDetectsNote, null); + sendableBuilder.addBooleanProperty("Intake distance sensor length", this::intakeDetectsNote, null); + sendableBuilder.addDoubleProperty("Period time", this::countPeridoic, null); } public void simulationPeriodic() { @@ -337,11 +309,21 @@ public void simulationPeriodic() { /* * public double calculateRPMAtDistance() { * - * double minRPM = Integer.MAX_VALUE; double distance = limelight.distanceToTargetSpeaker(); // - * placeholder for limelight for(int i = 0; i<= 360; i++) { double t = - * Math.sqrt((OFFSETFROMGROUND-SPEAKER_HEIGHT+distance*Math.tan(i))); double rpm = - * distance/Math.cos(i)*t; if(rpm