Skip to content

Commit 3b81be6

Browse files
committed
i don't know what im doing im so confused 😒😒😒
1 parent 8533d39 commit 3b81be6

7 files changed

Lines changed: 304 additions & 0 deletions

File tree

0 Bytes
Binary file not shown.
1 KB
Binary file not shown.
Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
package frc.robot.subsystems.shooter;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import edu.wpi.first.wpilibj2.command.RunCommand;
5+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
6+
import java.util.function.DoubleSupplier;
7+
8+
9+
public class Shooter extends SubsystemBase {
10+
private final ShooterIO io;
11+
12+
public Shooter(ShooterIO io) {
13+
this.io = io;
14+
}
15+
16+
/**
17+
* Runs the shooter in open loop using a voltage value.
18+
* The motor voltage continuously follows the supplier value
19+
* while the command is active, and stops when it's interrupted.
20+
*/
21+
public Command runVoltage(DoubleSupplier voltageSupplier) {
22+
return new RunCommand(
23+
() -> io.setVoltage(voltageSupplier.getAsDouble()),
24+
this
25+
).finallyDo(interrupted -> io.setVoltage(0));
26+
}
27+
28+
/**
29+
* Runs the shooter using a target RPM.
30+
* The RPM value is read continuously from the supplier, and the shooter
31+
* stops when the command ends.
32+
*/
33+
public Command runRPM(DoubleSupplier rpmSupplier) {
34+
return new RunCommand(
35+
() -> io.setRPM(rpmSupplier.getAsDouble()),
36+
this
37+
).finallyDo(interrupted -> io.setRPM(0));
38+
}
39+
40+
/**
41+
* Optional helper methods for direct control (not through Commands).
42+
* These might be used for autonomous or testing.
43+
*/
44+
public void setVoltage(double voltage) {
45+
io.setVoltage(voltage);
46+
}
47+
48+
public void setRPM(double rpm) {
49+
io.setRPM(rpm);
50+
}
51+
}
Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
package frc.robot.subsystems.shooter;
2+
3+
/**
4+
* Stores all constants used by the shooter subsystem.
5+
*
6+
* This includes motor IDs, PID gains, and feedforward constants.
7+
* If we ever switch ports or retune values, we only have to change them here.
8+
*/
9+
public final class ShooterConstants {
10+
11+
// === Motor IDs ===
12+
public static final int LEFT_MOTOR_ID = 1;
13+
public static final int RIGHT_MOTOR_ID = 2;
14+
15+
// === PID Gains ===
16+
// Start with small numbers; tune later in testing.
17+
public static final double kP = 0.0005;
18+
public static final double kI = 0.0;
19+
public static final double kD = 0.0;
20+
21+
// === Feedforward Gains ===
22+
// These are rough guesses β€” tune during shooting tests.
23+
public static final double kS = 0.2; // static friction
24+
public static final double kV = 0.12; // velocity
25+
public static final double kA = 0.015; // acceleration
26+
27+
// === Target Shooter Speeds (RPM) ===
28+
public static final double CLOSE_SHOT_RPM = 2500;
29+
public static final double FAR_SHOT_RPM = 4000;
30+
31+
private ShooterConstants() {
32+
// Prevent instantiation
33+
}
34+
}
Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
package frc.robot.subsystems.shooter;
2+
3+
/**
4+
* ShooterIO acts as a low-level interface for the shooter hardware.
5+
*
6+
* Basically, this is what the subsystem talks to in order to control
7+
* the actual shooter motors. The real motor code (Spark MAX, etc.)
8+
* would go in a class that implements this interface.
9+
*/
10+
public interface ShooterIO {
11+
12+
/**
13+
* Sets the target shooter speed in RPM.
14+
* The subsystem will call this whenever it wants the shooter to spin
15+
* at a certain speed.
16+
*/
17+
default void setRPM(double rpm) {
18+
// Left empty on purpose β€” the real implementation will fill this in.
19+
}
20+
21+
/**
22+
* Directly sets the motor voltage. This is used for open-loop control.
23+
*/
24+
default void setVoltage(double voltage) {
25+
// Left empty β€” real motor code goes here later.
26+
}
27+
28+
/**
29+
* Sets the feedforward gains (Ks, Kv, Ka).
30+
* These are constants used to estimate how much voltage to apply
31+
* for a given speed and acceleration.
32+
*/
33+
default void setFeedforwardGains(double Ks, double Kv, double Ka) {
34+
// Nothing yet β€” just defines the method for the real IO class.
35+
}
36+
37+
/**
38+
* Sets the PID controller gains (Kp, Ki, Kd).
39+
* These control how the shooter reacts to error in RPM.
40+
*/
41+
default void setPIDGains(double Kp, double Ki, double Kd) {
42+
// No implementation here β€” just a placeholder for now.
43+
}
44+
}
Lines changed: 115 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
package frc.robot.subsystems.shooter;
2+
3+
import com.revrobotics.CANSparkMax;
4+
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
5+
import com.revrobotics.RelativeEncoder;
6+
7+
import edu.wpi.first.math.controller.PIDController;
8+
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
9+
10+
/**
11+
* Real shooter IO that uses two Spark MAX motors.
12+
*
13+
* Basically, this is the "hardware" version that talks to the actual robot.
14+
* It handles voltages, PID tuning, and feedforward stuff.
15+
*/
16+
public class ShooterIOSparkMax implements ShooterIO {
17+
18+
// === Motors & encoders ===
19+
private final CANSparkMax leftMotor;
20+
private final CANSparkMax rightMotor;
21+
22+
private final RelativeEncoder leftEncoder;
23+
private final RelativeEncoder rightEncoder;
24+
25+
// === Controllers ===
26+
private final PIDController leftPID;
27+
private final PIDController rightPID;
28+
29+
private SimpleMotorFeedforward leftFF;
30+
private SimpleMotorFeedforward rightFF;
31+
32+
// === Constructor ===
33+
public ShooterIOSparkMax(int leftID, int rightID) {
34+
// create both motors
35+
leftMotor = new CANSparkMax(leftID, MotorType.kBrushless);
36+
rightMotor = new CANSparkMax(rightID, MotorType.kBrushless);
37+
38+
// reset to known state
39+
leftMotor.restoreFactoryDefaults();
40+
rightMotor.restoreFactoryDefaults();
41+
42+
// encoders for measuring speed (in RPM)
43+
leftEncoder = leftMotor.getEncoder();
44+
rightEncoder = rightMotor.getEncoder();
45+
46+
// simple starting PID and FF
47+
leftPID = new PIDController(0.0005, 0, 0);
48+
rightPID = new PIDController(0.0005, 0, 0);
49+
50+
leftFF = new SimpleMotorFeedforward(0, 0, 0);
51+
rightFF = new SimpleMotorFeedforward(0, 0, 0);
52+
53+
// make sure motors spin same way
54+
rightMotor.follow(leftMotor, true);
55+
}
56+
57+
// === Methods from ShooterIO ===
58+
59+
/** Open-loop control: directly set voltage to both motors. */
60+
@Override
61+
public void setVoltage(double voltage) {
62+
leftMotor.setVoltage(voltage);
63+
rightMotor.setVoltage(voltage);
64+
}
65+
66+
/**
67+
* Closed-loop control: set target shooter speed (in RPM).
68+
* The motors will try to match that speed using PID + feedforward.
69+
*/
70+
@Override
71+
public void setRPM(double rpm) {
72+
double leftSpeed = leftEncoder.getVelocity();
73+
double rightSpeed = rightEncoder.getVelocity();
74+
75+
// basic PID correction
76+
double leftPIDOut = leftPID.calculate(leftSpeed, rpm);
77+
double rightPIDOut = rightPID.calculate(rightSpeed, rpm);
78+
79+
// feedforward estimate
80+
double leftFFOut = leftFF.calculate(rpm);
81+
double rightFFOut = rightFF.calculate(rpm);
82+
83+
// combine and apply
84+
double leftVoltage = leftPIDOut + leftFFOut;
85+
double rightVoltage = rightPIDOut + rightFFOut;
86+
87+
leftMotor.setVoltage(leftVoltage);
88+
rightMotor.setVoltage(rightVoltage);
89+
}
90+
91+
/** Sets the PID gains for both motors. */
92+
@Override
93+
public void setPIDGains(double Kp, double Ki, double Kd) {
94+
leftPID.setP(Kp);
95+
leftPID.setI(Ki);
96+
leftPID.setD(Kd);
97+
98+
rightPID.setP(Kp);
99+
rightPID.setI(Ki);
100+
rightPID.setD(Kd);
101+
}
102+
103+
/** Sets feedforward constants for both sides. */
104+
@Override
105+
public void setFeedforwardGains(double Ks, double Kv, double Ka) {
106+
leftFF = new SimpleMotorFeedforward(Ks, Kv, Ka);
107+
rightFF = new SimpleMotorFeedforward(Ks, Kv, Ka);
108+
}
109+
110+
/** Stops both motors (used for safety or when command ends). */
111+
public void stopMotors() {
112+
leftMotor.set(0);
113+
rightMotor.set(0);
114+
}
115+
}
Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
public package frc.robot.subsystems.shooter;
2+
3+
/**
4+
* Simulated version of the ShooterIO.
5+
*
6+
* This class doesn’t control real motors β€” it just pretends to,
7+
* so we can test code without hardware.
8+
* For now, it mostly prints or stores values so we can see what would happen.
9+
*/
10+
public class ShooterIOsim implements ShooterIO {
11+
12+
private double currentRPM = 0.0;
13+
private double appliedVoltage = 0.0;
14+
15+
private double kS = 0.0;
16+
private double kV = 0.0;
17+
private double kA = 0.0;
18+
19+
private double kP = 0.0;
20+
private double kI = 0.0;
21+
private double kD = 0.0;
22+
23+
@Override
24+
public void setRPM(double rpm) {
25+
currentRPM = rpm;
26+
System.out.println("[Sim] Shooter target RPM set to: " + rpm);
27+
}
28+
29+
@Override
30+
public void setVoltage(double voltage) {
31+
appliedVoltage = voltage;
32+
System.out.println("[Sim] Shooter voltage set to: " + voltage + "V");
33+
}
34+
35+
@Override
36+
public void setFeedforwardGains(double Ks, double Kv, double Ka) {
37+
this.kS = Ks;
38+
this.kV = Kv;
39+
this.kA = Ka;
40+
System.out.println("[Sim] Feedforward gains set β†’ Ks: " + Ks + ", Kv: " + Kv + ", Ka: " + Ka);
41+
}
42+
43+
@Override
44+
public void setPIDGains(double Kp, double Ki, double Kd) {
45+
this.kP = Kp;
46+
this.kI = Ki;
47+
this.kD = Kd;
48+
System.out.println("[Sim] PID gains set β†’ Kp: " + Kp + ", Ki: " + Ki + ", Kd: " + Kd);
49+
}
50+
51+
/**
52+
* Optional helper to view what the simulation thinks the current state is.
53+
*/
54+
public void printSimStatus() {
55+
System.out.println("[Sim Status] RPM: " + currentRPM + " | Voltage: " + appliedVoltage);
56+
}
57+
}
58+
{
59+
60+
}

0 commit comments

Comments
Β (0)