diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 9940184..f34b88c 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -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 = 56; - public static final String GIT_SHA = "13b80c6341687f3972e9474507d1ef04edbf0773"; - public static final String GIT_DATE = "2026-02-14 13:50:55 MST"; - public static final String GIT_BRANCH = "operator-controls"; - public static final String BUILD_DATE = "2026-02-14 14:52:45 MST"; - public static final long BUILD_UNIX_TIME = 1771105965273L; + public static final int GIT_REVISION = 52; + public static final String GIT_SHA = "736d7ef8233e918aa0cc1cc3f06f2069517bed59"; + public static final String GIT_DATE = "2026-02-14 13:49:33 MST"; + public static final String GIT_BRANCH = "shoot-button"; + public static final String BUILD_DATE = "2026-02-14 14:55:59 MST"; + public static final long BUILD_UNIX_TIME = 1771106159811L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 7fd7859..55986e5 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -49,6 +49,7 @@ public class LED extends SubsystemBase implements Queryable { } } + @AutoLogOutput public String getMode(){ return mode.name(); } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 3d6f2e5..aaa965b 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -53,7 +53,7 @@ public class IntakeConstants { .withKI(0.0) .withKD(0.0); - public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2); + public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.1); public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0); public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 3c59906..5551815 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -61,9 +61,9 @@ public class Shooter extends SubsystemBase { private ShooterMode mode = ShooterMode.NotReady; public void setShooterReady() { - // if(this.mode == ShooterMode.NotReady) { - this.mode = ShooterMode.Shooting; - // } + if(this.mode == ShooterMode.NotReady) { + this.mode = ShooterMode.Ready; + } } public void setShooterNotReady() { @@ -98,52 +98,53 @@ public class Shooter extends SubsystemBase { // 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(); + boolean driverError = false; + // 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 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 = shooterSpeed < ShooterConstants.SHOOTER_SPEED_TOLERANCE.get(); - // boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended; + boolean badShooterVelocity = Math.abs(shooterSpeed - shooterSpeedTarget) > ShooterConstants.SHOOTER_SPEED_TOLERANCE.get(); + // boolean intakeBad = m_Intake.getMode() == IntxakeMode.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; - // } + 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 - // ); + mode = ( + bitmask == 0 ? + ShooterMode.Shooting : + ShooterMode.Ready + ); } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 289dad9..1a20a91 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -12,9 +12,7 @@ import frc4388.utility.status.CanDevice; public class ShooterConstants { // Motor conversions - public static final double FEEDER_INCHES_PER_ROT = 1.; - public static final double SHOOTERMOTOR1_GEAR_RATIO = 1.; - public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.; + public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5; public static final double INDEXER_GEAR_RATIO = 1.; // public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(30); @@ -40,15 +38,15 @@ public class ShooterConstants { 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 SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 1); + public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 5); public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) .withKP(0.0) - .withKI(0.0) - .withKD(0.2); + .withKI(0.1) + .withKD(0.08); public static Slot0Configs INDEXER_PID = new Slot0Configs() .withKV(0.0) @@ -61,8 +59,8 @@ public class ShooterConstants { public static ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KI", 0); public static ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KD", 0); - public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2); - public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0); + public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08); + public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.1); public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); // Limits diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index d07ba5c..a8e9479 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -49,7 +49,7 @@ public class ShooterReal implements ShooterIO { return; } - AngularVelocity motorRps = target.times(ShooterConstants.INDEXER_GEAR_RATIO); + AngularVelocity motorRps = target.times(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO); m_shooter1Motor.setControl(shooter1Velocity.withVelocity(motorRps)); m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps)); @@ -72,8 +72,8 @@ public class ShooterReal implements ShooterIO { @Override public void updateInputs(ShooterState state) { - state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR1_GEAR_RATIO); - state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR2_GEAR_RATIO); + state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO); + state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO); state.indexerVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); // state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);