Skip to content

Commit

Permalink
Merge branch 'main' of https://github.com/team581/2024-offseason-bot
Browse files Browse the repository at this point in the history
  • Loading branch information
@jordanjcoderman
jordanjcoderman committed Sep 25, 2024
2 parents 30aac87 + 846915b commit 2a10c74
Show file tree
Hide file tree
Showing 9 changed files with 157 additions and 59 deletions.
37 changes: 28 additions & 9 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import dev.doglog.DogLog;
import dev.doglog.DogLogOptions;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
Expand Down Expand Up @@ -32,7 +33,7 @@ public class Robot extends TimedRobot {
private final ArmSubsystem arm = new ArmSubsystem(hardware.armLeft, hardware.armRight);
private final IntakeSubsystem intake =
new IntakeSubsystem(hardware.intakeMain, hardware.intakeCenteringMotor);
private final SwerveSubsystem swerve = new SwerveSubsystem(hardware.driverController);
private final SwerveSubsystem swerve = new SwerveSubsystem();
private final ImuSubsystem imu = new ImuSubsystem(swerve.drivetrainPigeon);
private final Autos autos = new Autos();
private final RobotCommands robotCommands = new RobotCommands(null);
Expand Down Expand Up @@ -132,37 +133,55 @@ public void testPeriodic() {}
public void testExit() {}

private void configureBindings() {
swerve.setDefaultCommand(swerve.run(() -> {
if (DriverStation.isTeleop()) {
swerve.driveTeleop(hardware.driverController.getLeftX(),hardware.driverController.getLeftY() , hardware.driverController.getRightX());
}
}));

hardware
.driverController
.rightTrigger()
.onTrue(robotCommands.confirmShotCommand())
.onFalse(robotCommands.stowCommand());
.onFalse(robotCommands.stopShootingCommand());
hardware
.driverController
.leftTrigger()
.onTrue(robotCommands.intakeCommand())
.onFalse(robotCommands.stowCommand());
// TODO: Two of these bindings use right bumper
hardware
.driverController
.rightBumper()
.onTrue(robotCommands.feedingCommand())
.onFalse(robotCommands.stowCommand());
.onTrue(robotCommands.passCommand())
.onFalse(robotCommands.stopShootingCommand());
hardware
.driverController
.rightBumper()
.onTrue(robotCommands.passCommand())
.onFalse(robotCommands.stowCommand());
.leftBumper()
.onTrue(robotCommands.feedingCommand())
.onFalse(robotCommands.stopShootingCommand());

hardware
.operatorController
.rightTrigger()
.onTrue(robotCommands.ampCommand())
.onFalse(robotCommands.stowCommand());
.onFalse(robotCommands.stopShootingCommand());
hardware
.operatorController
.leftTrigger()
.onTrue(robotCommands.subwooferCommand())
.onFalse(robotCommands.stowCommand());
hardware
.operatorController
.x()
.onTrue(robotCommands.outtakeCommand())
.onFalse(robotCommands.stowCommand());
hardware.operatorController.a().onTrue(robotCommands.stowCommand());
hardware.operatorController.povUp().onTrue(robotCommands.climbUpCommand());
hardware.operatorController.povDown().onTrue(robotCommands.climbDownCommand());
hardware
.operatorController
.povLeft()
.onTrue(robotCommands.unjamCommand())
.onFalse(robotCommands.stowCommand());
}
}
4 changes: 1 addition & 3 deletions src/main/java/frc/robot/config/CompConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,6 @@ class CompConfig {
999,
CANIVORE_NAME,
999,
999,
new TalonFXConfiguration()
.withClosedLoopRamps(CLOSED_LOOP_RAMP)
.withOpenLoopRamps(OPEN_LOOP_RAMP)
Expand All @@ -86,8 +85,7 @@ class CompConfig {
centeringMotor -> {
centeringMotor.setSmartCurrentLimit(20);
centeringMotor.burnFlash();
},
new Debouncer(3.0 * 0.02)),
}),
new ArmConfig(
CANIVORE_NAME,
999,
Expand Down
4 changes: 1 addition & 3 deletions src/main/java/frc/robot/config/RobotConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,10 +41,8 @@ public record IntakeConfig(
int mainMotorID,
String mainMotorCanBusName,
int centeringMotorID,
int sensorID,
TalonFXConfiguration mainMotorConfig,
Consumer<CANSparkMax> centeringMotorConfig,
Debouncer debouncer) {}
Consumer<CANSparkMax> centeringMotorConfig) {}

public record ArmConfig(
String canBusName,
Expand Down
15 changes: 0 additions & 15 deletions src/main/java/frc/robot/intake/IntakeSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,17 +3,13 @@
import com.ctre.phoenix6.hardware.TalonFX;
import com.revrobotics.CANSparkMax;
import dev.doglog.DogLog;
import edu.wpi.first.math.filter.Debouncer;
import frc.robot.config.RobotConfig;
import frc.robot.util.scheduling.SubsystemPriority;
import frc.robot.util.state_machines.StateMachine;

public class IntakeSubsystem extends StateMachine<IntakeState> {
private final TalonFX mainMotor;
private final CANSparkMax centeringMotor;
private boolean sensorHasNote = false;
private boolean debouncedSensorHasNote = false;
private final Debouncer debouncer = RobotConfig.get().intake().debouncer();

public IntakeSubsystem(TalonFX mainMotor, CANSparkMax centeringMotor) {
super(SubsystemPriority.INTAKE, IntakeState.IDLE);
Expand All @@ -29,20 +25,11 @@ public void setState(IntakeState newState) {
setStateFromRequest(newState);
}

@Override
protected void collectInputs() {
debouncedSensorHasNote = debouncer.calculate(sensorHasNote);
}

@Override
protected IntakeState getNextState(IntakeState currentState) {
return currentState;
}

public boolean hasNote() {
return debouncedSensorHasNote;
}

@Override
protected void afterTransition(IntakeState newState) {
switch (newState) {
Expand All @@ -65,7 +52,5 @@ public void robotPeriodic() {
DogLog.log("Intake/StatorCurrent", mainMotor.getStatorCurrent().getValueAsDouble());
DogLog.log("Intake/SupplyCurrent", mainMotor.getSupplyCurrent().getValueAsDouble());
DogLog.log("Intake/AppliedVoltage", mainMotor.getMotorVoltage().getValueAsDouble());
DogLog.log("Intake/RawSensor", sensorHasNote);
DogLog.log("Intake/DebouncedSensor", hasNote());
}
}
3 changes: 1 addition & 2 deletions src/main/java/frc/robot/queuer/QueuerState.java
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,7 @@
package frc.robot.queuer;

public enum QueuerState {
IDLE_WITH_GP,
IDLE_NO_GP,
IDLE,
SHOOTING,
INTAKING,
OUTTAKING;
Expand Down
5 changes: 2 additions & 3 deletions src/main/java/frc/robot/queuer/QueuerSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ public class QueuerSubsystem extends StateMachine<QueuerState> {
private final Debouncer debouncer = RobotConfig.get().queuer().debouncer();

public QueuerSubsystem(TalonFX motor, DigitalInput sensor) {
super(SubsystemPriority.QUEUER, QueuerState.IDLE_NO_GP);
super(SubsystemPriority.QUEUER, QueuerState.IDLE);

this.sensor = sensor;
this.motor = motor;
Expand Down Expand Up @@ -47,8 +47,7 @@ public boolean hasNote() {
@Override
protected void afterTransition(QueuerState newState) {
switch (newState) {
case IDLE_NO_GP -> motor.disable();
case IDLE_WITH_GP -> motor.disable();
case IDLE -> motor.disable();
case SHOOTING -> motor.setVoltage(0); // probably like 12
case INTAKING -> motor.setVoltage(0); // probably like 4
case OUTTAKING -> motor.setVoltage(0); // proabably like -4
Expand Down
15 changes: 15 additions & 0 deletions src/main/java/frc/robot/robot_manager/RobotCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,4 +89,19 @@ public Command subwooferCommand() {
return Commands.runOnce(robot::prepareSubwooferRequest, requirements)
.andThen(robot.waitForState(RobotState.IDLE_NO_GP));
}

public Command climbUpCommand() {
return Commands.runOnce(robot::nextClimbStateRequest, requirements)
.andThen(robot.waitForState(RobotState.CLIMBING_2_HANGING));
}

public Command climbDownCommand() {
return Commands.runOnce(robot::previousClimbStateRequest, requirements)
.andThen(robot.waitForState(RobotState.CLIMBING_1_LINEUP));
}

public Command unjamCommand() {
return Commands.runOnce(robot::unjamRequest, requirements)
.andThen(robot.waitForState(RobotState.IDLE_NO_GP));
}
}
32 changes: 12 additions & 20 deletions src/main/java/frc/robot/robot_manager/RobotManager.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ protected RobotState getNextState(RobotState currentState) {
shooter.atGoal() && arm.atGoal() ? RobotState.SUBWOOFER_SCORING : currentState;
case UNJAM -> currentState;
case INTAKING -> queuer.hasNote() ? RobotState.IDLE_WITH_GP : currentState;
case OUTTAKING -> queuer.hasNote() || intake.hasNote() ? currentState : RobotState.IDLE_NO_GP;
case OUTTAKING -> queuer.hasNote() ? currentState : RobotState.IDLE_NO_GP;
};
}

Expand All @@ -87,7 +87,7 @@ protected void afterTransition(RobotState newState) {
arm.setState(ArmState.SUBWOOFER_SHOT);
shooter.setState(ShooterState.SUBWOOFER_SHOT);
intake.setState(IntakeState.IDLE);
queuer.setState(QueuerState.IDLE_WITH_GP);
queuer.setState(QueuerState.IDLE);
}
case SUBWOOFER_SCORING -> {
arm.setState(ArmState.SUBWOOFER_SHOT);
Expand All @@ -99,7 +99,7 @@ protected void afterTransition(RobotState newState) {
arm.setState(ArmState.SPEAKER_SHOT);
shooter.setState(ShooterState.SPEAKER_SHOT);
intake.setState(IntakeState.IDLE);
queuer.setState(QueuerState.IDLE_WITH_GP);
queuer.setState(QueuerState.IDLE);
}
case SPEAKER_SCORING -> {
arm.setState(ArmState.SPEAKER_SHOT);
Expand All @@ -111,7 +111,7 @@ protected void afterTransition(RobotState newState) {
arm.setState(ArmState.AMP);
shooter.setState(ShooterState.AMP);
intake.setState(IntakeState.IDLE);
queuer.setState(QueuerState.IDLE_WITH_GP);
queuer.setState(QueuerState.IDLE);
}
case AMP_SCORING -> {
arm.setState(ArmState.AMP);
Expand All @@ -123,7 +123,7 @@ protected void afterTransition(RobotState newState) {
arm.setState(ArmState.FEEDING);
shooter.setState(ShooterState.FEEDING);
intake.setState(IntakeState.IDLE);
queuer.setState(QueuerState.IDLE_WITH_GP);
queuer.setState(QueuerState.IDLE);
}
case FEEDING_SHOOTING -> {
arm.setState(ArmState.FEEDING);
Expand All @@ -135,7 +135,7 @@ protected void afterTransition(RobotState newState) {
arm.setState(ArmState.PASS);
shooter.setState(ShooterState.PASS);
intake.setState(IntakeState.IDLE);
queuer.setState(QueuerState.IDLE_WITH_GP);
queuer.setState(QueuerState.IDLE);
}
case PASS_SHOOTING -> {
arm.setState(ArmState.PASS);
Expand Down Expand Up @@ -165,25 +165,25 @@ protected void afterTransition(RobotState newState) {
arm.setState(ArmState.CLIMBING_1_LINEUP);
shooter.setState(ShooterState.IDLE_STOPPED);
intake.setState(IntakeState.IDLE);
queuer.setState(QueuerState.IDLE_NO_GP);
queuer.setState(QueuerState.IDLE);
}
case CLIMBING_2_HANGING -> {
arm.setState(ArmState.CLIMBING_2_HANGING);
shooter.setState(ShooterState.IDLE_STOPPED);
intake.setState(IntakeState.IDLE);
queuer.setState(QueuerState.IDLE_NO_GP);
queuer.setState(QueuerState.IDLE);
}
case IDLE_NO_GP -> {
arm.setState(ArmState.IDLE);
shooter.setState(ShooterState.IDLE_STOPPED);
intake.setState(IntakeState.IDLE);
queuer.setState(QueuerState.IDLE_NO_GP);
queuer.setState(QueuerState.IDLE);
}
case IDLE_WITH_GP -> {
arm.setState(ArmState.IDLE);
shooter.setState(ShooterState.IDLE_WARMUP);
intake.setState(IntakeState.IDLE);
queuer.setState(QueuerState.IDLE_WITH_GP);
queuer.setState(QueuerState.IDLE);
}
}
}
Expand Down Expand Up @@ -341,17 +341,9 @@ public void prepareFeedRequest() {
}

public void stopShootingRequest() {
// If we are actively taking a shot, ignore the request to avoid messing up shooting
switch (getState()) {
case SPEAKER_SCORING,
SUBWOOFER_SCORING,
AMP_SCORING,
FEEDING_SHOOTING,
PASS_SHOOTING,
SPEAKER_PREPARE_TO_SCORE,
SUBWOOFER_PREPARE_TO_SCORE,
AMP_PREPARE_TO_SCORE,
FEEDING_PREPARE_TO_SHOOT,
PASS_PREPARE_TO_SHOOT -> {}
case SPEAKER_SCORING, SUBWOOFER_SCORING, AMP_SCORING, FEEDING_SHOOTING, PASS_SHOOTING -> {}
default -> setStateFromRequest(RobotState.IDLE_WITH_GP);
}
}
Expand Down
Loading

0 comments on commit 2a10c74

Please sign in to comment.