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/intake/Intake.java
+4
View File
@@ -39,6 +39,10 @@ public class Intake extends SubsystemBase {
this.mode = mode;
}
public IntakeMode getMode() {
return mode;
}
// public enum FieldZone {
// // The robot should aim at the hub
src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java
+1
View File
@@ -78,6 +78,7 @@ public class IntakeReal implements IntakeIO {
// Assume that the angle is always accurate, because I think we will use a shaft encoder
// Assume that 0 degrees = forwards. Might need an offset here
// angle = clampAng(angle, IntakeConstants.ARM_LIMIT_RETRACTED, IntakeConstants.ARM_LIMIT_EXTENDED);
// (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO);
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;
}
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+15 -2
View File
@@ -33,8 +33,21 @@ public class ShooterConstants {
public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15);
// public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Indexer Active Velocity", 10);
// public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
public static final ConfigurableDouble INDEXER_FORWARD_VELOCITY = new ConfigurableDouble("Indexer FWD Velocity", 10);
public static final ConfigurableDouble INDEXER_REVERSE_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10);
// Tolerances
public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist", 1);
public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist", 1);
public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Shoot Ang tolerance", 1);
public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Shoot speed tolerance", 1);
public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance", 1);
public static final ConfigurableDouble SHOOT_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance", 1);
public static Slot0Configs SHOOTER_PID = new Slot0Configs()
.withKV(0.0)
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
-45
View File
@@ -43,52 +43,7 @@ public class ShooterReal implements ShooterIO {
shooter2Velocity.Slot = 0;
m_indexerVelocity.Slot = 0;
}
private Angle clampAng(Angle x, Angle min, Angle max){
if(x.gt(max)) {
return max;
}else if(x.lt(min)) {
return min;
}else{
return x;
}
}
// // TODO: Test
// @Override
// public void setShooterAngle(ShooterState state, Angle angle) {
// state.shooterTargetAngle = angle;
// // Assume that the angle is always accurate, because I think we will use a shaft encoder
// // Assume that 0 degrees = forwards. Might need an offset here
// Angle boundedAngle = clampAng(angle, ShooterConstants.ANGLE_LIMIT_LEFT, ShooterConstants.ANGLE_LIMIT_RIGHT);
// // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
// double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.ANGLE_MOTOR_GEAR_RATIO;
// PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
// m_angleMotor.setControl(posRequest);
// }
// TODO: Test
// @Override
// public void setShooterPitch(ShooterState state, Angle angle) {
// state.shooterTargetPitch = angle;
// // TODO: Test
// // This assumes that the 0 is paralell to the ground. Might need an offset here
// Angle boundedAngle = clampAng(angle, ShooterConstants.PITCH_LIMIT_UPPER, ShooterConstants.PITCH_LIMIT_LOWER);
// // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
// double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.PITCH_MOTOR_GEAR_RATIO;
// PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
// m_pitchMotor.setControl(posRequest);
// }
@Override
public void setShooterVelocity(ShooterState state, AngularVelocity target) {
state.motor1TargetVelocity = target;