Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -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.signals.NeutralModeValue;
import edu.wpi.first.wpilibj.motorcontrol.Talon;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.utility.configurable.ConfigurableDouble;
@@ -19,7 +20,7 @@ public class TestRobot extends SubsystemBase {
// TalonFX m_intakeMotor;
// TalonFX m_armMotor;
TalonFX m_intakeMotor;
// TalonFX m_storageMotor;
TalonFX m_outerShooter;
TalonFX m_innerShooter;
@@ -50,19 +51,19 @@ public class TestRobot extends SubsystemBase {
public TestRobot(
// TalonFX intakeMotor,
// TalonFX armMotor,
TalonFX intakeMotor,
// TalonFX storageMotor,
TalonFX outerShooter,
TalonFX innerShooter
) {
// m_intakeMotor = intakeMotor;
// m_armMotor = armMotor;
m_intakeMotor = intakeMotor;
// m_storageMotor = storageMotor;
m_outerShooter = outerShooter;
m_innerShooter = innerShooter;
// m_intakeMotor.getConfigurator().apply(INTAKE_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_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG);
@@ -94,17 +95,16 @@ public class TestRobot extends SubsystemBase {
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
// );
public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
.withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
// public static final TalonFXConfiguration STORAGE_MOTOR_CONFIG = new TalonFXConfiguration()
// .withCurrentLimits(
// new CurrentLimitsConfigs()
// .withStatorCurrentLimit(40) // TODO: tune???
// .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
// ).withMotorOutput(
// new MotorOutputConfigs()
// .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
// );
public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+7 -4
View File
@@ -1,6 +1,6 @@
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.MotorOutputConfigs;
@@ -16,6 +16,9 @@ public class ShooterConstants {
public static final double PITCH_MOTOR_GEAR_RATIO = 1.;
public static final double FLYWHEEL_GEAR_RATIO = 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
@@ -44,7 +47,7 @@ public class ShooterConstants {
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(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
@@ -54,7 +57,7 @@ public class ShooterConstants {
.withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
.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(
new CurrentLimitsConfigs()
.withStatorCurrentLimit(40) // TODO: tune???
@@ -64,7 +67,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 FEEDER_MOTOR_CONFIG = new TalonFXConfiguration()
public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
.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);
// Current pitchMotorCurrent = Amps.of(0);
AngularVelocity flywheelVelocity = RotationsPerSecond.of(0);
AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0);
Current flywheelMotorCurrent = Amps.of(0);
AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0);
AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0);
AngularVelocity indexerTargetVelocity = RotationsPerSecond.of(0);
Current motor1Current = Amps.of(0);
LinearVelocity feederVelocity = InchesPerSecond.of(0);
LinearVelocity feederTargetVelocity = InchesPerSecond.of(0);
Current feederMotorCurrent = Amps.of(0);
LinearVelocity motorLinearVelocity = InchesPerSecond.of(0);
AngularVelocity motorVelocity = RotationsPerSecond.of(0);
AngularVelocity motorTargetVelocity = RotationsPerSecond.of(0);
Current motor2Current = Amps.of(0);
}
// public default void setShooterAngle(ShooterState state, Angle angle) {}
// public default void setShooterPitch(ShooterState state, Angle angle) {}
public default void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {}
public default void setMotor1Velocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setMotor2Velocity(ShooterState state, AngularVelocity linearVelocity) {}
public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {}
public default void updateInputs(ShooterState state) {}
}
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+77 -43
View File
@@ -1,38 +1,40 @@
package frc4388.robot.subsystems.shooter;
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 com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
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(
// TalonFX angleMotor,
// TalonFX pitchMotor,
TalonFX flywheelMotor,
TalonFX feederMotor
TalonFX shooter1Motor,
TalonFX shooter2Motor,
TalonFX indexerMotor
) {
// m_angleMotor = angleMotor;
// m_pitchMotor = pitchMotor;
m_flywheelMotor = flywheelMotor;
m_feederMotor = feederMotor;
// Apply the configs
// m_angleMotor.getConfigurator().apply(ShooterConstants.ANGLE_MOTOR_CONFIG);
// m_pitchMotor.getConfigurator().apply(ShooterConstants.PITCH_MOTOR_CONFIG);
m_flywheelMotor.getConfigurator().apply(ShooterConstants.FLYWHEEL_MOTOR_CONFIG);
m_feederMotor.getConfigurator().apply(ShooterConstants.FEEDER_MOTOR_CONFIG);
m_shooter1Motor= shooter1Motor;
m_shooter2Motor= shooter2Motor;
m_indexerMotor = indexerMotor;
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG);
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG);
m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG);
}
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
// @Override
// public void setShooterAngle(ShooterState state, Angle angle) {
@@ -75,37 +96,50 @@ public class ShooterReal implements ShooterIO {
// 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
public void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {
state.flywheelTargetVelocity = angularVelocity;
// (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC)
double motorSpeed = angularVelocity.in(RotationsPerSecond) / ShooterConstants.FLYWHEEL_GEAR_RATIO;
VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed);
m_feederMotor.setControl(velocity);
public void setMotor1Velocity(ShooterState state, AngularVelocity target) {
state.motor2TargetVelocity = target;
double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
m_indexerMotor.setControl(new VelocityDutyCycle(motorRps));
}
@Override
public void setMotor2Velocity(ShooterState state, AngularVelocity target) {
state.motor2TargetVelocity = target;
double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
m_indexerMotor.setControl(new VelocityDutyCycle(motorRps));
}
@Override
public void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {
state.feederTargetVelocity = linearVelocity;
// (IN / SEC) * (ROT / IN) = (ROT / SEC)
double motorSpeed = linearVelocity.in(InchesPerSecond) / ShooterConstants.FEEDER_INCHES_PER_ROT;
VelocityDutyCycle velRequest = new VelocityDutyCycle(motorSpeed);
m_feederMotor.setControl(velRequest);
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
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.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
state.motorVelocity = m_shooter1Motor.getVelocity().getValue();
state.motorVelocity = m_shooter2Motor.getVelocity().getValue();
state.motorVelocity = m_indexerMotor.getVelocity().getValue();
state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue();
state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue();
state.feederVelocity = InchesPerSecond.of(m_feederMotor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
state.feederMotorCurrent = m_feederMotor.getStatorCurrent().getValue();
state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue();
}
}