Skip to content

Commit 90edfea

Browse files
committed
Pretty much finished ShooterIOSparkMax, and the build script reformated everything
1 parent 32ab970 commit 90edfea

4 files changed

Lines changed: 154 additions & 39 deletions

File tree

Lines changed: 29 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -1,42 +1,38 @@
11
package frc.robot.subsystems.shooter;
22

33
import edu.wpi.first.wpilibj2.command.Command;
4-
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
54
import edu.wpi.first.wpilibj2.command.RunCommand;
65
import edu.wpi.first.wpilibj2.command.SubsystemBase;
7-
86
import java.util.function.DoubleSupplier;
7+
import org.littletonrobotics.junction.Logger;
98

109
public class Shooter extends SubsystemBase {
11-
private final ShooterIO io;
12-
13-
public Shooter(ShooterIO io) {
14-
this.io=io;
15-
}
16-
17-
@Override public void periodic() {
18-
}
19-
20-
21-
public double getVoltage() {
22-
return io.getVoltage();
23-
}
24-
25-
public Command runVoltage(DoubleSupplier voltage) {
26-
return new RunCommand(
27-
()->io.setVoltage(voltage.getAsDouble()),
28-
this
29-
);
30-
}
31-
32-
public Command runRPM(DoubleSupplier rpm) {
33-
return new RunCommand(
34-
()->io.setRPM(rpm.getAsDouble()),
35-
this
36-
);
37-
}
38-
39-
public void setPIDGains(double Kp, double Ki, double Kd) {
40-
io.setPIDGains(Kp,Ki,Kd);
41-
}
10+
private final ShooterIO io;
11+
private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
12+
13+
public Shooter(ShooterIO io) {
14+
this.io = io;
15+
}
16+
17+
@Override
18+
public void periodic() {
19+
io.updateInputs(inputs);
20+
Logger.processInputs("Shooter", inputs);
21+
}
22+
23+
public double getVoltage() {
24+
return io.getVoltage();
25+
}
26+
27+
public Command runVoltage(DoubleSupplier voltage) {
28+
return new RunCommand(() -> io.setVoltage(voltage.getAsDouble()), this);
29+
}
30+
31+
public Command runRPM(DoubleSupplier rpm) {
32+
return new RunCommand(() -> io.setRPM(rpm.getAsDouble()), this);
33+
}
34+
35+
public void setPIDGains(double Kp, double Ki, double Kd) {
36+
io.setPIDGains(Kp, Ki, Kd);
37+
}
4238
}
Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
package frc.robot.subsystems.shooter;
2+
3+
public class ShooterConstants {
4+
5+
// MOTOR ID'S
6+
public static final int FRONT_MOTOR_ID = 0;
7+
public static final int BACK_MOTOR_ID = 0;
8+
9+
// SPEED
10+
public static final double MAX_VELOCITY =
11+
8; // m/s i think, not sure if this is fast or slow for a shooter
12+
}
Lines changed: 22 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,30 @@
11
package frc.robot.subsystems.shooter;
22

3+
import org.littletonrobotics.junction.AutoLog;
4+
35
public interface ShooterIO {
6+
@AutoLog
7+
public static class ShooterIOInputs { // these were mostly copied from existing input classes
8+
public double velocityRPM = 0.0;
9+
public double appliedVoltage = 0.0;
10+
11+
// arrays are used because we have multiple motors
12+
public double motorCurrent = 0.0;
13+
}
14+
15+
public default void updateInputs(ShooterIOInputs inputs) {}
16+
17+
public default void setVoltage(double voltage) {}
418

5-
public default void setVoltage(double voltage) {}
19+
public default double getVoltage() {
20+
return 0;
21+
}
622

7-
public default double getVoltage() {
8-
return 0;
9-
}
23+
public default void setRPM(double rpm) {}
1024

11-
public default void setRPM(double rpm) {}
25+
public default double getRPM() {
26+
return 0;
27+
}
1228

13-
public default void setPIDGains(double Kp, double Ki, double Kd) {}
29+
public default void setPIDGains(double Kp, double Ki, double Kd) {}
1430
}
Lines changed: 91 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
1+
package frc.robot.subsystems.shooter;
2+
3+
import static frc.robot.Constants.NEO_CURRENT_LIMIT;
4+
5+
import com.revrobotics.RelativeEncoder;
6+
import com.revrobotics.spark.SparkBase;
7+
import com.revrobotics.spark.SparkFlex;
8+
import com.revrobotics.spark.SparkLowLevel;
9+
import com.revrobotics.spark.config.SparkBaseConfig;
10+
import com.revrobotics.spark.config.SparkFlexConfig;
11+
import edu.wpi.first.math.controller.PIDController;
12+
import org.littletonrobotics.junction.Logger;
13+
14+
public class ShooterIOSparkMax implements ShooterIO {
15+
private SparkFlex frontMotor;
16+
private SparkFlex backMotor;
17+
private RelativeEncoder encoder;
18+
19+
private PIDController pidController = new PIDController(0, 0, 0);
20+
21+
public ShooterIOSparkMax() {
22+
frontMotor = new SparkFlex(ShooterConstants.FRONT_MOTOR_ID, SparkLowLevel.MotorType.kBrushless);
23+
backMotor = new SparkFlex(ShooterConstants.BACK_MOTOR_ID, SparkLowLevel.MotorType.kBrushless);
24+
25+
encoder = frontMotor.getEncoder();
26+
27+
SparkFlexConfig frontConfig = new SparkFlexConfig();
28+
29+
frontConfig // I just copied these from CoralIntakeIOSparkMax
30+
.idleMode(SparkBaseConfig.IdleMode.kBrake)
31+
.voltageCompensation(12)
32+
.smartCurrentLimit(NEO_CURRENT_LIMIT);
33+
34+
SparkFlexConfig backConfig = new SparkFlexConfig();
35+
36+
backConfig.apply(frontConfig);
37+
backConfig.follow(frontMotor);
38+
39+
frontMotor.configure(
40+
frontConfig,
41+
SparkBase.ResetMode.kResetSafeParameters,
42+
SparkBase.PersistMode.kPersistParameters);
43+
backMotor.configure(
44+
backConfig,
45+
SparkBase.ResetMode.kResetSafeParameters,
46+
SparkBase.PersistMode.kPersistParameters);
47+
}
48+
49+
@Override
50+
public void updateInputs(ShooterIOInputs inputs) {
51+
inputs.velocityRPM = getRPM();
52+
inputs.appliedVoltage = frontMotor.getAppliedOutput() * frontMotor.getBusVoltage(); // what
53+
inputs.motorCurrent =
54+
frontMotor
55+
.getOutputCurrent(); // You told me that this shouldn't be an array, so I'm guessing
56+
// that both motors have the same.
57+
}
58+
59+
@Override
60+
public double getRPM() {
61+
return encoder.getVelocity();
62+
}
63+
64+
@Override
65+
public void setPIDGains(double Kp, double Ki, double Kd) {
66+
pidController.setPID(Kp, Ki, Kd);
67+
}
68+
69+
@Override
70+
public void setVoltage(double voltage) {
71+
frontMotor.setVoltage(voltage);
72+
Logger.recordOutput(
73+
"Shooter/Set Voltage",
74+
voltage); // Took this from ElevatorIOSparkMax, hoping this is what I'm supposed to do
75+
}
76+
77+
@Override
78+
public double getVoltage() {
79+
return frontMotor.getAppliedOutput()
80+
* frontMotor
81+
.getBusVoltage(); // idk maybe this is it idk what the difference between voltage and
82+
// applied voltage is
83+
}
84+
85+
@Override
86+
public void setRPM(double rpm) {
87+
double voltage = pidController.calculate(getRPM(), rpm);
88+
89+
frontMotor.setVoltage(voltage);
90+
}
91+
}

0 commit comments

Comments
 (0)