Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-08 16:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'reveal-night' into AutoTesting

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-26 15:14:42 -07:00
committed by GitHub
parent f9174c23cd 835d8779b0
commit 15ac825d70
10 changed files with 193 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/paths/Taxi.path
+11 -11
View File
@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 2.0,
"y": 7.0
"x": 3.62702380952381,
"y": 6.0
},
"prevControl": null,
"nextControl": {
"x": 3.0,
"y": 7.0
"x": 4.627023809523808,
"y": 6.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.9612559241706156,
"y": 7.0
"x": 2.180142857142858,
"y": 6.0
},
"prevControl": {
"x": 1.9612559241706156,
"y": 7.0
"x": 1.180142857142858,
"y": 6.0
},
"nextControl": null,
"isLocked": false,
@@ -33,7 +33,7 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 0.2,
"maxVelocity": 1.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
"rotation": 178.89829388479367
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
"rotation": 179.95980050207663
},
"useDefaultConstraints": false
}
src/main/java/frc4388/robot/RobotContainer.java
+6 -7
View File
@@ -267,29 +267,28 @@ public class RobotContainer {
new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5)
.onTrue(new InstantCommand(() -> {
m_robotShooter.setShooterNotReady();
m_robotShooter.spinUpIdle();
}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotShooter.setShooterReady();
m_robotShooter.spinUpShooting();
m_robotIntake.setMode(IntakeMode.Idle);
}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotShooter.setShooterReadyFeeder();
m_robotShooter.spinUpFeeding();
}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotShooter.setShooterShoot();
m_robotShooter.allowShooting();
})).onFalse(new InstantCommand(() -> {
m_robotShooter.setShooterNOTShoot();
m_robotShooter.denyShooting();
}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
src/main/java/frc4388/robot/constants/BuildConstants.java
+6 -6
View File
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 96;
public static final String GIT_SHA = "3745cc2b1869e5850c93507277539c7cfed606c1";
public static final String GIT_DATE = "2026-02-25 18:59:11 MST";
public static final String GIT_BRANCH = "AutoTesting";
public static final String BUILD_DATE = "2026-02-25 20:19:51 MST";
public static final long BUILD_UNIX_TIME = 1772075991136L;
public static final int GIT_REVISION = 85;
public static final String GIT_SHA = "3197a3dce03bf34c001ef4009de3e7d98a181536";
public static final String GIT_DATE = "2026-02-24 22:17:58 MST";
public static final String GIT_BRANCH = "reveal-night";
public static final String BUILD_DATE = "2026-02-25 16:32:13 MST";
public static final long BUILD_UNIX_TIME = 1772062333884L;
public static final int DIRTY = 1;
private BuildConstants(){}
src/main/java/frc4388/robot/constants/Constants.java
+1 -1
View File
@@ -119,7 +119,7 @@ public final class Constants {
public static final LEDPatterns OPREADY_FEED = LEDPatterns.SOLID_BLUE;
public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE;
// public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE;
}
public static final class OIConstants {
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+121 -102
View File
@@ -1,5 +1,6 @@
package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Rotation;
import static edu.wpi.first.units.Units.RotationsPerSecond;
@@ -7,7 +8,10 @@ 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.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.constants.Constants;
import frc4388.robot.subsystems.LED;
@@ -50,43 +54,36 @@ public class Shooter extends SubsystemBase {
public enum ShooterMode {
// Shooter is actively shooting
Shooting,
// Shooter is going to fire soon
Ready,
ShootingFeeder,
ReadyFeeder,
// Not ready to shoot
NotReady,
Feeding,
Idle
}
private ShooterMode mode = ShooterMode.NotReady;
private ShooterMode mode = ShooterMode.Idle;
private boolean shooterButtonReady = false;
public void setShooterReady() {
this.mode = ShooterMode.Ready;
public void spinUpShooting() {
this.mode = ShooterMode.Shooting;
}
public void setShooterReadyFeeder() {
this.mode = ShooterMode.ReadyFeeder;
public void spinUpFeeding() {
this.mode = ShooterMode.Feeding;
}
public void setShooterNotReady() {
this.mode = ShooterMode.NotReady;
public void spinUpIdle() {
this.mode = ShooterMode.Idle;
}
public void setShooterShoot() {
public void allowShooting() {
shooterButtonReady = true;
}
public void setShooterNOTShoot() {
public void denyShooting() {
shooterButtonReady = false;
}
@AutoLogOutput
public ShooterMode getMode() {
return mode;
@@ -101,111 +98,133 @@ public class Shooter extends SubsystemBase {
io.updateInputs(state);
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));
// Get robot positon and speeds
ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
double XYSpeed = Math.sqrt(Math.pow(chassisSpeeds.vxMetersPerSecond,2) + Math.pow(chassisSpeeds.vyMetersPerSecond,2));
double AngSpeed = Math.abs(chassisSpeeds.omegaRadiansPerSecond * (180/Math.PI));
Pose2d robotPose2d = m_SwerveDrive.getPose2d();
//
double distanceToHub = (robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm());
// Calculate aim lead
// Get the current speed of the robot
Translation2d robotSpeed = new Translation2d(
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond
);
// Calculate a point to aim ahead of the actual position.
Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation());
// Get the robot's aim distance to hub
double distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm());
//Center of hub to cameras in inches
Logger.recordOutput("Hub Dist", distanceToHub);
if(this.mode != ShooterMode.NotReady) {
// 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;
double shooterSpeedTarget = Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond) + state.motor2TargetVelocity.in(RotationsPerSecond)) / 2;
boolean badShooterVelocity = Math.abs(shooterSpeed - shooterSpeedTarget) > ShooterConstants.SHOOTER_SPEED_TOLERANCE.get();
// boolean intakeBad = m_Intake.getMode() == IntxakeMode.Extended;
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();
int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (this.mode == ShooterMode.ReadyFeeder ? 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:
case 0b110: // Bad flywheel, no driver err
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
break;
case 0b011:
case 0b111: // Bad flywheel, yes driver err
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
break;
case 0b100:
m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED);
break;
case 0b101:
m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED_BADPHYS);
break;
}
// // We set the shooter mode to ready if there are no errors
mode = (
bitmask == 0 ?
ShooterMode.Shooting :
ShooterMode.Ready
);
} else {
m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN);
}
double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
double shooterSpeedTarget = Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond) + state.motor2TargetVelocity.in(RotationsPerSecond)) / 2;
boolean badShooterVelocity = Math.abs(shooterSpeed - shooterSpeedTarget) > ShooterConstants.SHOOTER_SPEED_TOLERANCE.get();
switch (mode) {
case Shooting:
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
if(shooterButtonReady) {
io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
} else {
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
int bitmask = (
(shooterButtonReady ? 1 : 0) +
(badShooterVelocity ? 2 : 0) +
(driverError ? 4 : 0)
);
switch (bitmask) {
case 0b000: // No errors but button is not pressed
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
m_robotLED.setMode(Constants.LEDConstants.OPREADY);
break;
case 0b001: // No errors and shoot button is pressed
io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
m_robotLED.setMode(Constants.LEDConstants.OPREADY);
break;
case 0b010: // Bad shooter velocity, button is not pressed
case 0b011: // Bad shooter velocty, button is pressed
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
break;
case 0b100: // Driver error, button is not pressed
case 0b101: // Driver error, button is pressed
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS);
break;
case 0b110: // Driver error, bad shooter vel, button is not pressed
case 0b111: // Driver error, bad shooter vel, button is pressed
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
break;
}
break;
case Ready:
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
break;
case ShootingFeeder:
case Feeding:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_FEED_VELOCITY.get()));
if(shooterButtonReady) {
io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
} else {
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
int bitmask2 = (
(shooterButtonReady ? 1 : 0) +
(badShooterVelocity ? 2 : 0)
);
switch (bitmask2) {
case 0b000: // No errors but button is not pressed
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED);
break;
case 0b001: // No errors and shoot button is pressed
io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED);
break;
case 0b010: // Bad shooter velocity, button is not pressed
case 0b011: // Bad shooter velocty, button is pressed
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
break;
// case 0b100: // Driver error, button is not pressed
// case 0b101: // Driver error, button is pressed
// m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
// io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
// break;
// case 0b110: // Driver error, bad shooter vel, button is not pressed
// case 0b111: // Driver error, bad shooter vel, button is pressed
// m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
// io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
// break;
}
break;
case ReadyFeeder:
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
break;
case Idle:
case NotReady:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get()));
io.setShooterCurrentLimitSpeed(
state,
ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get()
// Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()),
// RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get())
);
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN);
break;
}
}
}
}
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+6 -2
View File
@@ -18,14 +18,18 @@ public class ShooterConstants {
public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5;
public static final double INDEXER_GEAR_RATIO = 1.;
// public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40);
public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60);
public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35);
// public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", 0.3);
// public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.);
// public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10);
public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4);
public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0);
public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
// Shoot mode tolerances
src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java
+6
View File
@@ -38,6 +38,12 @@ public interface ShooterIO {
// public default void setShooterAngle(ShooterState state, Angle angle) {}
// public default void setShooterPitch(ShooterState state, Angle angle) {}
public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setShooterCurrentLimitSpeed(
ShooterState state,
double percentOutput
// Current currentLimit,
// AngularVelocity targetVelocity
) {}
// public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setIndexerOutput(ShooterState state, double percentOutput) {}
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+29
View File
@@ -1,9 +1,13 @@
package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
public class ShooterReal implements ShooterIO {
@@ -54,6 +58,31 @@ public class ShooterReal implements ShooterIO {
m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps));
}
@Override
public void setShooterCurrentLimitSpeed(
ShooterState state,
double percentOutput
// Current currentLimit,
// AngularVelocity targetVelocity
) {
// state.motor1TargetVelocity = targetVelocity;
// state.motor2TargetVelocity = targetVelocity;
// double current = Math.abs(state.motor1Current.in(Amps)) + Math.abs(state.motor2Current.in(Amps));
// double velocity = (Math.abs(state.motor1Velocity.in(RotationsPerSecond)) + Math.abs(state.motor2Velocity.in(RotationsPerSecond))) / 2;
// if(
// Math.abs(currentLimit.in(Amps)) > current &&
// Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity
// ) {
m_shooter1Motor.set(percentOutput);
m_shooter2Motor.set(percentOutput);
// } else {
// m_shooter1Motor.set(0);
// m_shooter2Motor.set(0);
// }
}
@Override
public void setIndexerOutput(ShooterState state, double percentOutput) {
state.indexerTargetOutput = percentOutput;
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+6
View File
@@ -4,6 +4,8 @@
package frc4388.robot.subsystems.swerve;
import static edu.wpi.first.units.Units.Rotations;
import java.util.function.Supplier;
import org.littletonrobotics.junction.AutoLogOutput;
@@ -245,6 +247,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
if(!TimesNegativeOne.isRed) {
leftStick.rotateBy(new Rotation2d(Math.PI/2.));
}
io.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
src/main/java/frc4388/utility/compute/FieldPositions.java
+1 -1
View File
@@ -6,7 +6,7 @@ public class FieldPositions {
// public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0);
// public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0);
public static final Translation2d RED_HUB_POSITION = new Translation2d(11.9014494, 4.0213534 + 0.3);
public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534 + 0.3);
public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534);// + 0.3);
// We set the default position to one just in case it doesn't update