From 650ee34bc6067a4a406f9235e55a7837c09fe164 Mon Sep 17 00:00:00 2001 From: Rong Date: Sat, 29 Mar 2025 13:34:10 -0700 Subject: [PATCH] Potentially risky but... first generation laser can failure detection --- .../elevator/ElevatorSubsystem.java | 42 ++++++++++++++++--- .../commands/SetUseDistanceSensorCommand.java | 36 ++++++++++++++++ 2 files changed, 73 insertions(+), 5 deletions(-) create mode 100644 src/main/java/competition/subsystems/elevator/commands/SetUseDistanceSensorCommand.java diff --git a/src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java index ab37ce04..9839aef4 100644 --- a/src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java @@ -91,6 +91,12 @@ public class ElevatorSubsystem extends BaseSetpointSubsystem { public final XDigitalInput bottomSensor; public final XLaserCAN distanceSensor; + public final DoubleProperty laserCanOffsetThreshold; + private boolean useDistanceSensor = false; + public int badMotorLaserCanHarmonyCount = 0; + public int harmonyThreshold = 3; + private Distance previousLaserCanDistance; + private final SysIdRoutine sysId; private final ComplimentaryFilter sensorFusionFilter; @@ -120,7 +126,6 @@ public ElevatorSubsystem(XCANMotorController.XCANMotorControllerFactory motorFac trimChangeAmount = pf.createPersistentProperty("TrimUpAmount", Inches.of(1)); laserCanMaxMeasurementLatency = pf.createPersistentProperty("laserCanMaxMeasurementLatency-S", 0.04); - //to be tuned // based on some initial experiments: // Elevator raises 36.375 inches (0.923925 meters) after 42.6535 revolutions @@ -142,8 +147,9 @@ public ElevatorSubsystem(XCANMotorController.XCANMotorControllerFactory motorFac this.motionMagicJerk = pf.createPersistentProperty("motionMagicMaxJerk", 0.1); this.motionMagicEnabled = pf.createPersistentProperty("motionMagicEnabled", false); - pf.setDefaultLevel(PropertyLevel.Important); + this.laserCanOffsetThreshold = pf.createPersistentProperty("laserCanOffsetThreshold-m", 0.0254); // 1 Inch + pf.setDefaultLevel(PropertyLevel.Important); this.sysId = new SysIdRoutine( new SysIdRoutine.Config( @@ -184,6 +190,7 @@ public ElevatorSubsystem(XCANMotorController.XCANMotorControllerFactory motorFac if (contract.isElevatorDistanceSensorReady()) { this.distanceSensor = xLaserCANFactory.create(contract.getElevatorDistanceSensor(), this.getPrefix()); + useDistanceSensor = true; registerDataFrameRefreshable(distanceSensor); } else { this.distanceSensor = null; @@ -327,7 +334,7 @@ private Optional getCalibratedLaserDistance() { } private Optional getRawLaserDistance() { - if (contract.isElevatorDistanceSensorReady()) { + if (contract.isElevatorDistanceSensorReady() && useDistanceSensor) { var distance = distanceSensor.getDistance(); var latency = distanceSensor.getMeasurementLatency(); return distance.filter(d -> latency.lt(Seconds.of(laserCanMaxMeasurementLatency.get()))); @@ -417,12 +424,16 @@ public void configureMotionMagicConstraints(){ masterMotor.setTrapezoidalProfileJerk(RadiansPerSecondPerSecond.of(motionMagicJerk.get()).per(Second)); } + public void setUseDistanceSensor(boolean value) { + useDistanceSensor = value; + } + @Override public void periodic() { if (contract.isElevatorReady()) { masterMotor.periodic(); } - if(motionMagicAcceleration.hasChangedSinceLastCheck() || motionMagicJerk.hasChangedSinceLastCheck()){ + if (motionMagicAcceleration.hasChangedSinceLastCheck() || motionMagicJerk.hasChangedSinceLastCheck()){ configureMotionMagicConstraints(); } //bandage case: isTouchingBottom flashes true for one tick on startup, investigate later? @@ -440,7 +451,28 @@ public void periodic() { aKitLog.record("isElevatorCalibrated", isCalibrated()); aKitLog.record("isElevatorMaintainerAtGoal", this.isMaintainerAtGoal()); isNotCalibratedAlert.set(!isCalibrated()); - getRawLaserDistance().ifPresent(d -> aKitLog.record("ElevatorDistanceSensor-m", d.in(Meters))); + getRawLaserDistance().ifPresent(d -> { + aKitLog.record("ElevatorDistanceSensor-m", d.in(Meters)); + + if (previousLaserCanDistance == null) { + previousLaserCanDistance = d; + } + + // If our laser can distance is dropping above a certain threshold, for three times + // All while our master motor thinks that it has positive velocity (up I am assuming) then + // We are in a state of not good stuff and should disable the distance sensor + if (previousLaserCanDistance.minus(d).in(Meters) > laserCanOffsetThreshold.get() + && masterMotor.getRawVelocity().baseUnitMagnitude() > 0) { + if (badMotorLaserCanHarmonyCount < harmonyThreshold) { + badMotorLaserCanHarmonyCount++; + } else { + badMotorLaserCanHarmonyCount = 0; + useDistanceSensor = false; + } + } + + previousLaserCanDistance = d; + }); getCalibratedLaserDistance().ifPresent(d -> aKitLog.record("CalibratedElevatorDistanceSensor-m", d.in(Meters))); aKitLog.record("CalibratedElevatorMotorSensor-m", getCalibratedMotorDistance().in(Meters)); aKitLog.record("MotorOffset-rotations", elevatorMotorPositionOffset.in(Rotations)); diff --git a/src/main/java/competition/subsystems/elevator/commands/SetUseDistanceSensorCommand.java b/src/main/java/competition/subsystems/elevator/commands/SetUseDistanceSensorCommand.java new file mode 100644 index 00000000..bac6247f --- /dev/null +++ b/src/main/java/competition/subsystems/elevator/commands/SetUseDistanceSensorCommand.java @@ -0,0 +1,36 @@ +package competition.subsystems.elevator.commands; + +import competition.subsystems.elevator.ElevatorSubsystem; +import xbot.common.command.BaseCommand; + +import javax.inject.Inject; + +public class SetUseDistanceSensorCommand extends BaseCommand { + + final ElevatorSubsystem elevator; + private boolean value; + + @Inject + public SetUseDistanceSensorCommand(ElevatorSubsystem elevator) { + this.elevator = elevator; + } + + public void setValue(boolean value) { + this.value = value; + } + + @Override + public void initialize() { + elevator.setUseDistanceSensor(value); + } + + @Override + public void execute() { + // No-code + } + + @Override + public boolean isFinished() { + return true; + } +}