Skip to content
This repository has been archived by the owner on Apr 20, 2024. It is now read-only.

Commit

Permalink
made queuer subsystem
Browse files Browse the repository at this point in the history
  • Loading branch information
Aidan committed Feb 10, 2024
1 parent 2cad3f8 commit 8c6a8f6
Show file tree
Hide file tree
Showing 8 changed files with 76 additions and 65 deletions.
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,9 +51,9 @@ public class Robot extends LoggedRobot {
RobotConfig.get().shoulder().leftMotorID(), RobotConfig.get().canivoreName()));
private final ShooterSubsystem shooter =
new ShooterSubsystem(
new TalonFX(RobotConfig.get().shooter().leftMotorID(), RobotConfig.get().canivoreName()),
new TalonFX(
RobotConfig.get().shooter().bottomMotorID(), RobotConfig.get().canivoreName()),
new TalonFX(RobotConfig.get().shooter().topMotorID(), RobotConfig.get().canivoreName()));
RobotConfig.get().shooter().rightMotorID(), RobotConfig.get().canivoreName()));
private final IClimberSubsystem climber =
RobotConfig.IS_PRACTICE_BOT
? new ClimberSubsystemStub()
Expand Down
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/config/CompConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@ class CompConfig {
new ShooterConfig(
18,
19,
// Left Motor
new TalonFXConfiguration()
.withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1.0 / 1.5))
.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(120))
Expand All @@ -41,6 +42,7 @@ class CompConfig {
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Coast)
.withInverted(InvertedValue.Clockwise_Positive)),
// Right Motor
new TalonFXConfiguration()
.withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1.0 / 1.5))
.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(120))
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/config/PracticeConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ class PracticeConfig {
new ShooterConfig(
18,
19,
// Bottom motor
// Left motor
new TalonFXConfiguration()
.withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1))
.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(120))
Expand All @@ -41,7 +41,7 @@ class PracticeConfig {
new MotorOutputConfigs()
.withInverted(InvertedValue.Clockwise_Positive)
.withNeutralMode(NeutralModeValue.Coast)),
// Top motor
// Right motor
new TalonFXConfiguration()
.withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1))
.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(120))
Expand Down
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/config/RobotConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,10 +22,10 @@ public record RobotConfig(
IMUConfig imu,
LightsConfig lights) {
public record ShooterConfig(
int bottomMotorID,
int topMotorID,
TalonFXConfiguration bottomMotorConfig,
TalonFXConfiguration topMotorConfig,
int leftMotorID,
int rightMotorID,
TalonFXConfiguration leftMotorConfig,
TalonFXConfiguration rightMotorConfig,
Consumer<InterpolatingDoubleTreeMap> speakerShotRpms,
Consumer<InterpolatingDoubleTreeMap> floorShotRpms) {}

Expand Down Expand Up @@ -67,7 +67,7 @@ public record SwerveConfig(

// TODO: Change this to false during events
public static final boolean IS_DEVELOPMENT = true;
private static final String PRACTICE_BOT_SERIAL_NUMBER = "0322443D"; //TODO: get serial number
private static final String PRACTICE_BOT_SERIAL_NUMBER = "0322443D"; // TODO: get serial number
public static final String SERIAL_NUMBER = System.getenv("serialnum");
public static final boolean IS_PRACTICE_BOT =
SERIAL_NUMBER != null && SERIAL_NUMBER.equals(PRACTICE_BOT_SERIAL_NUMBER);
Expand Down
14 changes: 7 additions & 7 deletions src/main/java/frc/robot/robot_manager/RobotManager.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -335,7 +335,7 @@ public void robotPeriodic() {
}
break;
case PREPARE_AMP_SHOT:
if (shoulder.atAngle(ShoulderPositions.AMP_SHOT) && shooter.atGoal(ShooterMode.AMP_SHOT)) {
if (shoulder.atAngle(ShoulderPositions.AMP_SHOT)) {
state = RobotState.AMP_SHOOT;
}
break;
Expand Down Expand Up @@ -420,25 +420,25 @@ public void robotPeriodic() {
case GROUND_INTAKING:
shoulder.setAngle(ShoulderPositions.GROUND_INTAKING);
intake.intakingRequest();
shooter.setMode(ShooterMode.INTAKE);
shooter.setMode(ShooterMode.INTAKING);
climber.setGoal(ClimberMode.IDLE);
break;
case GROUND_INTAKING_SETTLING:
shoulder.setAngle(ShoulderPositions.STOWED_DOWN);
intake.intakingRequest();
shooter.setMode(ShooterMode.INTAKE);
shooter.setMode(ShooterMode.INTAKING);
climber.setGoal(ClimberMode.IDLE);
break;
case SOURCE_INTAKING:
shoulder.setAngle(ShoulderPositions.SOURCE_INTAKING);
intake.intakingRequest();
shooter.setMode(ShooterMode.INTAKE);
shooter.setMode(ShooterMode.INTAKING);
climber.setGoal(ClimberMode.IDLE);
break;
case SOURCE_INTAKING_SETTLING:
shoulder.setAngle(ShoulderPositions.STOWED_UP);
intake.intakingRequest();
shooter.setMode(ShooterMode.INTAKE);
shooter.setMode(ShooterMode.INTAKING);
climber.setGoal(ClimberMode.IDLE);
break;
case OUTTAKING:
Expand Down Expand Up @@ -502,13 +502,13 @@ public void robotPeriodic() {
case PREPARE_AMP_SHOT:
shoulder.setAngle(ShoulderPositions.AMP_SHOT);
intake.idleRequest();
shooter.setMode(ShooterMode.AMP_SHOT);
shooter.setMode(ShooterMode.IDLE);
climber.setGoal(ClimberMode.IDLE);
break;
case AMP_SHOOT:
shoulder.setAngle(ShoulderPositions.AMP_SHOT);
intake.shootingRequest();
shooter.setMode(ShooterMode.AMP_SHOT);
shooter.setMode(ShooterMode.IDLE);
climber.setGoal(ClimberMode.IDLE);
break;
case WAITING_CLIMBER_RAISED:
Expand Down
8 changes: 5 additions & 3 deletions src/main/java/frc/robot/shooter/ShooterMode.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,11 @@

public enum ShooterMode {
IDLE,
FLOOR_SHOT,
PASSING_NOTE,
OUTTAKE,
SUBWOOFER_SHOT,
AMP_SHOT,
SPEAKER_SHOT,
INTAKE;
FLOOR_SHOT,
// Isnt really used
INTAKING;
}
86 changes: 49 additions & 37 deletions src/main/java/frc/robot/shooter/ShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,11 +16,12 @@

public class ShooterSubsystem extends LifecycleSubsystem {
private static final int IDLE_RPM = 1000;
private static final double PASSING_RPM = 1500; // Make this not horrible
private static final double OUTTAKE_RPM = 2000; // TODO: adjust for desirable outtake speeds
private static final double SUBWOOFER_SHOOTING_RPM = 5000;
private static final double AMP_SHOOTING_RPM = 1000;
private static final double TOLERANCE_RPM = 100;
private final TalonFX bottomMotor;
private final TalonFX topMotor;
private final TalonFX leftMotor;
private final TalonFX rightMotor;
private final LoggedDashboardNumber ntRPM = new LoggedDashboardNumber("Shooter/RPMOverride", -1);
private final VelocityTorqueCurrentFOC velocityRequest =
new VelocityTorqueCurrentFOC(0).withSlot(0).withLimitReverseMotion(true);
Expand All @@ -37,17 +38,17 @@ public class ShooterSubsystem extends LifecycleSubsystem {
private ShooterMode goalMode = ShooterMode.IDLE;
private final StaticBrake brakeRequest = new StaticBrake();

public ShooterSubsystem(TalonFX bottomMotor, TalonFX topMotor) {
public ShooterSubsystem(TalonFX leftMotor, TalonFX rightMotor) {
super(SubsystemPriority.SHOOTER);

this.bottomMotor = bottomMotor;
this.topMotor = topMotor;
this.rightMotor = leftMotor;
this.leftMotor = rightMotor;

RobotConfig.get().shooter().speakerShotRpms().accept(speakerDistanceToRPM);
RobotConfig.get().shooter().floorShotRpms().accept(floorSpotDistanceToRPM);

bottomMotor.getConfigurator().apply(RobotConfig.get().shooter().bottomMotorConfig());
topMotor.getConfigurator().apply(RobotConfig.get().shooter().topMotorConfig());
leftMotor.getConfigurator().apply(RobotConfig.get().shooter().leftMotorConfig());
rightMotor.getConfigurator().apply(RobotConfig.get().shooter().rightMotorConfig());
}

@Override
Expand All @@ -59,49 +60,60 @@ public void enabledPeriodic() {

usedGoalRPM = overrideRPM == -1 ? goalRPM : overrideRPM;

if (goalMode == ShooterMode.INTAKE) {
bottomMotor.setControl(brakeRequest);
topMotor.setControl(brakeRequest);
if (goalMode == ShooterMode.INTAKING) {
rightMotor.setControl(brakeRequest);
leftMotor.setControl(brakeRequest);
} else {
bottomMotor.setControl(velocityRequest.withVelocity(usedGoalRPM / 60));
topMotor.setControl(velocityRequest.withVelocity(usedGoalRPM / 60));
rightMotor.setControl(velocityRequest.withVelocity((usedGoalRPM) / 60));
leftMotor.setControl(velocityRequest.withVelocity((usedGoalRPM - 500) / 60));
}
}

@Override
public void robotPeriodic() {
if (goalMode == ShooterMode.SPEAKER_SHOT) {
goalRPM = speakerDistanceToRPM.get(speakerDistance);
} else if (goalMode == ShooterMode.AMP_SHOT) {
goalRPM = AMP_SHOOTING_RPM;
} else if (goalMode == ShooterMode.SUBWOOFER_SHOT) {
goalRPM = SUBWOOFER_SHOOTING_RPM;
} else if (goalMode == ShooterMode.FLOOR_SHOT) {
goalRPM = floorSpotDistanceToRPM.get(floorSpotDistance);
} else {
goalRPM = IDLE_RPM;
switch (goalMode) {
case SPEAKER_SHOT:
goalRPM = speakerDistanceToRPM.get(speakerDistance);
break;
case SUBWOOFER_SHOT:
goalRPM = SUBWOOFER_SHOOTING_RPM;
break;
case FLOOR_SHOT:
goalRPM = floorSpotDistanceToRPM.get(floorSpotDistance);
break;
case OUTTAKE:
goalRPM = OUTTAKE_RPM;
break;
case IDLE:
goalRPM = IDLE_RPM;
break;
case PASSING_NOTE:
goalRPM = PASSING_RPM;
break;
default:
break;
}
Logger.recordOutput("Shooter/DistanceToSpeaker", speakerDistance);
Logger.recordOutput("Shooter/DistanceToFloorSpot", floorSpotDistance);
Logger.recordOutput("Shooter/Mode", goalMode);
Logger.recordOutput("Shooter/GoalRPM", goalRPM);
Logger.recordOutput("Shooter/TopMotor/Temperature", topMotor.getDeviceTemp().getValue());
Logger.recordOutput("Shooter/TopMotor/RPM", getRPM(topMotor));
Logger.recordOutput("Shooter/TopMotor/Temperature", leftMotor.getDeviceTemp().getValue());
Logger.recordOutput("Shooter/TopMotor/RPM", getRPM(leftMotor));
Logger.recordOutput(
"Shooter/TopMotor/POutput", topMotor.getClosedLoopProportionalOutput().getValue());
"Shooter/TopMotor/POutput", leftMotor.getClosedLoopProportionalOutput().getValue());
Logger.recordOutput(
"Shooter/TopMotor/SupplierCurrent", topMotor.getSupplyCurrent().getValueAsDouble());
Logger.recordOutput("Shooter/TopMotor/StatorCurrent", topMotor.getStatorCurrent().getValue());
Logger.recordOutput("Shooter/TopMotor/Voltage", topMotor.getMotorVoltage().getValue());
"Shooter/TopMotor/SupplierCurrent", leftMotor.getSupplyCurrent().getValueAsDouble());
Logger.recordOutput("Shooter/TopMotor/StatorCurrent", leftMotor.getStatorCurrent().getValue());
Logger.recordOutput("Shooter/TopMotor/Voltage", leftMotor.getMotorVoltage().getValue());
Logger.recordOutput(
"Shooter/BottomMotor/StatorCurrent", bottomMotor.getStatorCurrent().getValue());
Logger.recordOutput("Shooter/BottomMotor/Voltage", bottomMotor.getMotorVoltage().getValue());
Logger.recordOutput("Shooter/BottomMotor/RPM", getRPM(bottomMotor));
Logger.recordOutput("Shooter/BottomMotor/Temperature", bottomMotor.getDeviceTemp().getValue());
"Shooter/BottomMotor/StatorCurrent", rightMotor.getStatorCurrent().getValue());
Logger.recordOutput("Shooter/BottomMotor/Voltage", rightMotor.getMotorVoltage().getValue());
Logger.recordOutput("Shooter/BottomMotor/RPM", getRPM(rightMotor));
Logger.recordOutput("Shooter/BottomMotor/Temperature", rightMotor.getDeviceTemp().getValue());
Logger.recordOutput(
"Shooter/BottomMotor/POutput", bottomMotor.getClosedLoopProportionalOutput().getValue());
"Shooter/BottomMotor/POutput", rightMotor.getClosedLoopProportionalOutput().getValue());
Logger.recordOutput(
"Shooter/BottomMotor/SupplierCurrent", bottomMotor.getSupplyCurrent().getValueAsDouble());
"Shooter/BottomMotor/SupplierCurrent", rightMotor.getSupplyCurrent().getValueAsDouble());
Logger.recordOutput("Shooter/AtGoal", atGoal(goalMode));
}

Expand All @@ -114,8 +126,8 @@ public boolean atGoal(ShooterMode mode) {
return true;
}

if (Math.abs(goalRPM - getRPM(bottomMotor)) < TOLERANCE_RPM
&& Math.abs(goalRPM - getRPM(topMotor)) < TOLERANCE_RPM) {
if (Math.abs(goalRPM - getRPM(rightMotor)) < TOLERANCE_RPM
&& Math.abs(goalRPM - getRPM(leftMotor)) < TOLERANCE_RPM) {
return true;
}

Expand Down
13 changes: 4 additions & 9 deletions src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
import frc.robot.config.RobotConfig;
import frc.robot.generated.PracticeBotTunerConstants;
import java.util.function.Supplier;

Expand All @@ -22,14 +21,10 @@
* in command-based projects easily.
*/
public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem {
public static final SwerveModuleConstants BackRight =
PracticeBotTunerConstants.BackRight;
public static final SwerveModuleConstants BackLeft =
PracticeBotTunerConstants.BackLeft;
public static final SwerveModuleConstants FrontRight =
PracticeBotTunerConstants.FrontRight;
public static final SwerveModuleConstants FrontLeft =
PracticeBotTunerConstants.FrontLeft;
public static final SwerveModuleConstants BackRight = PracticeBotTunerConstants.BackRight;
public static final SwerveModuleConstants BackLeft = PracticeBotTunerConstants.BackLeft;
public static final SwerveModuleConstants FrontRight = PracticeBotTunerConstants.FrontRight;
public static final SwerveModuleConstants FrontLeft = PracticeBotTunerConstants.FrontLeft;
public static final SwerveDrivetrainConstants DrivetrainConstants =
PracticeBotTunerConstants.DrivetrainConstants;
private static final double kSimLoopPeriod = 0.005; // 5 ms
Expand Down

0 comments on commit 8c6a8f6

Please sign in to comment.