Skip to content

Commit

Permalink
[Robot] reindex
Browse files Browse the repository at this point in the history
  • Loading branch information
@rmheuer
rmheuer committed Apr 27, 2024
1 parent 3ef4122 commit 1df97bc
Show file tree
Hide file tree
Showing 4 changed files with 124 additions and 34 deletions.
7 changes: 6 additions & 1 deletion Robot/src/main/java/com/swrobotics/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<String> 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;
Expand Down
5 changes: 2 additions & 3 deletions Robot/src/main/java/com/swrobotics/robot/config/NTData.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,10 +59,9 @@ public final class NTData {
public static final NTEntry<Double> TRAP_FINGER_HOLD_ANGLE = new NTDouble("Trap Finger/Hold Angle (deg)", 0).setPersistent();
public static final NTEntry<Double> TRAP_FINGER_RELEASE_ANGLE = new NTDouble("Trap Finger/Release Angle (deg)", 90).setPersistent();

public static final NTEntry<Double> INDEXER_SIDES_TAKE_SPEED = new NTDouble("Indexer/Sides/Take Speed (pct)", 0.9).setPersistent();
public static final NTEntry<Double> INDEXER_SIDES_FEED_SPEED = new NTDouble("Indexer/Sides/Feed Speed (pct)", 0.7).setPersistent();
public static final NTEntry<Double> INDEXER_TOP_TAKE_SPEED = new NTDouble("Indexer/Top/Take Speed (pct)", 0.3).setPersistent();
public static final NTEntry<Double> INDEXER_TOP_TAKE_SPEED = new NTDouble("Indexer/Top/Take Speed (pct)", 0.7).setPersistent();
public static final NTEntry<Double> INDEXER_TOP_FEED_SPEED = new NTDouble("Indexer/Top/Feed Speed (pct)", 0.6).setPersistent();
public static final NTEntry<Double> INDEXER_TOP_INDEX_SPEED = new NTDouble("Indexer/Top/Index Speed (pct)", 0.4).setPersistent();
public static final NTEntry<Double> INDEXER_FEED_ADDITIONAL_TIME = new NTDouble("Indexer/Feed Additional Time (sec)", 0.0).setPersistent();
public static final NTEntry<Boolean> INDEXER_HAS_PIECE = new NTBoolean("Indexer/Has Piece", false);

Expand Down
2 changes: 1 addition & 1 deletion Robot/src/main/java/com/swrobotics/robot/subsystems/amp/AmpArm2Subsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
144 changes: 115 additions & 29 deletions Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java
View file
Original file line number Diff line number Diff line change
@@ -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;
}

Expand All @@ -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;
}
}

0 comments on commit 1df97bc

Please sign in to comment.