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

Shoooterrr

Browse Source
This commit is contained in:
Shikhar
2026-01-27 18:13:51 -07:00
parent 7b71cfb9d2
commit 42e911fbdc
4 changed files with 110 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/TestRobot.java
+15 -15
View File
@@ -11,6 +11,7 @@ import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.wpilibj.motorcontrol.Talon;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.configurable.ConfigurableDouble;
@@ -19,7 +20,7 @@ public class TestRobot extends SubsystemBase {
// TalonFX m_intakeMotor; // TalonFX m_intakeMotor;
// TalonFX m_armMotor; // TalonFX m_armMotor;
TalonFX m_intakeMotor; // TalonFX m_storageMotor;
TalonFX m_outerShooter; TalonFX m_outerShooter;
TalonFX m_innerShooter; TalonFX m_innerShooter;
@@ -50,19 +51,19 @@ public class TestRobot extends SubsystemBase {
public TestRobot( public TestRobot(
// TalonFX intakeMotor, // TalonFX intakeMotor,
// TalonFX armMotor, // TalonFX armMotor,
TalonFX intakeMotor, // TalonFX storageMotor,
TalonFX outerShooter, TalonFX outerShooter,
TalonFX innerShooter TalonFX innerShooter
) { ) {
// m_intakeMotor = intakeMotor; // m_intakeMotor = intakeMotor;
// m_armMotor = armMotor; // m_armMotor = armMotor;
m_intakeMotor = intakeMotor; // m_storageMotor = storageMotor;
m_outerShooter = outerShooter; m_outerShooter = outerShooter;
m_innerShooter = innerShooter; m_innerShooter = innerShooter;
// m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG); // m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG);
// m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG); // m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG);
m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG); // m_storageMotor.getConfigurator().apply(STORAGE_MOTOR_CONFIG);
m_outerShooter.getConfigurator().apply(OUTER_MOTOR_CONFIG); m_outerShooter.getConfigurator().apply(OUTER_MOTOR_CONFIG);
m_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG); m_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG);
@@ -94,17 +95,16 @@ public class TestRobot extends SubsystemBase {
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
// ); // );
public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration() // public static final TalonFXConfiguration STORAGE_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits( // .withCurrentLimits(
new CurrentLimitsConfigs() // new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune??? // .withStatorCurrentLimit(40) // TODO: tune???
.withStatorCurrentLimitEnable(true) // TODO: Figure out what this means // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
).withMotorOutput( // ).withMotorOutput(
new MotorOutputConfigs() // new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny // .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
); // );
public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration() public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits( .withCurrentLimits(
new CurrentLimitsConfigs() new CurrentLimitsConfigs()
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+7 -4
View File
@@ -1,6 +1,6 @@
package frc4388.robot.subsystems.shooter; package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.Degrees;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs;
@@ -16,6 +16,9 @@ public class ShooterConstants {
public static final double PITCH_MOTOR_GEAR_RATIO = 1.; public static final double PITCH_MOTOR_GEAR_RATIO = 1.;
public static final double FLYWHEEL_GEAR_RATIO = 1.; public static final double FLYWHEEL_GEAR_RATIO = 1.;
public static final double FEEDER_INCHES_PER_ROT = 1.; 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 INDEXER_GEAR_RATIO = 1.;
// Limits // Limits
@@ -44,7 +47,7 @@ public class ShooterConstants {
public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22);
} }
public static final TalonFXConfiguration PITCH_MOTOR_CONFIG = new TalonFXConfiguration() public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits( .withCurrentLimits(
new CurrentLimitsConfigs() new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune??? .withStatorCurrentLimit(40) // TODO: tune???
@@ -54,7 +57,7 @@ public class ShooterConstants {
.withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
); );
public static final TalonFXConfiguration FLYWHEEL_MOTOR_CONFIG = new TalonFXConfiguration() public static final TalonFXConfiguration SHOOTER2_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits( .withCurrentLimits(
new CurrentLimitsConfigs() new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune??? .withStatorCurrentLimit(40) // TODO: tune???
@@ -64,7 +67,7 @@ public class ShooterConstants {
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
); );
public static final TalonFXConfiguration FEEDER_MOTOR_CONFIG = new TalonFXConfiguration() public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits( .withCurrentLimits(
new CurrentLimitsConfigs() new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune??? .withStatorCurrentLimit(40) // TODO: tune???
src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java
+11 -8
View File
@@ -24,19 +24,22 @@ public interface ShooterIO {
// Angle shooterTargetPitch = Rotations.of(0); // Angle shooterTargetPitch = Rotations.of(0);
// Current pitchMotorCurrent = Amps.of(0); // Current pitchMotorCurrent = Amps.of(0);
AngularVelocity flywheelVelocity = RotationsPerSecond.of(0); AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0);
AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0); AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0);
Current flywheelMotorCurrent = Amps.of(0); AngularVelocity indexerTargetVelocity = RotationsPerSecond.of(0);
Current motor1Current = Amps.of(0);
LinearVelocity feederVelocity = InchesPerSecond.of(0); LinearVelocity motorLinearVelocity = InchesPerSecond.of(0);
LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); AngularVelocity motorVelocity = RotationsPerSecond.of(0);
Current feederMotorCurrent = Amps.of(0); AngularVelocity motorTargetVelocity = RotationsPerSecond.of(0);
Current motor2Current = Amps.of(0);
} }
// public default void setShooterAngle(ShooterState state, Angle angle) {} // public default void setShooterAngle(ShooterState state, Angle angle) {}
// public default void setShooterPitch(ShooterState state, Angle angle) {} // public default void setShooterPitch(ShooterState state, Angle angle) {}
public default void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {} public default void setMotor1Velocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {} public default void setMotor2Velocity(ShooterState state, AngularVelocity linearVelocity) {}
public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {}
public default void updateInputs(ShooterState state) {} public default void updateInputs(ShooterState state) {}
} }
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+76 -42
View File
@@ -1,38 +1,40 @@
package frc4388.robot.subsystems.shooter; package frc4388.robot.subsystems.shooter;
import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.InchesPerSecond;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.units.measure.*; import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.utility.configurable.ConfigurableDouble;
public class ShooterReal implements ShooterIO { public class ShooterReal extends SubsystemBase 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);
// TalonFX m_angleMotor;
// TalonFX m_pitchMotor;
TalonFX m_flywheelMotor;
TalonFX m_feederMotor;
public ShooterReal( public ShooterReal(
// TalonFX angleMotor, TalonFX shooter1Motor,
// TalonFX pitchMotor, TalonFX shooter2Motor,
TalonFX flywheelMotor, TalonFX indexerMotor
TalonFX feederMotor
) { ) {
// m_angleMotor = angleMotor; m_shooter1Motor= shooter1Motor;
// m_pitchMotor = pitchMotor; m_shooter2Motor= shooter2Motor;
m_flywheelMotor = flywheelMotor; m_indexerMotor = indexerMotor;
m_feederMotor = feederMotor;
// Apply the configs
// m_angleMotor.getConfigurator().apply(ShooterConstants.ANGLE_MOTOR_CONFIG); m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG);
// m_pitchMotor.getConfigurator().apply(ShooterConstants.PITCH_MOTOR_CONFIG); m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG);
m_flywheelMotor.getConfigurator().apply(ShooterConstants.FLYWHEEL_MOTOR_CONFIG); m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG);
m_feederMotor.getConfigurator().apply(ShooterConstants.FEEDER_MOTOR_CONFIG);
} }
private Angle clampAng(Angle x, Angle min, Angle max){ private Angle clampAng(Angle x, Angle min, Angle max){
@@ -45,6 +47,25 @@ public class ShooterReal implements ShooterIO {
} }
} }
public static Slot0Configs SHOOTER_PID;
static {
SHOOTER_PID = new Slot0Configs();
SHOOTER_PID.kV = 0.0;
SHOOTER_PID.kP = 0.0;
SHOOTER_PID.kI = 0.0;
SHOOTER_PID.kD = 0.0;
}
ConfigurableDouble kV = new ConfigurableDouble("Shooter kV", 0.12);
ConfigurableDouble kP = new ConfigurableDouble("Shooter kP", 0.11);
ConfigurableDouble kI = new ConfigurableDouble("Shooter kI", 0.48);
ConfigurableDouble kD = new ConfigurableDouble("Shooter kD", 0.01);
// // TODO: Test // // TODO: Test
// @Override // @Override
// public void setShooterAngle(ShooterState state, Angle angle) { // public void setShooterAngle(ShooterState state, Angle angle) {
@@ -75,37 +96,50 @@ public class ShooterReal implements ShooterIO {
// m_pitchMotor.setControl(posRequest); // m_pitchMotor.setControl(posRequest);
// } // }
@Override
public void periodic() {
SHOOTER_PID = new Slot0Configs();
SHOOTER_PID.kV = kV.get();
SHOOTER_PID.kP = kP.get();
SHOOTER_PID.kI = kI.get();
SHOOTER_PID.kD = kD.get();
m_shooter1Motor.getConfigurator().apply(SHOOTER_PID);
m_shooter2Motor.getConfigurator().apply(SHOOTER_PID);
}
@Override @Override
public void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) { public void setMotor1Velocity(ShooterState state, AngularVelocity target) {
state.flywheelTargetVelocity = angularVelocity; state.motor2TargetVelocity = target;
// (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
double motorSpeed = angularVelocity.in(RotationsPerSecond) / ShooterConstants.FLYWHEEL_GEAR_RATIO; m_indexerMotor.setControl(new VelocityDutyCycle(motorRps));
VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed);
m_feederMotor.setControl(velocity);
} }
@Override @Override
public void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) { public void setMotor2Velocity(ShooterState state, AngularVelocity target) {
state.feederTargetVelocity = linearVelocity; state.motor2TargetVelocity = target;
// (IN / SEC) * (ROT / IN) = (ROT / SEC) double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
double motorSpeed = linearVelocity.in(InchesPerSecond) / ShooterConstants.FEEDER_INCHES_PER_ROT; m_indexerMotor.setControl(new VelocityDutyCycle(motorRps));
VelocityDutyCycle velRequest = new VelocityDutyCycle(motorSpeed); }
m_feederMotor.setControl(velRequest);
@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));
} }
@Override @Override
public void updateInputs(ShooterState state) { public void updateInputs(ShooterState state) {
// state.shooterAngle = m_angleMotor.getPosition().getValue().times(ShooterConstants.ANGLE_MOTOR_GEAR_RATIO);
// state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue();
// state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO); state.motorVelocity = m_shooter1Motor.getVelocity().getValue();
// state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); state.motorVelocity = m_shooter2Motor.getVelocity().getValue();
state.motorVelocity = m_indexerMotor.getVelocity().getValue();
state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue(); state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue(); state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue();
state.feederVelocity = InchesPerSecond.of(m_feederMotor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
state.feederMotorCurrent = m_feederMotor.getStatorCurrent().getValue();
} }
} }