Skip to content

Commit

Permalink
Move indexer stuff to its own subsystem.
Browse files Browse the repository at this point in the history 
Also clean up a bunch of commands
  • Loading branch information
@Krylez
Krylez committed Feb 23, 2024
1 parent d9f0523 commit 41ea4f7
Show file tree
Hide file tree
Showing 15 changed files with 371 additions and 148 deletions.
87 changes: 69 additions & 18 deletions src/main/java/com/cyberknights4911/robot2024/Robot2024.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,8 @@
import com.cyberknights4911.robot2024.collect.CollectIOSim;
import com.cyberknights4911.robot2024.control.ControllerBinding;
import com.cyberknights4911.robot2024.drive.ModuleIOSparkFlex;
import com.cyberknights4911.robot2024.indexer.Indexer;
import com.cyberknights4911.robot2024.indexer.IndexerIO;
import com.cyberknights4911.robot2024.shooter.Shooter;
import com.cyberknights4911.robot2024.shooter.ShooterIO;
import com.cyberknights4911.robot2024.shooter.ShooterIOSim;
Expand All @@ -34,14 +36,17 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import org.littletonrobotics.junction.LoggedRobot;

/** The main class for the 2024 robot to be named at a future date. */
public final class Robot2024 implements RobotContainer {
private final Climb climb;
private final Collect collect;
private final Indexer indexer;
private final Shooter shooter;
private final Drive drive;
private final Constants constants;
Expand All @@ -53,6 +58,7 @@ public Robot2024() {
burnManager = new SparkBurnManager(constants);
climb = createClimb();
collect = createCollect();
indexer = createIndexer();
shooter = createShooter();
drive = createDrive();

Expand All @@ -68,6 +74,7 @@ private void configureControls() {
binding.supplierFor(StickAction.STRAFE),
binding.supplierFor(StickAction.ROTATE)));

// TODO: Maybe remove this or make it harder to do by accident.
binding.triggersFor(ButtonAction.ZeroGyro).onTrue(drive.zeroPoseToCurrentRotation());

binding
Expand All @@ -91,6 +98,8 @@ private void configureControls() {
binding.supplierFor(StickAction.FORWARD),
binding.supplierFor(StickAction.STRAFE),
Math.PI / 2));

// TODO: combine with auto-aim.
binding
.triggersFor(ButtonAction.SpeakerLockOn)
.whileTrue(
Expand All @@ -103,39 +112,41 @@ private void configureControls() {

binding.triggersFor(ButtonAction.StowShooter).onTrue(Commands.none());

binding.triggersFor(ButtonAction.CollectNote).onTrue(Commands.none());
binding.triggersFor(ButtonAction.CollectNote).onTrue(collectNote());

binding.triggersFor(ButtonAction.FireNote).onTrue(Commands.none());

binding.triggersFor(ButtonAction.StowClimber).onTrue(climb.climb(drive));

binding.triggersFor(ButtonAction.ExtendClimber).onTrue(Commands.none());
binding.triggersFor(ButtonAction.ExtendClimber).onTrue(climb.extendClimber());
}

private Command collectNote() {
Command stowShooter = shooter.stowShooter();
Command runIndexer = indexer.runIndexUntilSensor();
Command collectNote =
collect.collectAtTunableSpeed(
() -> {
/* TODO: notify drive team that note is secure (lights & rumble) */
});

return Commands.parallel(stowShooter, runIndexer, collectNote)
.andThen(
() -> {
/* TODO: notify drive team that we're ready to fire (probably rumble only) */
});
}

@Override
public void onRobotPeriodic(LoggedRobot robot) {
binding.checkControllers();

if (GameAlerts.shouldAlert(GameAlerts.Endgame1)) {
CommandScheduler.getInstance()
.schedule(
Commands.runOnce(() -> binding.setDriverRumble(true))
.withTimeout(1.5)
.andThen(() -> binding.setDriverRumble(false))
.withTimeout(1.0));
CommandScheduler.getInstance().schedule(rumbleLongOnce());
}

if (GameAlerts.shouldAlert(GameAlerts.Endgame2)) {
CommandScheduler.getInstance()
.schedule(
Commands.runOnce(() -> binding.setDriverRumble(true))
.withTimeout(1.0)
.andThen(() -> binding.setDriverRumble(false))
.withTimeout(0.5)
.andThen(() -> binding.setDriverRumble(true))
.withTimeout(1.0)
.andThen(() -> binding.setDriverRumble(false))
.withTimeout(0.5));
CommandScheduler.getInstance().schedule(rumbleMediumTwice());
}
}

Expand Down Expand Up @@ -171,6 +182,17 @@ private Collect createCollect() {
}
}

private Indexer createIndexer() {
switch (constants.mode()) {
case SIM:
return new Indexer(SimRobot2024Constants.INDEXER_CONSTANTS, new IndexerIO() {});
case REAL:
case REPLAY:
default:
return new Indexer(Robot2024Constants.INDEXER_CONSTANTS, new IndexerIO() {});
}
}

private Shooter createShooter() {
switch (constants.mode()) {
case SIM:
Expand Down Expand Up @@ -228,6 +250,35 @@ private Drive createDrive() {
new ModuleIO() {});
}
}

private ParallelRaceGroup rumbleLongOnce() {
return Commands.runOnce(() -> binding.setDriverRumble(true))
.withTimeout(1.5)
.andThen(() -> binding.setDriverRumble(false))
.withTimeout(1.0);
}

private ParallelRaceGroup rumbleMediumTwice() {
return Commands.runOnce(() -> binding.setDriverRumble(true))
.withTimeout(1.0)
.andThen(() -> binding.setDriverRumble(false))
.withTimeout(0.5)
.andThen(() -> binding.setDriverRumble(true))
.withTimeout(1.0)
.andThen(() -> binding.setDriverRumble(false))
.withTimeout(0.5);
}

private ParallelRaceGroup rumbleQuickTwice() {
return Commands.runOnce(() -> binding.setDriverRumble(true))
.withTimeout(0.5)
.andThen(() -> binding.setDriverRumble(false))
.withTimeout(0.25)
.andThen(() -> binding.setDriverRumble(true))
.withTimeout(0.5)
.andThen(() -> binding.setDriverRumble(false))
.withTimeout(0.25);
}
}

// TODO:
Expand Down
12 changes: 10 additions & 2 deletions src/main/java/com/cyberknights4911/robot2024/Robot2024Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@
import com.cyberknights4911.robot2024.climb.ClimbConstantsBuilder;
import com.cyberknights4911.robot2024.collect.CollectConstants;
import com.cyberknights4911.robot2024.collect.CollectConstantsBuilder;
import com.cyberknights4911.robot2024.indexer.IndexerConstants;
import com.cyberknights4911.robot2024.indexer.IndexerConstantsBuilder;
import com.cyberknights4911.robot2024.shooter.ShooterConstants;
import com.cyberknights4911.robot2024.shooter.ShooterConstantsBuilder;
import com.cyberknights4911.util.FeedForwardValues;
Expand Down Expand Up @@ -106,13 +108,19 @@ private Robot2024Constants() {}
public static CollectConstants COLLECT_CONSTANTS =
CollectConstantsBuilder.builder()
.motorCollectId(11)
.motorGuideId(12)
.sensorId(0)
.solenoidLeftId(0)
.solenoidRightId(0)
.collectGearRatio(24.0 / 18.0)
.collectFeedBackValues(new PidValues(0, 0, 0))
.guideFeedBackValues(new PidValues(0, 0, 0))
.build();

public static IndexerConstants INDEXER_CONSTANTS =
IndexerConstantsBuilder.builder()
.motorId(31)
.sensorId(0)
.speed(100)
.feedBackValues(new PidValues(0, 0, 0))
.build();

public static ShooterConstants SHOOTER_CONSTANTS =
Expand Down
12 changes: 10 additions & 2 deletions src/main/java/com/cyberknights4911/robot2024/SimRobot2024Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@
import com.cyberknights4911.robot2024.climb.ClimbConstantsBuilder;
import com.cyberknights4911.robot2024.collect.CollectConstants;
import com.cyberknights4911.robot2024.collect.CollectConstantsBuilder;
import com.cyberknights4911.robot2024.indexer.IndexerConstants;
import com.cyberknights4911.robot2024.indexer.IndexerConstantsBuilder;
import com.cyberknights4911.robot2024.shooter.ShooterConstants;
import com.cyberknights4911.robot2024.shooter.ShooterConstantsBuilder;
import com.cyberknights4911.util.FeedForwardValues;
Expand Down Expand Up @@ -99,13 +101,19 @@ private SimRobot2024Constants() {}
public static CollectConstants COLLECT_CONSTANTS =
CollectConstantsBuilder.builder()
.motorCollectId(11)
.motorGuideId(12)
.sensorId(0)
.solenoidLeftId(0)
.solenoidRightId(0)
.collectGearRatio(24.0 / 18.0)
.collectFeedBackValues(new PidValues(0, 0, 0))
.guideFeedBackValues(new PidValues(0, 0, 0))
.build();

public static IndexerConstants INDEXER_CONSTANTS =
IndexerConstantsBuilder.builder()
.motorId(31)
.sensorId(0)
.speed(100)
.feedBackValues(new PidValues(0, 0, 0))
.build();

public static ShooterConstants SHOOTER_CONSTANTS =
Expand Down
47 changes: 16 additions & 31 deletions src/main/java/com/cyberknights4911/robot2024/collect/Collect.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,18 +24,14 @@

public class Collect extends SubsystemBase {
private static final LoggedTunableNumber beamThreshold =
new LoggedTunableNumber("Collect/beamThreshold");
new LoggedTunableNumber("Collect/BeamThreshold");
private static final LoggedTunableNumber ejectSpeed =
new LoggedTunableNumber("Collect/EjectVelocityRPM");
private static final LoggedTunableNumber collectSpeed =
new LoggedTunableNumber("Collect/CollectVelocityRPM");
private static final LoggedTunableNumber feedShooterSpeed =
new LoggedTunableNumber("Collect/FeedShooterVelocityRPM");
private static final LoggedTunableNumber collectKp = new LoggedTunableNumber("Collect/CollectKp");
private static final LoggedTunableNumber collectKd = new LoggedTunableNumber("Collect/CollectKd");
private static final LoggedTunableNumber ejectTime = new LoggedTunableNumber("Collect/EjectTime");
private static final LoggedTunableNumber guideKp = new LoggedTunableNumber("Collect/GuideKp");
private static final LoggedTunableNumber guideKd = new LoggedTunableNumber("Collect/GuideKd");

// Measured in OnShape
private static final Translation2d SEGMENT_1_START = new Translation2d(3.267, 15.613);
Expand Down Expand Up @@ -68,14 +64,10 @@ public Collect(CollectConstants constants, CollectIO collectIO) {
this.collectIO = collectIO;
collectKp.initDefault(constants.collectFeedBackValues().kP());
collectKd.initDefault(constants.collectFeedBackValues().kD());
feedShooterSpeed.initDefault(constants.feedShooterSpeed());
collectSpeed.initDefault(constants.collectSpeed());
ejectSpeed.initDefault(constants.ejectSpeed());
ejectTime.initDefault(constants.ejectTime());
guideKp.initDefault(constants.guideFeedBackValues().kP());
guideKd.initDefault(constants.guideFeedBackValues().kP());
collectIO.configureCollectPID(collectKp.get(), 0.0, collectKd.get());
collectIO.configureGuidePID(guideKp.get(), 0.0, guideKd.get());

mechanism = new Mechanism2d(28.0, 28.0);
// Where the collector is attached to the frame
Expand Down Expand Up @@ -113,18 +105,6 @@ public void runCollectVelocity(double velocityRpm) {
Logger.recordOutput("Collect/SetpointRPM", velocityRpm);
}

/** Run guide closed loop at the specified velocity. */
public void runGuideVelocity(double velocityRpm) {
var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRpm);
collectIO.setGuideVelocity(velocityRadPerSec);

Logger.recordOutput("Collect/GuideSetpointRPM", velocityRpm);
}

public void stopGuide() {
collectIO.stopGuide();
}

public void extendCollecter() {
collectIO.setCollecterPosition(true);
}
Expand All @@ -142,10 +122,6 @@ public void periodic() {
collectIO.configureCollectPID(collectKp.get(), 0.0, collectKd.get());
}

if (guideKp.hasChanged(hashCode()) || guideKd.hasChanged(hashCode())) {
collectIO.configureGuidePID(guideKp.get(), 0.0, guideKd.get());
}

if (inputs.leftSolenoid && inputs.rightSolenoid) {
segment1.setAngle(SEGMENT_1_END_ANGLE);
segment2.setAngle(SEGMENT_2_END_ANGLE);
Expand All @@ -160,7 +136,6 @@ public void periodic() {

if (DriverStation.isDisabled()) {
stopCollector();
stopGuide();
}
}

Expand Down Expand Up @@ -204,6 +179,21 @@ private Command collectAtSpeed(LoggedTunableNumber desiredSpeed) {
this);
}

/**
* Creates a command that runs the collector at the proper collector speed. The collector will
* stay running after this command ends, but the provided callback will be notified when the
* gamepiece reaches the collector's sensor.
*
* @param sensorTrippedCallback will notify when the gamepiece reaches the sensor. At this point,
* the gamepiece is secure in the robot and the driver should be able to move the robot freely
* while the gamepiece moves through the robot.
*/
public Command collectAtTunableSpeed(Runnable sensorTrippedCallback) {
return Commands.runOnce(() -> runCollectVelocity(collectSpeed.get()), this)
.until(this::isBeamBreakBlocked)
.andThen(() -> sensorTrippedCallback.run());
}

/**
* Creates a command that runs the collector at the proper collector speed, but stops when the
* gamepiece reaches the sensor.
Expand All @@ -214,11 +204,6 @@ public Command collectGamePieceAndStop() {
.andThen(this::stopCollector, this);
}

/** Creates a command that runs the collector in the mode for handing gamepieces to the shooter */
public Command handGamePieceToShooter() {
return collectAtSpeed(feedShooterSpeed);
}

/** Creates a command for ejecting gamepieces backwards, out of the collector. */
public Command ejectGamePiece() {
return collectAtSpeed(ejectSpeed)
Expand Down
5 changes: 1 addition & 4 deletions src/main/java/com/cyberknights4911/robot2024/collect/CollectConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,11 @@
@RecordBuilder
public record CollectConstants(
int motorCollectId,
int motorGuideId,
int sensorId,
int solenoidLeftId,
int solenoidRightId,
double collectGearRatio,
double ejectTime,
double ejectSpeed,
double collectSpeed,
double feedShooterSpeed,
PidValues collectFeedBackValues,
PidValues guideFeedBackValues) {}
PidValues collectFeedBackValues) {}
11 changes: 1 addition & 10 deletions src/main/java/com/cyberknights4911/robot2024/collect/CollectIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,17 +41,8 @@ public default void setCollectVelocity(double velocityRadPerSec) {}
/** Stop collector in open loop. */
public default void stopCollector() {}

/** Run guide closed loop at the specified velocity. */
public default void setGuideVelocity(double velocityRadPerSec) {}

/** Stop guide in open loop. */
public default void stopGuide() {}

public default void setCollecterPosition(boolean extended) {}

/** Set collect velocity PID constants. */
/** Set velocity PID constants. */
public default void configureCollectPID(double kP, double kI, double kD) {}

/** Set guide velocity PID constants. */
public default void configureGuidePID(double kP, double kI, double kD) {}
}
Loading

0 comments on commit 41ea4f7

Please sign in to comment.