mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Update to 2026.1.1. Add a lot of shooter boilerplate
This commit is contained in:
Michael Mikovsky
@@ -0,0 +1,58 @@
|
||||
package frc4388.robot.subsystems.shooter;
|
||||
|
||||
import static edu.wpi.first.units.Units.Rotation;
|
||||
|
||||
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.utility.status.FaultReporter;
|
||||
|
||||
public class Shooter extends SubsystemBase {
|
||||
ShooterIO io;
|
||||
ShooterStateAutoLogged state = new ShooterStateAutoLogged();
|
||||
|
||||
Supplier<Pose2d> m_swervePoseSupplier;
|
||||
|
||||
public Shooter(
|
||||
ShooterIO io,
|
||||
Supplier<Pose2d> swervePoseSupplier
|
||||
) {
|
||||
this.io = io;
|
||||
this.m_swervePoseSupplier = swervePoseSupplier;
|
||||
}
|
||||
|
||||
public enum FieldZone {
|
||||
// The robot should aim at the hub
|
||||
InShootZone,
|
||||
// The robot should aim towards the wall
|
||||
AimAtWall,
|
||||
|
||||
}
|
||||
|
||||
// Calculate what should be done based off of the position of the robot
|
||||
// TODO: Implement field zones
|
||||
public FieldZone getTarget(Pose2d position) {
|
||||
return FieldZone.InShootZone;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
|
||||
|
||||
// FaultReporter.register(this); // TODO Implement fault reporter
|
||||
|
||||
|
||||
Logger.processInputs("Shooter", state);
|
||||
|
||||
Pose2d pose = m_swervePoseSupplier.get();
|
||||
Angle robotRot = pose.getRotation().getMeasure();
|
||||
|
||||
io.updateInputs(state);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,71 @@
|
||||
package frc4388.robot.subsystems.shooter;
|
||||
|
||||
import static edu.wpi.first.units.Units.*;
|
||||
|
||||
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
|
||||
public class ShooterConstants {
|
||||
// Motor conversions
|
||||
public static final double ANGLE_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 FEEDER_INCHES_PER_ROT = 1.;
|
||||
|
||||
// Limits
|
||||
|
||||
// 0 is the forward angle on the robot.
|
||||
// negative is left, positive is right
|
||||
public static final Angle ANGLE_LIMIT_LEFT = Degrees.of(-180);
|
||||
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);
|
||||
|
||||
// Motor configs
|
||||
public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(40) // TODO: tune???
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.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 PITCH_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(40) // TODO: tune???
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.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()
|
||||
.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 FEEDER_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(40) // TODO: tune???
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
|
||||
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
);
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
package frc4388.robot.subsystems.shooter;
|
||||
|
||||
import static edu.wpi.first.units.Units.Amps;
|
||||
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 org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
import edu.wpi.first.units.CurrentUnit;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import edu.wpi.first.units.measure.Current;
|
||||
import edu.wpi.first.units.measure.LinearVelocity;
|
||||
|
||||
public interface ShooterIO {
|
||||
@AutoLog
|
||||
public class ShooterState {
|
||||
Angle shooterAngle = Rotations.of(0);
|
||||
Angle shooterTargetAngle = Rotations.of(0);
|
||||
Current angleMotorCurrent = Amps.of(0);
|
||||
|
||||
Angle shooterPitch = Rotations.of(0);
|
||||
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);
|
||||
|
||||
LinearVelocity feederVelocity = InchesPerSecond.of(0);
|
||||
LinearVelocity feederTargetVelocity = InchesPerSecond.of(0);
|
||||
Current feederMotorCurrent = 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 updateInputs(ShooterState state) {}
|
||||
}
|
||||
@@ -0,0 +1,111 @@
|
||||
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.controls.VelocityDutyCycle;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import edu.wpi.first.units.measure.*;
|
||||
|
||||
public class ShooterReal implements ShooterIO {
|
||||
|
||||
TalonFX m_angleMotor;
|
||||
TalonFX m_pitchMotor;
|
||||
TalonFX m_flywheelMotor;
|
||||
TalonFX m_feederMotor;
|
||||
|
||||
public ShooterReal(
|
||||
TalonFX angleMotor,
|
||||
TalonFX pitchMotor,
|
||||
TalonFX flywheelMotor,
|
||||
TalonFX feederMotor
|
||||
) {
|
||||
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);
|
||||
}
|
||||
|
||||
private Angle clampAng(Angle x, Angle min, Angle max){
|
||||
if(x.gt(max)) {
|
||||
return max;
|
||||
}else if(x.lt(min)) {
|
||||
return min;
|
||||
}else{
|
||||
return x;
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: Test
|
||||
@Override
|
||||
public void setShooterAngle(ShooterState state, Angle angle) {
|
||||
state.shooterTargetAngle = angle;
|
||||
// Assume that the angle is always accurate, because I think we will use a shaft encoder
|
||||
// Assume that 0 degrees = forwards. Might need an offset here
|
||||
|
||||
Angle boundedAngle = clampAng(angle, ShooterConstants.ANGLE_LIMIT_LEFT, ShooterConstants.ANGLE_LIMIT_RIGHT);
|
||||
// (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
|
||||
double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.ANGLE_MOTOR_GEAR_RATIO;
|
||||
PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
|
||||
m_angleMotor.setControl(posRequest);
|
||||
}
|
||||
|
||||
|
||||
// TODO: Test
|
||||
@Override
|
||||
public void setShooterPitch(ShooterState state, Angle angle) {
|
||||
state.shooterTargetPitch = angle;
|
||||
// TODO: Test
|
||||
// This assumes that the 0 is paralell to the ground. Might need an offset here
|
||||
|
||||
|
||||
Angle boundedAngle = clampAng(angle, ShooterConstants.PITCH_LIMIT_UPPER, ShooterConstants.PITCH_LIMIT_LOWER);
|
||||
// (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
|
||||
double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.PITCH_MOTOR_GEAR_RATIO;
|
||||
PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
|
||||
m_angleMotor.setControl(posRequest);
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
|
||||
@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.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();
|
||||
}
|
||||
|
||||
}
|
||||
@@ -4,6 +4,8 @@
|
||||
|
||||
package frc4388.robot.subsystems.swerve;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
@@ -315,6 +317,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
return state.currentPose;
|
||||
}
|
||||
|
||||
public Supplier<Pose2d> getPoseSupplier() {
|
||||
return () -> this.getPose2d();
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
io.tareEverything();
|
||||
robotKnowsWhereItIs = false;
|
||||
|
||||
Reference in New Issue
Block a user