Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

IDk

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-10 18:42:47 -08:00
parent 539c1bd8eb
commit 983b95fdc7
6 changed files with 118 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+59 -34
View File
@@ -2,23 +2,38 @@ package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.intake.Intake;
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
import frc4388.robot.subsystems.swerve.SwerveDrive;
public class Shooter extends SubsystemBase {
public ShooterIO io;
ShooterStateAutoLogged state = new ShooterStateAutoLogged();
SwerveDrive m_SwerveDrive;
Intake m_Intake;
LED m_robotLED;
// Supplier<Pose2d> m_swervePoseSupplier;
public Shooter(
ShooterIO io
// Supplier<Pose2d> swervePoseSupplier
ShooterIO io,
SwerveDrive swerveDrive,
Intake intake,
LED robotLED
) {
this.io = io;
this.m_SwerveDrive = swerveDrive;
this.m_Intake = intake;
this.m_robotLED = robotLED;
// this.m_swervePoseSupplier = swervePoseSupplier;
}
@@ -32,57 +47,67 @@ public class Shooter extends SubsystemBase {
public enum ShooterMode {
//Shooter is at speed it fires balls
Active,
//Shooter is at a resting velocity
Resting,
//Shooter is inactive (Off)
Inactive,
// Shooter is actively shooting
Shooting,
// Shooter is going to fire soon
Ready,
// Not ready to shoot
NotReady,
}
private ShooterMode mode = ShooterMode.Inactive;
private ShooterMode mode = ShooterMode.NotReady;
public void setMode(ShooterMode mode) {
this.mode = mode;
// public void setMode(ShooterMode mode) {
// this.mode = mode;
// }
public void setShooterReady() {
if(this.mode == ShooterMode.NotReady) {
this.mode = ShooterMode.Ready;
}
}
public void setShooterNotReady() {
this.mode = ShooterMode.NotReady;
}
// Calculate what should be done based off of the position of the robot
// TODO: Implement field zones
public FieldZone getTarget(Pose2d position) {
return FieldZone.InShootZone;
@AutoLogOutput
public ShooterMode getMode() {
return mode;
}
@Override
public void periodic() {
// FaultReporter.register(this); // TODO Implement fault reporter
Logger.processInputs("Shooter", state);
// Pose2d pose = m_swervePoseSupplier.get();
// Angle robotRot = pose.getRotation().getMeasure();
io.updateInputs(state);
if(this.mode != ShooterMode.NotReady) {
double badRobotSpeed = Math.sqrt(Math.pow(m_SwerveDrive.chassisSpeeds.vxMetersPerSecond,2) + Math.pow(m_SwerveDrive.chassisSpeeds.vyMetersPerSecond,2));
double badAngSpeed = Math.abs(m_SwerveDrive.chassisSpeeds.omegaRadiansPerSecond);
double badShooterVelocity = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended;
// TODO: get if the robot is within the correct distance of the hub
}
switch (mode) {
case Active:
case Shooting:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get()));
// io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY);
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_ACTIVE_VELOCITY.get()));
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_FORWARD_VELOCITY.get()));
break;
case Resting:
case Ready:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get()));
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get()));
break;
case NotReady:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get()));
// io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
io.setIndexerVelocity(state, RotationsPerSecond.of(0));
break;
case Inactive:
io.setShooterVelocity(state, RotationsPerSecond.of(0));
// io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
io.setIndexerVelocity(state, RotationsPerSecond.of(0));
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get()));
break;
}