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/RobotContainer.java
+11 -7
View File
@@ -40,6 +40,7 @@ import frc4388.robot.subsystems.shooter.Shooter.ShooterMode;
import frc4388.robot.subsystems.swerve.SwerveDrive;
import frc4388.robot.subsystems.vision.Vision;
import frc4388.utility.DeferredBlock;
import frc4388.utility.compute.FieldPositions;
import frc4388.utility.compute.TimesNegativeOne;
import frc4388.utility.controller.DeadbandedXboxController;
// Autos
@@ -112,24 +113,29 @@ public class RobotContainer {
configureButtonBindings();
DeferredBlock.addBlock(() -> { // Called on first robot enable
// Called on first robot enable
DeferredBlock.addBlock(() -> {
m_robotSwerveDrive.resetGyro();
}, false);
DeferredBlock.addBlock(() -> { // Called on every robot enable
// Called on every robot enable
DeferredBlock.addBlock(() -> {
TimesNegativeOne.update();
FieldPositions.update();
}, true);
DriverStation.silenceJoystickConnectionWarning(true);
// Drive normally
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
// Drive normally
m_robotSwerveDrive.driveWithInput(
getDeadbandedDriverController().getLeft(),
getDeadbandedDriverController().getRight(),true);
}, m_robotSwerveDrive)
.withName("SwerveDrive DefaultCommand"));
m_robotSwerveDrive.setToSlow();
makeAutoChooser();
@@ -196,6 +202,7 @@ public class RobotContainer {
m_robotShooter.io.updateGains();
}));
// IF the driver is holding the left trigger, intake driving
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
.whileTrue(new RunCommand(
@@ -205,9 +212,6 @@ public class RobotContainer {
false
);
}, m_robotSwerveDrive))
// .onFalse(new InstantCommand(() -> , m_robotSwerveDrive));
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Extended);
}))
@@ -222,7 +226,7 @@ public class RobotContainer {
() -> {
m_robotSwerveDrive.driveFacingPosition(
getDeadbandedDriverController().getLeft(),
FieldConstants.BLUE_HUB_POS);
FieldPositions.HUB_POSITION);
}, m_robotSwerveDrive)
)
.onTrue(new InstantCommand(() -> {
src/main/java/frc4388/robot/constants/BuildConstants.java
+8 -8
View File
@@ -5,15 +5,15 @@ package frc4388.robot.constants;
*/
public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String MAVEN_NAME = "2026KPopRobotHunters-new";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 38;
public static final String GIT_SHA = "6ce6d0eb0b8faec5af448ec23ffca156303cbed5";
public static final String GIT_DATE = "2026-02-09 19:38:55 MST";
public static final String GIT_BRANCH = "arm-test";
public static final String BUILD_DATE = "2026-02-10 17:12:07 MST";
public static final long BUILD_UNIX_TIME = 1770768727439L;
public static final int DIRTY = 1;
public static final int GIT_REVISION = 40;
public static final String GIT_SHA = "983b95fdc704ef35b6f5e2dada2c6348f3c67190";
public static final String GIT_DATE = "2026-02-10 19:42:47 MST";
public static final String GIT_BRANCH = "shoot-button";
public static final String BUILD_DATE = "2026-02-11 12:41:33 MST";
public static final long BUILD_UNIX_TIME = 1770838893524L;
public static final int DIRTY = 0;
private BuildConstants(){}
}
src/main/java/frc4388/robot/constants/Constants.java
+13 -6
View File
@@ -98,13 +98,20 @@ public final class Constants {
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED;
public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW;
public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK;
public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW;
// LED color for when the intake is out
public static final LEDPatterns INTAKE_OUT = LEDPatterns.SOLID_RED;
// LED color for when the intake is out, but the driver conditions are bad
public static final LEDPatterns INTAKE_OUT_BADPHYS = LEDPatterns.RED_STROBE;
public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES;
public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES;
// LED color for when the flywheel speed isn't in tolarance
public static final LEDPatterns BAD_FLYWEEL = LEDPatterns.SOLID_GOLD;
// LED color for when the flywheel speed isn't in tolarance, but the driver conditions are bad
public static final LEDPatterns BAD_FLYWEEL_BADPHYS = LEDPatterns.GOLD_STROBE;
// Operator ready to shoot
public static final LEDPatterns OPREADY = LEDPatterns.SOLID_WHITE;
// Operator ready to shoot, but the driver conditions are bad
public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE;
}
public static final class OIConstants {
src/main/java/frc4388/robot/subsystems/climber/Climber.java
+4 -23
View File
@@ -1,7 +1,5 @@
package frc4388.robot.subsystems.climber;
import static edu.wpi.first.units.Units.Rotation;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
@@ -9,20 +7,19 @@ import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.utility.status.FaultReporter;
public class Climber extends SubsystemBase {
ClimberIO io;
ClimberStateAutoLogged state = new ClimberStateAutoLogged();
Supplier<Pose2d> m_swervePoseSupplier;
// Supplier<Pose2d> m_swervePoseSupplier;
public Climber(
ClimberIO io,
Supplier<Pose2d> swervePoseSupplier
ClimberIO io
// Supplier<Pose2d> swervePoseSupplier
) {
this.io = io;
this.m_swervePoseSupplier = swervePoseSupplier;
// this.m_swervePoseSupplier = swervePoseSupplier;
}
// public enum ClimberMode {
@@ -44,19 +41,6 @@ public class Climber extends SubsystemBase {
// }
// public enum FieldZone {
// // The robot should aim at the hub
// InShootZone,
// // The robot should aim towards the wall
// AimAtWall,
// }
// // 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;
// }
@Override
public void periodic() {
@@ -68,9 +52,6 @@ public class Climber extends SubsystemBase {
Logger.processInputs("Climber", state);
Pose2d pose = m_swervePoseSupplier.get();
Angle robotRot = pose.getRotation().getMeasure();
io.updateInputs(state);
}
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) {
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+7 -7
View File
@@ -31,21 +31,21 @@ public class ShooterConstants {
public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30);
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_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_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 0);
public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 99);
public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Shoot Ang tolerance", 1);
public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360);
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 ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1);
public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance DEG", 3);
public static final ConfigurableDouble SHOOT_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance", 1);
public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 1);