From 6ce6d0eb0b8faec5af448ec23ffca156303cbed5 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 9 Feb 2026 18:38:55 -0800 Subject: [PATCH] Get IDK to work --- .../subsystems/intake/IntakeConstants.java | 9 ++++++ .../robot/subsystems/intake/IntakeReal.java | 26 ++++++++--------- .../robot/subsystems/shooter/Shooter.java | 12 ++------ .../subsystems/shooter/ShooterConstants.java | 13 +++++++-- .../robot/subsystems/shooter/ShooterReal.java | 28 +++++++++---------- 5 files changed, 48 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 9d1fc48..f3c8aad 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -57,6 +57,15 @@ public class IntakeConstants { .withKI(0.0) .withKD(0.0); + public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2); + public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KP", 0); + public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KP", 0); + + public static ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2); + public static ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0); + public static ConfigurableDouble roller_kD = new ConfigurableDouble("Roller KD", 0); + + // 0 is paralell to the ground, 90 is directly up // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index ce89c55..ea9c61a 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -58,6 +58,12 @@ public class IntakeReal implements IntakeIO { @Override public void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) { state.rollerTargetVelocity = angularVelocity; + + if(angularVelocity.baseUnitMagnitude() == 0) { + m_rollerMotor.set(0); + return; + } + // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) AngularVelocity motorSpeed = angularVelocity.div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO); @@ -80,14 +86,6 @@ public class IntakeReal implements IntakeIO { m_armMotor.setControl(armPosition.withPosition(motorAngle)); } - ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2); - ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KP", 0); - ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KP", 0); - - ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2); - ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0); - ConfigurableDouble roller_kD = new ConfigurableDouble("Roller KD", 0); - @Override public void updateInputs(IntakeState state) { state.armAngle = m_armMotor.getPosition().getValue().times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); @@ -100,14 +98,14 @@ public class IntakeReal implements IntakeIO { @Override public void updateGains() { - IntakeConstants.ARM_PID.kP = arm_kP.get(); - IntakeConstants.ARM_PID.kI = arm_kI.get(); - IntakeConstants.ARM_PID.kD = arm_kD.get(); + IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get(); + IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get(); + IntakeConstants.ARM_PID.kD = IntakeConstants.arm_kD.get(); m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG); - IntakeConstants.ROLLER_PID.kP = roller_kP.get(); - IntakeConstants.ROLLER_PID.kI = roller_kI.get(); - IntakeConstants.ROLLER_PID.kD = roller_kD.get(); + IntakeConstants.ROLLER_PID.kP = IntakeConstants.roller_kP.get(); + IntakeConstants.ROLLER_PID.kI = IntakeConstants.roller_kI.get(); + IntakeConstants.ROLLER_PID.kD = IntakeConstants.roller_kD.get(); m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 488d634..869ed14 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -1,18 +1,11 @@ package frc4388.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.Rotation; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; -import java.util.function.Supplier; - 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.robot.subsystems.intake.IntakeConstants; -import frc4388.robot.subsystems.shooter.ShooterIO.ShooterState; public class Shooter extends SubsystemBase { public ShooterIO io; @@ -47,6 +40,7 @@ public class Shooter extends SubsystemBase { Inactive, } + public void setMode(ShooterMode mode) { switch (mode) { case Active: @@ -57,12 +51,12 @@ public class Shooter extends SubsystemBase { case Resting: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get())); + io.setIndexerVelocity(state, RotationsPerSecond.of(0)); break; case Inactive: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get())); + io.setIndexerVelocity(state, RotationsPerSecond.of(0)); break; } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 69b7f1a..f398491 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -31,10 +31,10 @@ 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 SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 10); - public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); + // public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) @@ -47,6 +47,15 @@ public class ShooterConstants { .withKP(0.0) .withKI(0.0) .withKD(0.0); + + + public static ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2); + public static ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KP", 0); + public static ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KP", 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_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 4d1b2c9..8aee915 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -94,6 +94,12 @@ public class ShooterReal implements ShooterIO { state.motor1TargetVelocity = target; state.motor2TargetVelocity = target; + if(target.baseUnitMagnitude() == 0) { + m_shooter1Motor.set(0); + m_shooter2Motor.set(0); + return; + } + AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO); m_shooter1Motor.setControl(shooter1Velocity.withVelocity(motorRps)); @@ -107,14 +113,6 @@ public class ShooterReal implements ShooterIO { m_indexerMotor.setControl(m_indexerVelocity.withVelocity(motorRps)); } - - ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2); - ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KP", 0); - ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KP", 0); - - ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2); - ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0); - ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); @Override public void updateInputs(ShooterState state) { @@ -130,19 +128,19 @@ public class ShooterReal implements ShooterIO { state.indexerCurrent = m_indexerMotor.getStatorCurrent().getValue(); } - + @Override public void updateGains() { // TEMPORARY PIDs - ShooterConstants.SHOOTER_PID.kP = shooter_kP.get(); - ShooterConstants.SHOOTER_PID.kI = shooter_kI.get(); - ShooterConstants.SHOOTER_PID.kD = shooter_kD.get(); + ShooterConstants.SHOOTER_PID.kP = ShooterConstants.shooter_kP.get(); + ShooterConstants.SHOOTER_PID.kI = ShooterConstants.shooter_kI.get(); + ShooterConstants.SHOOTER_PID.kD = ShooterConstants.shooter_kD.get(); m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); - ShooterConstants.INDEXER_PID.kP = indexer_kP.get(); - ShooterConstants.INDEXER_PID.kI = indexer_kI.get(); - ShooterConstants.INDEXER_PID.kD = indexer_kD.get(); + ShooterConstants.INDEXER_PID.kP = ShooterConstants.indexer_kP.get(); + ShooterConstants.INDEXER_PID.kI = ShooterConstants.indexer_kI.get(); + ShooterConstants.INDEXER_PID.kD = ShooterConstants.indexer_kD.get(); m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_PID); }