Nh command loop #178
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,33 +1,51 @@ | ||
| package frc.robot.commands.ramp; | ||
|
|
||
| import edu.wpi.first.wpilibj.Timer; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import frc.robot.constants.Constants; | ||
| import frc.robot.subsystems.LightStrip; | ||
| import frc.robot.subsystems.Ramp; | ||
| import frc.robot.utils.TimeoutCounter; | ||
|
|
||
| import java.util.function.DoubleSupplier; | ||
|
|
||
| public class RampMove extends Command { | ||
|
|
||
| private final Ramp ramp; | ||
| private final DoubleSupplier encoderValue; | ||
| private final TimeoutCounter timeoutCounter; | ||
| private final Timer timer = new Timer(); | ||
|
|
||
| public RampMove(Ramp ramp, LightStrip lightStrip, DoubleSupplier encoderValue) { | ||
| this.ramp = ramp; | ||
| this.encoderValue = encoderValue; | ||
| this.timeoutCounter = new TimeoutCounter("RampMove", lightStrip); | ||
| addRequirements(this.ramp); | ||
| } | ||
|
|
||
| @Override | ||
| public void initialize() { | ||
| timer.restart(); | ||
| ramp.setRampPos(encoderValue.getAsDouble()); | ||
| } | ||
|
|
||
|
|
||
| @Override | ||
| public void execute() { | ||
| ramp.updateFF(); | ||
| } | ||
|
|
||
| @Override | ||
| public void end(boolean interrupted) { | ||
| timer.stop(); | ||
| } | ||
|
|
||
| @Override | ||
| public boolean isFinished() { | ||
| if (timer.hasElapsed(Constants.RAMP_MOVE_TIMEOUT)){ | ||
| timeoutCounter.increaseTimeoutCount(); | ||
| return true; | ||
| } | ||
| return Math.abs(ramp.getRampPos() - ramp.getDesiredPosition()) < Constants.RAMP_MOVE_THRESHOLD; | ||
| } | ||
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -43,6 +43,7 @@ public void initialize() { | |
|
|
||
| @Override | ||
| public void execute() { | ||
| ramp.updateFF(); | ||
| } | ||
|
|
||
| @Override | ||
|
|
||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,52 +1,58 @@ | ||
| package frc.robot.subsystems; | ||
|
|
||
| import edu.wpi.first.wpilibj.motorcontrol.Spark; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import frc.robot.constants.Constants; | ||
| import frc.robot.utils.BlinkinPattern; | ||
| import frc.robot.utils.smartshuffleboard.SmartShuffleboard; | ||
|
|
||
| import java.util.concurrent.ConcurrentHashMap; | ||
| import java.util.concurrent.atomic.AtomicBoolean; | ||
| import java.util.concurrent.atomic.AtomicReference; | ||
| import java.util.function.BooleanSupplier; | ||
|
|
||
| public class LightStrip extends SubsystemBase { | ||
| private final Spark colorSensorPort; | ||
| private final AtomicReference<BlinkinPattern> pattern = new AtomicReference<>(BlinkinPattern.BLACK); | ||
| private final ConcurrentHashMap<BooleanSupplier, BlinkinPattern> predicateLightEvents = new ConcurrentHashMap<>(); | ||
| private final AtomicBoolean running = new AtomicBoolean(true); | ||
| public LightStrip(int port) { | ||
| this.colorSensorPort = new Spark(port); | ||
| new Thread(() -> { | ||
| while (running.get()){ | ||
| for (BooleanSupplier supplier : predicateLightEvents.keySet()){ | ||
| if (supplier.getAsBoolean()){ | ||
| setPattern(predicateLightEvents.get(supplier)); | ||
| predicateLightEvents.remove(supplier); | ||
| break; | ||
| } | ||
| } | ||
| try { | ||
| Thread.sleep(40); | ||
| } catch (InterruptedException e) { | ||
| throw new RuntimeException(e); | ||
| } | ||
| } | ||
| }).start(); | ||
| } | ||
|
|
||
| public void setPattern(BlinkinPattern pattern) { | ||
| this.pattern.set(pattern); | ||
| colorSensorPort.set(pattern.getPwm()); | ||
armadilloz marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| if (Constants.LED_DEBUG){ | ||
| SmartShuffleboard.put("Lightstrip", "pwm", pattern.getPwm()); | ||
| SmartShuffleboard.put("Lightstrip", "pattern", pattern.toString()); | ||
| } | ||
| } | ||
|
|
||
| public BlinkinPattern getPattern() { | ||
| return pattern.get(); | ||
| } | ||
| public void scheduleOnTrue(BooleanSupplier callable, BlinkinPattern pattern) { | ||
| predicateLightEvents.put(callable, pattern); | ||
| } | ||
|
|
||
| public void setRunning(boolean running) { | ||
| this.running.set(running); | ||
| } | ||
| } | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Seems like there is no real need to protect the
patternand therunningsince they are primitives (boolean and a reference), though using an atomic reference here does not hurt. The one thing that does require protecting as it is modified from both threads is the map, which is currently vulnerable.There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
changed to concurrent hashmap