Skip to content

Commit 18b1c72

Browse files
OrcasphynxOrcasphynx
committed
Merge branch 'feature/shooter' into development
2 parents ff09a22 + 0bc3646 commit 18b1c72

File tree

4 files changed

+267
-0
lines changed

4 files changed

+267
-0
lines changed

‎src/main/java/frc/robot/BuildConstants.java‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
package frc.robot;
2+
3+
/** Automatically generated file containing build version information. */
4+
public final class BuildConstants {
5+
public static final String MAVEN_GROUP = "";
6+
public static final String MAVEN_NAME = "Robot-2026";
7+
public static final String VERSION = "unspecified";
8+
public static final int GIT_REVISION = 85;
9+
public static final String GIT_SHA = "ff09a2297c8c71d5901bc155be4759e33416b164";
10+
public static final String GIT_DATE = "2026-02-17 20:00:20 EST";
11+
public static final String GIT_BRANCH = "development";
12+
public static final String BUILD_DATE = "2026-02-17 20:13:19 EST";
13+
public static final long BUILD_UNIX_TIME = 1771377199347L;
14+
public static final int DIRTY = 0;
15+
16+
private BuildConstants() {}
17+
}

‎src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
package frc.robot.subsystems.shooter;
2+
3+
import static edu.wpi.first.units.Units.*;
4+
5+
import com.ctre.phoenix6.configs.Slot0Configs;
6+
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
7+
import com.ctre.phoenix6.controls.DutyCycleOut;
8+
import edu.wpi.first.units.measure.Angle;
9+
import edu.wpi.first.units.measure.Current;
10+
import edu.wpi.first.units.measure.Distance;
11+
12+
public class ShooterConstants {
13+
14+
private ShooterConstants() {}
15+
16+
// TODO: Change to actual ports
17+
public static final int FLYWHEEL_MOTOR_ID = 4;
18+
public static final int HOOD_MOTOR_ID = 5;
19+
20+
// TODO: Tune Flywheel and Hood Motor
21+
22+
// Flywheel motor
23+
public static final double FLYWHEEL_KS = 0;
24+
public static final double FLYWHEEL_KV = 0;
25+
public static final double FLYWHEEL_KP = 0;
26+
public static final double FLYWHEEL_KD = 0;
27+
28+
public static Slot0Configs createFlywheelMotorSlot0Configs() {
29+
Slot0Configs slot = new Slot0Configs();
30+
slot.kS = FLYWHEEL_KS;
31+
slot.kV = FLYWHEEL_KV;
32+
slot.kP = FLYWHEEL_KP;
33+
slot.kD = FLYWHEEL_KD;
34+
return slot;
35+
}
36+
37+
public static final double ALLOWABLE_HOOD_ERROR = 0.1;
38+
public static final double HOOD_FORWARD_LIMIT = 100;
39+
public static final double HOOD_REVERSE_LIMIT = 0;
40+
public static final double HOOD_KS = 0;
41+
public static final double HOOD_KP = 0;
42+
public static final double HOOD_KD = 0;
43+
44+
public static Slot0Configs createHoodMotorSlot0Configs() {
45+
Slot0Configs slot = new Slot0Configs();
46+
slot.kS = HOOD_KS;
47+
slot.kP = HOOD_KP;
48+
slot.kD = HOOD_KD;
49+
return slot;
50+
}
51+
52+
public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() {
53+
SoftwareLimitSwitchConfigs configs = new SoftwareLimitSwitchConfigs();
54+
configs.ForwardSoftLimitEnable = false;
55+
configs.ReverseSoftLimitEnable = false;
56+
configs.ForwardSoftLimitThreshold = HOOD_FORWARD_LIMIT;
57+
configs.ReverseSoftLimitThreshold = HOOD_REVERSE_LIMIT;
58+
return configs;
59+
}
60+
61+
public static final DutyCycleOut SAFE_HOMING_EFFORT = new DutyCycleOut(-0.2);
62+
public static final Current SAFE_STATOR_LIMIT = Amp.of(0.8);
63+
64+
// Conversion Constants
65+
public static final Angle ROTATIONS_PER_LAUNCH_DEGREE =
66+
Rotations.of(1); // TODO: Get Better Estimate
67+
public static final Distance FLYWHEEL_RADIUS = Inch.of(1); // TODO: Get Better Estimate
68+
69+
// Lemon Yeeting Constants
70+
public static final Distance SHOOTER_HEIGHT = Inch.of(65); // TODO: Get Better Estimate
71+
public static final Distance GOAL_HEIGHT = Inch.of(23); // TODO: Get Better Estimate
72+
public static final Distance OFFSET_DISTANCE = Meter.of(1); // TODO: Get Better Estimate
73+
public static final Distance MIN_VERTEX_DISTANCE = Inch.of(23.5);
74+
public static final Angle MIN_HOOD_ANGLE = Degrees.of(0); // TODO: Get Better Estimate
75+
public static final double OPTIMAL_ENTRY_SLOPE = -1; // TODO: Tune
76+
}

‎src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
package frc.robot.subsystems.shooter;
2+
3+
import frc.robot.preferences.DoublePreference;
4+
5+
public class ShooterPreferences {
6+
private ShooterPreferences() {}
7+
8+
// TODO: Replace with formulas
9+
public static DoublePreference flywheelLaunchSpeed =
10+
new DoublePreference("Shooter/Launch Speed", 0.1); // in rotations per second
11+
public static DoublePreference hoodLaunchAngle =
12+
new DoublePreference("Shooter/Hood Launch Position", 3); // in rotations
13+
}

‎src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java‎

Lines changed: 161 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,161 @@
1+
package frc.robot.subsystems.shooter;
2+
3+
import static edu.wpi.first.units.Units.*;
4+
5+
import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
6+
import com.ctre.phoenix6.controls.VelocityVoltage;
7+
import com.ctre.phoenix6.hardware.TalonFX;
8+
import edu.wpi.first.epilogue.Logged;
9+
import edu.wpi.first.epilogue.Logged.Importance;
10+
import edu.wpi.first.units.measure.Angle;
11+
import edu.wpi.first.units.measure.AngularVelocity;
12+
import edu.wpi.first.units.measure.LinearVelocity;
13+
import edu.wpi.first.wpilibj2.command.Command;
14+
import edu.wpi.first.wpilibj2.command.Commands;
15+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
16+
import frc.robot.subsystems.intake.IntakeConstants;
17+
18+
@Logged
19+
public class ShooterSubsystem extends SubsystemBase {
20+
private final TalonFX flywheelMotor;
21+
private final TalonFX hoodMotor;
22+
23+
@Logged(name = "Velocity Target", importance = Importance.CRITICAL)
24+
private AngularVelocity velocityTarget; // Rotations Per Second
25+
26+
private VelocityVoltage velocityControl;
27+
28+
@Logged(name = "Hood Target", importance = Importance.CRITICAL)
29+
private Angle hoodTarget; // Rotations
30+
31+
private PositionTorqueCurrentFOC hoodControl;
32+
33+
public class LaunchRequest {
34+
private Angle launchHoodTarget;
35+
private AngularVelocity launchVelocity;
36+
37+
public LaunchRequest(Angle theta, LinearVelocity velocity) {
38+
// hood angle in degrees (0 degrees is all the way back) is converted to motor rotations
39+
launchHoodTarget =
40+
Rotations.of(
41+
theta.in(Degrees) * ShooterConstants.ROTATIONS_PER_LAUNCH_DEGREE.in(Rotations));
42+
// meters per second is converted to rotations per second of the flywheel
43+
launchVelocity =
44+
RotationsPerSecond.of(
45+
velocity.in(MetersPerSecond)
46+
/ (2 * Math.PI * ShooterConstants.FLYWHEEL_RADIUS.in(Meters)));
47+
}
48+
49+
public Angle getHoodTarget() {
50+
return launchHoodTarget;
51+
}
52+
53+
public AngularVelocity getVelocityTarget() {
54+
return launchVelocity;
55+
}
56+
}
57+
58+
public ShooterSubsystem() {
59+
flywheelMotor = new TalonFX(ShooterConstants.FLYWHEEL_MOTOR_ID);
60+
hoodMotor = new TalonFX(ShooterConstants.HOOD_MOTOR_ID);
61+
62+
flywheelMotor.getConfigurator().apply(ShooterConstants.createFlywheelMotorSlot0Configs());
63+
velocityTarget = RotationsPerSecond.of(0);
64+
velocityControl = new VelocityVoltage(0);
65+
66+
hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorSlot0Configs());
67+
hoodMotor.getConfigurator().apply(ShooterConstants.createHoodSoftwareLimitSwitchConfigs());
68+
hoodTarget = Rotations.of(0);
69+
hoodControl = new PositionTorqueCurrentFOC(0);
70+
}
71+
72+
@Override
73+
public void periodic() {
74+
flywheelMotor.setControl(velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond)));
75+
hoodMotor.setControl(hoodControl.withVelocity(hoodTarget.in(Rotations)));
76+
}
77+
78+
public Command spinFlywheelCommand() {
79+
return runOnce(
80+
() ->
81+
velocityTarget =
82+
RotationsPerSecond.of(ShooterPreferences.flywheelLaunchSpeed.getValue()))
83+
.withName("Start Spinning Flywheel");
84+
}
85+
86+
public Command stopFlywheelCommand() {
87+
return runOnce(() -> velocityTarget = RotationsPerSecond.of(0))
88+
.withName("Stop Spinning Flywheel");
89+
}
90+
91+
@Logged(name = "At Hood Setpoint", importance = Importance.CRITICAL)
92+
public boolean atHoodSetpoint() {
93+
return Math.abs(hoodMotor.getPosition().getValueAsDouble() - hoodTarget.in(Rotations))
94+
< IntakeConstants.ALLOWABLE_EXTENSION_ERROR;
95+
}
96+
97+
public Command setHoodCommand(Angle position) {
98+
return runOnce(() -> hoodTarget = position)
99+
.andThen(Commands.waitUntil(() -> atHoodSetpoint()))
100+
.withName("Set Hood Angle");
101+
}
102+
103+
public Command launchLemonsCommand() {
104+
return setHoodCommand(Rotations.of(ShooterPreferences.hoodLaunchAngle.getValue()))
105+
.andThen(spinFlywheelCommand())
106+
.withName("Start Launching Lemons");
107+
}
108+
109+
public Command stowCommand() {
110+
return stopFlywheelCommand().andThen(setHoodCommand(Rotations.of(0))).withName("Stow Shooter");
111+
}
112+
113+
public Command homeShooterCommand() {
114+
return runEnd(
115+
() -> hoodMotor.set(ShooterConstants.SAFE_HOMING_EFFORT.Output),
116+
() -> hoodMotor.setPosition(0))
117+
.until(
118+
() -> {
119+
return hoodMotor.getStatorCurrent().getValueAsDouble()
120+
> ShooterConstants.SAFE_STATOR_LIMIT.in(Amp);
121+
});
122+
}
123+
124+
// helper method to find, given a distance from the robot to the tag,
125+
// (1) necessary angle of the hood
126+
// (2) necessary velocity of flywheel
127+
// to land a lemon in the goal
128+
public LaunchRequest createLaunchRequest(double distanceToTag) {
129+
double y1 = ShooterConstants.SHOOTER_HEIGHT.in(Meters);
130+
double x2 = distanceToTag + ShooterConstants.OFFSET_DISTANCE.in(Meters);
131+
double y2 = ShooterConstants.GOAL_HEIGHT.in(Meters);
132+
133+
double slope = ShooterConstants.OPTIMAL_ENTRY_SLOPE;
134+
double a, b, vertex;
135+
Angle theta, motorAngle;
136+
do {
137+
// system of equations
138+
// (y2) = a(x2*x2) + b(x2) + y1
139+
// slope = 2a(x2) + b
140+
a = (slope * x2 + y1 - y2) / (x2 * x2);
141+
b = (slope - 2 * a * x2);
142+
theta = Radians.of(Math.atan(b)); // launch angle (Hood Angle Conversion: MATH.PI/2 - theta)
143+
motorAngle = Radians.of(Math.PI / 2 - theta.in(Radians));
144+
vertex = -1 * b / (2 * a);
145+
slope -= 0.05;
146+
} while ((vertex > x2 - ShooterConstants.MIN_VERTEX_DISTANCE.in(Meters))
147+
&& !(motorAngle.in(Degrees) < ShooterConstants.MIN_HOOD_ANGLE.in(Degrees)));
148+
149+
if (motorAngle.in(Degrees) < ShooterConstants.MIN_HOOD_ANGLE.in(Degrees)) return null;
150+
151+
// system of equations
152+
// (-b/2a) = (velocity)*cos(theta)*t
153+
// 2g(t) = (velocity)*sin(theta)
154+
LinearVelocity velocity =
155+
MetersPerSecond.of(
156+
Math.sqrt(
157+
2 * 9.8 * vertex / (Math.sin(theta.in(Radians)) * Math.cos(theta.in(Radians)))));
158+
159+
return new LaunchRequest(theta, velocity);
160+
}
161+
}

0 commit comments

Comments
 (0)