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

Jump to bottom

start of robot manager #28

Merged
merged 10 commits into from
Sep 11, 2024
Merged

start of robot manager #28

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
8e344d1
start of robot manager
ryanknj5 Sep 6, 2024
3002d99
add states to arm and shooter + continue robot manager
ryanknj5 Sep 7, 2024
123fb4f
spotless
ryanknj5 Sep 7, 2024
5da0fea
continue robot manager
ryanknj5 Sep 7, 2024
fc974bf
state transition change
ryanknj5 Sep 8, 2024
fe82a78
Fix typos & formatting
jonahsnider Sep 8, 2024
acfed3e
robot manager fixes
ryanknj5 Sep 8, 2024
0c8fea6
small fix and typo
ryanknj5 Sep 10, 2024
74bf666
Format with Spotless
jonahsnider Sep 10, 2024
3fdd5de
robot manager finished
ryanknj5 Sep 11, 2024
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
1 change: 1 addition & 0 deletions src/main/java/frc/robot/arm/ArmAngle.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,4 +11,5 @@ public class ArmAngle {
public static final Rotation2d CLIMBING_1_LINEUP = Rotation2d.fromDegrees(30);
public static final Rotation2d CLIMBING_2_HANGING = Rotation2d.fromDegrees(20);
public static final Rotation2d AMP = Rotation2d.fromDegrees(20);
public static final Rotation2d PASS = Rotation2d.fromDegrees(5);
}
1 change: 1 addition & 0 deletions src/main/java/frc/robot/arm/ArmState.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ public enum ArmState {
CLIMBING_1_LINEUP,
CLIMBING_2_HANGING,
AMP,
PASS,

PRE_MATCH_HOMING;
}
7 changes: 7 additions & 0 deletions src/main/java/frc/robot/arm/ArmSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,9 @@ public boolean atGoal() {
case AMP ->
MathUtil.isNear(ArmAngle.AMP.getDegrees(), leftMotorAngle, 1)
&& MathUtil.isNear(ArmAngle.AMP.getDegrees(), rightMotorAngle, 1);
case PASS ->
MathUtil.isNear(ArmAngle.PASS.getDegrees(), leftMotorAngle, 1)
&& MathUtil.isNear(ArmAngle.PASS.getDegrees(), rightMotorAngle, 1);
};
}

Expand Down Expand Up @@ -155,6 +158,10 @@ protected void afterTransition(ArmState newState) {
leftMotor.setControl(positionRequest.withPosition(ArmAngle.AMP.getRotations()));
rightMotor.setControl(positionRequest.withPosition(ArmAngle.AMP.getRotations()));
}
case PASS -> {
leftMotor.setControl(positionRequest.withPosition(ArmAngle.PASS.getRotations()));
rightMotor.setControl(positionRequest.withPosition(ArmAngle.PASS.getRotations()));
}
}
}

Expand Down
179 changes: 178 additions & 1 deletion src/main/java/frc/robot/robot_manager/RobotManager.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,18 +1,195 @@
package frc.robot.robot_manager;

import frc.robot.arm.ArmState;
import frc.robot.arm.ArmSubsystem;
import frc.robot.imu.ImuSubsystem;
import frc.robot.intake.IntakeState;
import frc.robot.intake.IntakeSubsystem;
import frc.robot.localization.LocalizationSubsystem;
import frc.robot.queuer.QueuerState;
import frc.robot.queuer.QueuerSubsystem;
import frc.robot.shooter.ShooterState;
import frc.robot.shooter.ShooterSubsystem;
import frc.robot.util.scheduling.SubsystemPriority;
import frc.robot.util.state_machines.StateMachine;
import frc.robot.vision.VisionSubsystem;

public class RobotManager extends StateMachine<RobotState> {
public RobotManager() {
public final ArmSubsystem arm;
public final ShooterSubsystem shooter;
public final LocalizationSubsystem localization;
public final VisionSubsystem vision;
public final ImuSubsystem imu;
public final IntakeSubsystem intake;
public final QueuerSubsystem queuer;

private final double distanceToFeedSpot = 0.0;
private final double distanceToSpeaker = 0.0;

public RobotManager(
ArmSubsystem arm,
ShooterSubsystem shooter,
LocalizationSubsystem localization,
VisionSubsystem vision,
ImuSubsystem imu,
IntakeSubsystem intake,
QueuerSubsystem queuer) {
super(SubsystemPriority.ROBOT_MANAGER, RobotState.IDLE_NO_GP);
this.arm = arm;
this.shooter = shooter;
this.localization = localization;
this.vision = vision;
this.imu = imu;
this.intake = intake;
this.queuer = queuer;
}

@Override
protected void collectInputs() {}

@Override
protected RobotState getNextState(RobotState currentState) {
jonahsnider marked this conversation as resolved.
Show resolved Hide resolved
return switch (currentState) {
case SPEAKER_WAITING,
AMP_WAITING,
FEEDING_WAITING,
PASS_WAITING,
IDLE_NO_GP,
IDLE_WITH_GP,
CLIMBING_1_LINEUP,
CLIMBING_2_HANGING ->
currentState;
case SPEAKER_SCORING, AMP_SCORING, FEEDING_SHOOTING, PASS_SHOOTING ->
!queuer.hasNote() ? RobotState.IDLE_NO_GP : currentState;
jonahsnider marked this conversation as resolved.
Show resolved Hide resolved

case SPEAKER_PREPARE_TO_SCORE ->
shooter.atGoal() && arm.atGoal() ? RobotState.SPEAKER_SCORING : currentState;

case AMP_PREPARE_TO_SCORE ->
shooter.atGoal() && arm.atGoal() ? RobotState.AMP_SCORING : currentState;

case FEEDING_PREPARE_TO_SHOOT ->
shooter.atGoal() && arm.atGoal() ? RobotState.FEEDING_SHOOTING : currentState;
case PASS_PREPARE_TO_SHOOT ->
shooter.atGoal() && arm.atGoal() ? RobotState.PASS_SHOOTING : currentState;
case UNJAM -> currentState;
case INTAKING -> queuer.hasNote() ? RobotState.IDLE_WITH_GP : currentState;
case OUTTAKING ->
!queuer.hasNote() || !intake.hasNote() ? RobotState.IDLE_NO_GP : currentState;
jonahsnider marked this conversation as resolved.
Show resolved Hide resolved
};
}

@Override
protected void afterTransition(RobotState newState) {
switch (newState) {
case SPEAKER_PREPARE_TO_SCORE, SPEAKER_WAITING -> {
arm.setState(ArmState.SPEAKER_SHOT);
shooter.setState(ShooterState.SPEAKER_SHOT);
intake.setState(IntakeState.IDLE);
queuer.seState(QueuerState.IDLE_WITH_GP);
}
case SPEAKER_SCORING -> {
arm.setState(ArmState.SPEAKER_SHOT);
shooter.setState(ShooterState.SPEAKER_SHOT);
intake.setState(IntakeState.IDLE);
queuer.seState(QueuerState.SHOOTING);
}
case AMP_PREPARE_TO_SCORE, AMP_WAITING -> {
arm.setState(ArmState.AMP);
shooter.setState(ShooterState.AMP);
intake.setState(IntakeState.IDLE);
queuer.seState(QueuerState.IDLE_WITH_GP);
}
case AMP_SCORING -> {
arm.setState(ArmState.AMP);
shooter.setState(ShooterState.AMP);
intake.setState(IntakeState.IDLE);
queuer.seState(QueuerState.SHOOTING);
}
case FEEDING_PREPARE_TO_SHOOT, FEEDING_WAITING -> {
arm.setState(ArmState.FEEDING);
shooter.setState(ShooterState.FEEDING);
intake.setState(IntakeState.IDLE);
queuer.seState(QueuerState.IDLE_WITH_GP);
}
case FEEDING_SHOOTING -> {
arm.setState(ArmState.FEEDING);
shooter.setState(ShooterState.FEEDING);
intake.setState(IntakeState.IDLE);
queuer.seState(QueuerState.SHOOTING);
jonahsnider marked this conversation as resolved.
Show resolved Hide resolved
}
case PASS_PREPARE_TO_SHOOT, PASS_WAITING -> {
arm.setState(ArmState.PASS);
shooter.setState(ShooterState.PASS);
intake.setState(IntakeState.IDLE);
queuer.seState(QueuerState.IDLE_WITH_GP);
}
case PASS_SHOOTING -> {
arm.setState(ArmState.PASS);
shooter.setState(ShooterState.PASS);
intake.setState(IntakeState.IDLE);
queuer.seState(QueuerState.SHOOTING);
}
case UNJAM -> {
arm.setState(ArmState.AMP);
shooter.setState(ShooterState.PASS);
intake.setState(IntakeState.OUTTAKING);
queuer.seState(QueuerState.OUTTAKING);
}
case INTAKING -> {
arm.setState(ArmState.IDLE);
shooter.setState(ShooterState.IDLE_STOPPED);
intake.setState(IntakeState.INTAKING);
queuer.seState(QueuerState.INTAKING);
}
case OUTTAKING -> {
arm.setState(ArmState.IDLE);
shooter.setState(ShooterState.IDLE_STOPPED);
intake.setState(IntakeState.OUTTAKING);
queuer.seState(QueuerState.OUTTAKING);
}
case CLIMBING_1_LINEUP -> {
arm.setState(ArmState.CLIMBING_1_LINEUP);
shooter.setState(ShooterState.IDLE_STOPPED);
intake.setState(IntakeState.IDLE);
queuer.seState(QueuerState.IDLE_NO_GP);
}
case CLIMBING_2_HANGING -> {
arm.setState(ArmState.CLIMBING_2_HANGING);
shooter.setState(ShooterState.IDLE_STOPPED);
intake.setState(IntakeState.IDLE);
queuer.seState(QueuerState.IDLE_NO_GP);
}
case IDLE_NO_GP -> {
arm.setState(ArmState.IDLE);
shooter.setState(ShooterState.IDLE_STOPPED);
intake.setState(IntakeState.IDLE);
queuer.seState(QueuerState.IDLE_NO_GP);
}
case IDLE_WITH_GP -> {
arm.setState(ArmState.IDLE);
shooter.setState(ShooterState.IDLE_WARMUP);
intake.setState(IntakeState.IDLE);
queuer.seState(QueuerState.IDLE_WITH_GP);
}
}
}

@Override
public void robotPeriodic() {
super.robotPeriodic();

// Continuous state actions
switch (getState()) {
case SPEAKER_PREPARE_TO_SCORE, SPEAKER_SCORING, SPEAKER_WAITING -> {
shooter.setDistanceToSpeaker(distanceToSpeaker);
arm.setDistanceToSpeaker(distanceToSpeaker);
}
case FEEDING_PREPARE_TO_SHOOT, FEEDING_SHOOTING, FEEDING_WAITING -> {
shooter.setDistanceToFeedSpot(distanceToFeedSpot);
arm.setDistanceToFeedSpot(distanceToFeedSpot);
}
default -> {}
}
}
}
5 changes: 4 additions & 1 deletion src/main/java/frc/robot/robot_manager/RobotState.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,5 +23,8 @@ public enum RobotState {

PASS_WAITING,
PASS_PREPARE_TO_SHOOT,
PASS_SHOOTING;
PASS_SHOOTING,

CLIMBING_1_LINEUP,
CLIMBING_2_HANGING;
}
ryanknj5 marked this conversation as resolved.
Show resolved Hide resolved
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/shooter/ShooterRpms.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,4 +5,6 @@ public class ShooterRpms {
public static final double DROP = 500.0;
public static final double PODIUM = 4000.0;
public static final double IDLE_WARMUP = 1000.0;
public static final double AMP = 500.0;
public static final double PASS = 2000.0;
}
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/shooter/ShooterState.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,5 +7,7 @@ public enum ShooterState {
DROP,
SUBWOOFER_SHOT,
FEEDING,
PODIUM_SHOT;
PODIUM_SHOT,
AMP,
PASS;
}
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/shooter/ShooterSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,8 @@ public boolean atGoal() {
MathUtil.isNear(feedSpotDistanceToRpm.get(distanceToFeedSpot), shooterRPM, 50);
case SPEAKER_SHOT ->
MathUtil.isNear(speakerDistanceToRpm.get(distanceToSpeaker), shooterRPM, 50);
case AMP -> MathUtil.isNear(ShooterRpms.AMP, shooterRPM, 50);
case PASS -> MathUtil.isNear(ShooterRpms.PASS, shooterRPM, 50);
};
}

Expand Down Expand Up @@ -74,6 +76,8 @@ protected void afterTransition(ShooterState newState) {
velocityRequest.withVelocity(speakerDistanceToRpm.get(distanceToFeedSpot) / 60.0));
case DROP -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.DROP / 60.0));
case PODIUM_SHOT -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.PODIUM / 60.0));
case AMP -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.AMP / 60.0));
case PASS -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.PASS / 60.0));
}
}

Expand Down