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

Implement field positions, tolerance, LED states

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-11 15:18:12 -07:00
parent 983b95fdc7
commit f2415195ce
7 changed files with 125 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+61 -8
View File
@@ -2,15 +2,21 @@ package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import java.text.FieldPosition;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.constants.Constants;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.intake.Intake;
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.utility.compute.FieldPositions;
public class Shooter extends SubsystemBase {
public ShooterIO io;
@@ -57,10 +63,6 @@ public class Shooter extends SubsystemBase {
private ShooterMode mode = ShooterMode.NotReady;
// public void setMode(ShooterMode mode) {
// this.mode = mode;
// }
public void setShooterReady() {
if(this.mode == ShooterMode.NotReady) {
this.mode = ShooterMode.Ready;
@@ -86,14 +88,65 @@ public class Shooter extends SubsystemBase {
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;
ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds;
double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2));
double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI));
Pose2d robotPose2d = m_SwerveDrive.getPose2d();
double distanceToHub = robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm();
// TODO: get if the robot is within the angle of the hub
boolean driverError =
XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() |
distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get();
double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
boolean badShooterVelocity = shooterSpeed < ShooterConstants.SHOOTER_SPEED_TOLERANCE.get();
boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended;
int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (intakeBad ? 4 : 0);
switch (bitmask) {
case 0b000: // No Errors
m_robotLED.setMode(Constants.LEDConstants.OPREADY);
break;
case 0b001: // No op err, yes driver err
m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS);
break;
case 0b010: // Bad flywheel, no driver err
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
break;
case 0b011: // Bad flywheel, yes driver err
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
break;
case 0b100: // Bad intake, no driver err
m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT);
break;
case 0b101: // Bad intake, yes driver err
m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS);
break;
case 0b110: // Bad intake and shooter (intake is more important), no driver err
m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT);
break;
case 0b111: // Bad intake and shooter (intake is more important), yes driver err
m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS);
break;
}
// We set the shooter mode to ready if there are no errors
mode = (
bitmask == 0 ?
ShooterMode.Ready :
ShooterMode.NotReady
);
// TODO: get if the robot is within the correct distance of the hub
}
switch (mode) {