Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

changed the manipulator logic to compesate for beam placement changes #61

Merged
merged 2 commits into from
Feb 8, 2025
Merged

changed the manipulator logic to compesate for beam placement changes #61

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
737038d
changed the manipulator logic to compesate for beam placement changes
elvizer Feb 8, 2025
def58f8
fixed some beam logic
elvizer Feb 8, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -300,8 +300,8 @@ public static class ManipulatorConstants {
public static final int leftMotorId = 14;
public static final int rightMotorId = 15;

public static final int beamPort = 3;
public static final int switchPort = 4;
public static final int coralBeam = 3;
public static final int algaeBeam = 4;

public static final AngularVelocity feedSpeed = RadiansPerSecond.of(1);

Expand Down
94 changes: 49 additions & 45 deletions src/main/java/frc/robot/subsystems/Manipulator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,15 +36,13 @@
import java.util.function.Consumer;

public class Manipulator extends AdvancedSubsystem {
private final DigitalInput _beam = new DigitalInput(ManipulatorConstants.beamPort);
private final DigitalInput _limitSwitch = new DigitalInput(ManipulatorConstants.switchPort);
private final DigitalInput _coralBeam = new DigitalInput(ManipulatorConstants.coralBeam);
private final DigitalInput _algaeBeam = new DigitalInput(ManipulatorConstants.algaeBeam);

private final BooleanEvent _beamOnFalse =
new BooleanEvent(CommandScheduler.getInstance().getDefaultButtonLoop(), this::getBeam)
.falling();
private final BooleanEvent _switchOnTrue =
new BooleanEvent(CommandScheduler.getInstance().getDefaultButtonLoop(), this::getSwitch)
.rising();
private final BooleanEvent _coralEvent =
new BooleanEvent(CommandScheduler.getInstance().getDefaultButtonLoop(), this::getCoralBeam);
private final BooleanEvent _algaeEvent =
new BooleanEvent(CommandScheduler.getInstance().getDefaultButtonLoop(), this::getAlgaeBeam);

private final Consumer<Piece> _currentPieceSetter;

Expand All @@ -64,11 +62,11 @@ public class Manipulator extends AdvancedSubsystem {

private final StatusSignal<AngularVelocity> _feedVelocityGetter = _leftMotor.getVelocity();

private DIOSim _beamSim;
private DIOSim _limitSwitchSim;
private DIOSim _coralBeamSim;
private DIOSim _algaeBeamSim;

private BooleanEntry _beamSimValue;
private BooleanEntry _limitSwitchSimValue;
private BooleanEntry _coralBeamSimValue;
private BooleanEntry _algaeBeamSimValue;

public Manipulator(Consumer<Piece> currentPieceSetter) {
setDefaultCommand(idle());
Expand All @@ -92,11 +90,11 @@ public Manipulator(Consumer<Piece> currentPieceSetter) {
FaultLogger.register(_rightMotor);

if (Robot.isSimulation()) {
_beamSim = new DIOSim(_beam);
_limitSwitchSim = new DIOSim(_limitSwitch);
_coralBeamSim = new DIOSim(_coralBeam);
_algaeBeamSim = new DIOSim(_algaeBeam);

_beamSimValue = Tuning.entry("/Tuning/Manipulator Beam", false);
_limitSwitchSimValue = Tuning.entry("/Tuning/Manipulator Limit Switch", false);
_coralBeamSimValue = Tuning.entry("/Tuning/Manipulator Coral Beam", false);
_algaeBeamSimValue = Tuning.entry("/Tuning/Manipulator Algae Beam", false);

_leftFlywheelSim =
new FlywheelSim(
Expand Down Expand Up @@ -145,15 +143,14 @@ private void startSimThread() {
_simNotifier.startPeriodic(1 / Constants.simUpdateFrequency.in(Hertz));
}

@Logged(name = "Beam")
public boolean getBeam() {
return !_beam.get();
@Logged(name = "Coral Beam")
public boolean getCoralBeam() {
return !_coralBeam.get();
}

@Logged(name = "Switch")
public boolean getSwitch() {
// TODO: might have two switches
return _limitSwitch.get();
@Logged(name = "Algae Beam")
public boolean getAlgaeBeam() {
return !_algaeBeam.get();
}

@Logged(name = "Speed")
Expand All @@ -169,23 +166,23 @@ private Command setSpeed(double speed) {
});
}

/** Sets the current piece when the beam changes from true to false. */
private Command watchBeam(Piece piece) {
/** Sets the current piece when the coral beam changes state. */
private Command watchCoralBeam(Piece piece, boolean onTrue) {
BooleanEvent coralEvent = onTrue ? _coralEvent.rising() : _coralEvent.falling();

return Commands.run(
() -> {
if (_beamOnFalse.getAsBoolean()) {
_currentPieceSetter.accept(piece);
}
if (coralEvent.getAsBoolean()) _currentPieceSetter.accept(piece);
});
}

/** Sets the current piece when the limit switch changes from false to true. */
private Command watchSwitch(Piece piece) {
/** Sets the current piece when the algae beam changes state. */
private Command watchAlgaeBeam(Piece piece, boolean onTrue) {
BooleanEvent algaeEvent = onTrue ? _algaeEvent.rising() : _algaeEvent.falling();

return Commands.run(
() -> {
if (_switchOnTrue.getAsBoolean()) {
_currentPieceSetter.accept(piece);
}
if (algaeEvent.getAsBoolean()) _currentPieceSetter.accept(piece);
});
}

Expand All @@ -199,7 +196,7 @@ public Command holdCoral() {
return run(() -> {
_leftMotor.setControl(_feedVoltageSetter.withOutput(0));
})
.alongWith(watchBeam(Piece.NONE))
.alongWith(watchCoralBeam(Piece.NONE, false))
.withName("Hold Coral");
}

Expand All @@ -209,29 +206,30 @@ public Command holdAlgae() {
_leftMotor.setControl(
_feedVoltageSetter.withOutput(ManipulatorConstants.holdAlgaeVoltage));
})
.alongWith(watchBeam(Piece.NONE))
.alongWith(watchAlgaeBeam(Piece.NONE, false))
.withName("Hold Algae");
}

/** General intake. */
public Command intake() {
return setSpeed(ManipulatorConstants.feedSpeed.in(RadiansPerSecond))
.alongWith(watchBeam(Piece.CORAL), watchSwitch(Piece.ALGAE))
.alongWith(watchCoralBeam(Piece.CORAL, true), watchAlgaeBeam(Piece.ALGAE, true))
.withName("Intake");
}

/** General outtake. */
public Command outtake() {
return setSpeed(-ManipulatorConstants.feedSpeed.in(RadiansPerSecond))
.alongWith(watchBeam(Piece.NONE))
.alongWith(watchCoralBeam(Piece.NONE, false), watchAlgaeBeam(Piece.NONE, false))
.withName("Outtake");
}

/** Passoff from the serializer. */
public Command passoff() {
return setSpeed(0)
.until(this::getBeam)
.andThen(Commands.runOnce(() -> _currentPieceSetter.accept(Piece.CORAL)))
.alongWith(watchCoralBeam(Piece.CORAL, false))
.until(() -> getCurrentPiece() == Piece.CORAL)
.finallyDo(this::pulse)
.withName("Passoff");
}

Expand All @@ -240,6 +238,12 @@ public Command inversePassoff() {
return setSpeed(0).withName("Inverse Passoff");
}

/** Pulse the manipulator until coral triggers the beam */
public Command pulse() {
return setSpeed(ManipulatorConstants.feedSpeed.div(2).in(RadiansPerSecond))
.until(() -> _coralEvent.rising().getAsBoolean());
}

@Override
public void periodic() {
super.periodic();
Expand All @@ -249,21 +253,21 @@ public void periodic() {
public void simulationPeriodic() {
super.simulationPeriodic();

_beamSim.setValue(!_beamSimValue.get());
_limitSwitchSim.setValue(_limitSwitchSimValue.get());
_coralBeamSim.setValue(!_coralBeamSimValue.get());
_algaeBeamSim.setValue(!_algaeBeamSimValue.get());
}

@Override
public void close() {
_beam.close();
_limitSwitch.close();
_coralBeam.close();
_algaeBeam.close();

_leftMotor.close();
_rightMotor.close();

_simNotifier.close();

_beamSimValue.close();
_limitSwitchSimValue.close();
_coralBeamSimValue.close();
_algaeBeamSimValue.close();
}
}