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

Robot should be ready

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-09 17:18:54 -08:00
parent d90bddac0f
commit 51d2b80ea0
14 changed files with 183 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+14 -10
View File
@@ -1,5 +1,9 @@
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;
@@ -11,7 +15,7 @@ import frc4388.robot.subsystems.intake.IntakeConstants;
import frc4388.robot.subsystems.shooter.ShooterIO.ShooterState;
public class Shooter extends SubsystemBase {
ShooterIO io;
public ShooterIO io;
ShooterStateAutoLogged state = new ShooterStateAutoLogged();
// Supplier<Pose2d> m_swervePoseSupplier;
@@ -46,19 +50,19 @@ public class Shooter extends SubsystemBase {
public void setMode(ShooterMode mode) {
switch (mode) {
case Active:
io.setMotor1Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY);
io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY);
io.setIndexerVelocity(state, ShooterConstants.INDEXER_ACTIVE_VELOCITY);
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get()));
// io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY);
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_ACTIVE_VELOCITY.get()));
break;
case Resting:
io.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY);
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()));
break;
case Inactive:
io.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY);
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()));
break;
}
}
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+26 -14
View File
@@ -11,6 +11,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import frc4388.utility.configurable.ConfigurableDouble;
import frc4388.utility.status.CanDevice;
public class ShooterConstants {
@@ -20,16 +21,20 @@ public class ShooterConstants {
public static final double SHOOTERMOTOR1_GEAR_RATIO = 1.;
public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.;
public static final double INDEXER_GEAR_RATIO = 1.;
public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(0.0);
public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(-15);
public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(0);
public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
// public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(30);
// public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(15);
// public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
// public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(10);
// public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
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_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 10);
public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
public static Slot0Configs SHOOTER_PID = new Slot0Configs()
.withKV(0.0)
@@ -37,6 +42,12 @@ public class ShooterConstants {
.withKI(0.0)
.withKD(0.0);
public static Slot0Configs INDEXER_PID = new Slot0Configs()
.withKV(0.0)
.withKP(0.0)
.withKI(0.0)
.withKD(0.0);
// Limits
// 0 is the forward angle on the robot.
@@ -45,8 +56,8 @@ public class ShooterConstants {
// public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180);
// 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);
// public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
// public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
// Motor configs
// public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration()
@@ -61,10 +72,10 @@ public class ShooterConstants {
// );
public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22);
public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23);
public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24);
public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22);
public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23);
public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24);
public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
@@ -73,7 +84,7 @@ public class ShooterConstants {
.withStatorCurrentLimitEnable(true)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
public static final TalonFXConfiguration SHOOTER2_MOTOR_CONFIG = new TalonFXConfiguration()
@@ -86,6 +97,7 @@ public class ShooterConstants {
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java
+6 -3
View File
@@ -36,14 +36,17 @@ public interface ShooterIO {
Current motor2Current = Amps.of(0);
Current indexerCurrent = Amps.of(0);
LinearVelocity motorLinearVelocity = InchesPerSecond.of(0);
// LinearVelocity motorLinearVelocity = InchesPerSecond.of(0);
}
// public default void setShooterAngle(ShooterState state, Angle angle) {}
// public default void setShooterPitch(ShooterState state, Angle angle) {}
public default void setMotor1Velocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setMotor2Velocity(ShooterState state, AngularVelocity linearVelocity) {}
public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {}
// public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {}
public default void updateInputs(ShooterState state) {}
public default void updateGains() {}
}
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+44 -11
View File
@@ -8,12 +8,15 @@ import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import frc4388.robot.subsystems.intake.IntakeConstants;
import frc4388.utility.configurable.ConfigurableDouble;
public class ShooterReal implements ShooterIO {
TalonFX m_shooter1Motor;
TalonFX m_shooter2Motor;
TalonFX m_indexerMotor;
VelocityDutyCycle shooter1Velocity = new VelocityDutyCycle(0);
VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0);
VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
@@ -27,14 +30,18 @@ public class ShooterReal implements ShooterIO {
m_shooter1Motor = shooter1Motor;
m_shooter2Motor = shooter2Motor;
m_indexerMotor = indexerMotor;
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG);
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG);
m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG);
shooter1Velocity.Slot = 0;
shooter2Velocity.Slot = 0;
m_indexerVelocity.Slot = 0;
}
private Angle clampAng(Angle x, Angle min, Angle max){
@@ -83,34 +90,60 @@ public class ShooterReal implements ShooterIO {
@Override
public void setMotor1Velocity(ShooterState state, AngularVelocity target) {
public void setShooterVelocity(ShooterState state, AngularVelocity target) {
state.motor1TargetVelocity = target;
state.motor2TargetVelocity = target;
double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
m_shooter1Motor.setControl(new VelocityDutyCycle(motorRps));
m_shooter2Motor.setControl(new VelocityDutyCycle(motorRps));
AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO);
m_shooter1Motor.setControl(shooter1Velocity.withVelocity(motorRps));
m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps));
}
@Override
public void setIndexerVelocity(ShooterState state, AngularVelocity target) {
state.indexerTargetVelocity = target;
double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
m_indexerMotor.setControl(new VelocityDutyCycle(motorRps));
AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO);
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) {
state.motor1Velocity = m_shooter1Motor.getVelocity().getValue();
state.motor2Velocity = m_shooter2Motor.getVelocity().getValue();
state.indexerVelocity = m_indexerMotor.getVelocity().getValue();
state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().times(ShooterConstants.SHOOTERMOTOR1_GEAR_RATIO);
state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().times(ShooterConstants.SHOOTERMOTOR2_GEAR_RATIO);
state.indexerVelocity = m_indexerMotor.getVelocity().getValue().times(ShooterConstants.INDEXER_GEAR_RATIO);
state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
// state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue();
state.motor2Current = m_shooter2Motor.getStatorCurrent().getValue();
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();
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();
m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_PID);
}
}