Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 08:48:05 -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/RobotContainer.java
+39 -22
View File
@@ -65,7 +65,7 @@ public class RobotContainer {
public final Vision m_vision = new Vision();
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO);
public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO);
public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED);
private Boolean operatorManualMode = false;
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
@@ -175,15 +175,15 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter));
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter));
// new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
// .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter));
// new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
// .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Extended);}, m_robotIntake));
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake));
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
// .onTrue(new InstantCommand(() -> {;}, m_robotIntake));
// new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake));
@@ -196,18 +196,7 @@ public class RobotContainer {
m_robotShooter.io.updateGains();
}));
// IF the driver is holding the aim button, aim the robot towards the hub
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
.whileTrue(new RunCommand(
() -> {
m_robotSwerveDrive.driveFacingPosition(
getDeadbandedDriverController().getLeft(),
FieldConstants.BLUE_HUB_POS);
}, m_robotSwerveDrive))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
// IF the driver is holding the aim button, aim the robot towards the hub
// IF the driver is holding the left trigger, intake driving
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
.whileTrue(new RunCommand(
() -> {
@@ -216,7 +205,35 @@ public class RobotContainer {
false
);
}, m_robotSwerveDrive))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
// .onFalse(new InstantCommand(() -> , m_robotSwerveDrive));
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Extended);
}))
.onFalse(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Retracted);
m_robotSwerveDrive.softStop();
}, m_robotSwerveDrive));
// IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
.whileTrue(new RunCommand(
() -> {
m_robotSwerveDrive.driveFacingPosition(
getDeadbandedDriverController().getLeft(),
FieldConstants.BLUE_HUB_POS);
}, m_robotSwerveDrive)
)
.onTrue(new InstantCommand(() -> {
// When Right trigger is pressed,
m_robotIntake.setMode(IntakeMode.Extended);
}))
.onFalse(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Retracted);
m_robotSwerveDrive.softStop();
}, m_robotSwerveDrive));
// D-PAD fine alignment
new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)
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;