From 1df97bc6345e34099ba5a6cc213e6c0ef10bdbac Mon Sep 17 00:00:00 2001 From: rmheuer <63077980+rmheuer@users.noreply.github.com> Date: Sat, 27 Apr 2024 10:46:45 -0500 Subject: [PATCH] [Robot] reindex --- .../com/swrobotics/robot/RobotContainer.java | 7 +- .../com/swrobotics/robot/config/NTData.java | 5 +- .../subsystems/amp/AmpArm2Subsystem.java | 2 +- .../subsystems/speaker/IndexerSubsystem.java | 144 ++++++++++++++---- 4 files changed, 124 insertions(+), 34 deletions(-) diff --git a/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java b/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java index 3bd38b2..48a4244 100644 --- a/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java +++ b/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java @@ -218,12 +218,17 @@ public void disabledInit() { lights.disabledInit(); String song = musicSelector.get(); - if (hasDoneFirstInit && !song.equals("None")) { + boolean wasEStopped = DriverStation.isEStopped(); + + if (hasDoneFirstInit && (!song.equals("None") || wasEStopped)) { if (song.equals("Random")) { List songs = MusicSubsystem.getAvailableSongs(); song = songs.get((int) (Math.random() * songs.size())); } + if (wasEStopped) + song = "music" + File.separator + "abort.chrp"; + CommandScheduler.getInstance().schedule(musicCommand = new PlaySongCommand(music, song)); } hasDoneFirstInit = true; diff --git a/Robot/src/main/java/com/swrobotics/robot/config/NTData.java b/Robot/src/main/java/com/swrobotics/robot/config/NTData.java index c0bb3aa..3d14838 100644 --- a/Robot/src/main/java/com/swrobotics/robot/config/NTData.java +++ b/Robot/src/main/java/com/swrobotics/robot/config/NTData.java @@ -59,10 +59,9 @@ public final class NTData { public static final NTEntry TRAP_FINGER_HOLD_ANGLE = new NTDouble("Trap Finger/Hold Angle (deg)", 0).setPersistent(); public static final NTEntry TRAP_FINGER_RELEASE_ANGLE = new NTDouble("Trap Finger/Release Angle (deg)", 90).setPersistent(); - public static final NTEntry INDEXER_SIDES_TAKE_SPEED = new NTDouble("Indexer/Sides/Take Speed (pct)", 0.9).setPersistent(); - public static final NTEntry INDEXER_SIDES_FEED_SPEED = new NTDouble("Indexer/Sides/Feed Speed (pct)", 0.7).setPersistent(); - public static final NTEntry INDEXER_TOP_TAKE_SPEED = new NTDouble("Indexer/Top/Take Speed (pct)", 0.3).setPersistent(); + public static final NTEntry INDEXER_TOP_TAKE_SPEED = new NTDouble("Indexer/Top/Take Speed (pct)", 0.7).setPersistent(); public static final NTEntry INDEXER_TOP_FEED_SPEED = new NTDouble("Indexer/Top/Feed Speed (pct)", 0.6).setPersistent(); + public static final NTEntry INDEXER_TOP_INDEX_SPEED = new NTDouble("Indexer/Top/Index Speed (pct)", 0.4).setPersistent(); public static final NTEntry INDEXER_FEED_ADDITIONAL_TIME = new NTDouble("Indexer/Feed Additional Time (sec)", 0.0).setPersistent(); public static final NTEntry INDEXER_HAS_PIECE = new NTBoolean("Indexer/Has Piece", false); diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/amp/AmpArm2Subsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/amp/AmpArm2Subsystem.java index bae9205..1899b28 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/amp/AmpArm2Subsystem.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/amp/AmpArm2Subsystem.java @@ -28,7 +28,7 @@ public final class AmpArm2Subsystem extends SubsystemBase { private static final double motorToArmRatio = 50; private static final double encoderToArmRatio = 2; - private static final double cancoderOffset = 0.317627; + private static final double cancoderOffset = 0.163330; private Position targetPos; diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java index d76e10b..ccac99f 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java @@ -1,34 +1,31 @@ package com.swrobotics.robot.subsystems.speaker; +import com.swrobotics.lib.net.NTDouble; +import com.swrobotics.lib.net.NTString; import com.swrobotics.robot.config.IOAllocation; import com.swrobotics.robot.config.NTData; -import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.motorcontrol.PWMTalonSRX; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.motorcontrol.VictorSP; import edu.wpi.first.wpilibj2.command.SubsystemBase; public final class IndexerSubsystem extends SubsystemBase { - private final PWMTalonSRX sidesMotor = new PWMTalonSRX(IOAllocation.RIO.PWM_INDEXER_SIDES_MOTOR); private final VictorSP topMotor = new VictorSP(IOAllocation.RIO.PWM_INDEXER_TOP_MOTOR); private final DigitalInput beamBreak = new DigitalInput(IOAllocation.RIO.DIO_INDEXER_BEAM_BREAK); - private final Debouncer beamBreakDebounce = new Debouncer(0.1, Debouncer.DebounceType.kRising); +// private final Debouncer beamBreakDebounce = new Debouncer(0.1, Debouncer.DebounceType.kRising); private final IntakeSubsystem intake; private boolean feedToShooter; - private boolean reverseTake, waitingForPiece; - private boolean hadPiece; +// private boolean reverseTake, waitingForPiece; +// private boolean hadPiece; +// private boolean hasReindexed; public IndexerSubsystem(IntakeSubsystem intake) { this.intake = intake; - sidesMotor.setInverted(true); - feedToShooter = false; - reverseTake = false; - waitingForPiece = false; hadPiece = false; } @@ -38,43 +35,132 @@ public void setFeedToShooter(boolean feedToShooter) { public boolean hasPiece() { // Beam break is true when not blocked - return !beamBreakDebounce.calculate(beamBreak.get()); + return !beamBreak.get(); +// return !beamBreakDebounce.calculate(beamBreak.get()); + } + + // Idle state forward: feed in from intake + // When detect note, switch to reverse state + // Reverse state: run in reverse + // When stopped detecting note, switch to index state + // Index state: run forward + // When detect note, switch to stopped state + // Stopped state: no run + + // When not intaking and not reverse intaking, switch to stopped state + + private enum State { + IDLE, + FEED_FORWARD, + FEED_REVERSE, + INDEX, + HOLD + } + private State state = State.IDLE; + private boolean hadPiece = false; + private boolean reverseIntaking = false; + private double stateStartTimestamp = Timer.getFPGATimestamp(); + + private void switchState(State state) { + stateStartTimestamp = Timer.getFPGATimestamp(); + this.state = state; } + NTString stateDebug = new NTString("Debug/Indexer State", ""); + NTDouble stateTime = new NTDouble("Debug/Indexer State Time", 0); + @Override public void periodic() { boolean hasPiece = hasPiece(); NTData.INDEXER_HAS_PIECE.set(hasPiece); + boolean intaking = intake.getState() == IntakeSubsystem.State.INTAKE; + + boolean anyRequest = intaking || reverseIntaking; + if (anyRequest) + switchState(state); // Prevent timeout while requesting + + // Time out if something goes wrong + double time = Timer.getFPGATimestamp() - stateStartTimestamp; + stateTime.set(time); + stateDebug.set(state.toString()); + if (!anyRequest && time > 1) { + switchState(State.IDLE); + } - double sides = 0, top = 0; + double top = 0; if (feedToShooter) { - sides = NTData.INDEXER_SIDES_FEED_SPEED.get(); top = NTData.INDEXER_TOP_FEED_SPEED.get(); - } else if (reverseTake && waitingForPiece) { - sides = -NTData.INDEXER_SIDES_FEED_SPEED.get(); - top = -NTData.INDEXER_TOP_FEED_SPEED.get(); - - // Stop reversing once the piece has gone fully past the beam break - // Then the case below will run indexer forward to get it to normal position - if (!hasPiece && hadPiece) - waitingForPiece = false; - } else if ((intake.getState() == IntakeSubsystem.State.INTAKE || reverseTake) && !hasPiece) { - sides = NTData.INDEXER_SIDES_TAKE_SPEED.get(); - top = NTData.INDEXER_TOP_TAKE_SPEED.get(); + } else { + switch (state) { + case IDLE -> { + top = 0; + if (reverseIntaking) + switchState(State.FEED_REVERSE); + if (intaking) + switchState(State.FEED_FORWARD); + } + case FEED_FORWARD -> { + top = NTData.INDEXER_TOP_TAKE_SPEED.get(); + if (hasPiece) + switchState(State.FEED_REVERSE); + } + case FEED_REVERSE -> { + top = -NTData.INDEXER_TOP_TAKE_SPEED.get(); + if (!hasPiece && hadPiece) // Stopped seeing the note + switchState(State.INDEX); + } + case INDEX -> { + top = NTData.INDEXER_TOP_INDEX_SPEED.get(); + if (hasPiece) + switchState(State.HOLD); + } + case HOLD -> { + top = 0; + if (!anyRequest) + switchState(State.IDLE); + } + } } - sidesMotor.set(sides); topMotor.set(top); - hadPiece = hasPiece; + +// boolean hasPiece = hasPiece(); +// NTData.INDEXER_HAS_PIECE.set(hasPiece); +// +// boolean intaking = intake.getState() == IntakeSubsystem.State.INTAKE; +// +// double sides = 0, top = 0; +// if (feedToShooter) { +// sides = NTData.INDEXER_SIDES_FEED_SPEED.get(); +// top = NTData.INDEXER_TOP_FEED_SPEED.get(); +// } else if (reverseTake && waitingForPiece) { +// sides = -NTData.INDEXER_SIDES_FEED_SPEED.get(); +// top = -NTData.INDEXER_TOP_FEED_SPEED.get(); +// +// // Stop reversing once the piece has gone fully past the beam break +// // Then the case below will run indexer forward to get it to normal position +// if (!hasPiece && hadPiece) +// waitingForPiece = false; +// } else if ((intaking || reverseTake) && !hasPiece) { +// sides = NTData.INDEXER_SIDES_TAKE_SPEED.get(); +// top = NTData.INDEXER_TOP_TAKE_SPEED.get(); +// } +// +// sidesMotor.set(sides); +// topMotor.set(top); +// +// hadPiece = hasPiece; } public void beginReverse() { - reverseTake = true; - waitingForPiece = true; + reverseIntaking = true; +// reverseTake = true; +// waitingForPiece = true; } public void endReverse() { - reverseTake = false; + reverseIntaking = false; +// reverseTake = false; } }