|
1 | 1 | package frc.robot.subsystems.shooter; |
2 | 2 |
|
3 | 3 | import static frc.robot.Constants.NEO_CURRENT_LIMIT; |
| 4 | +import static frc.robot.subsystems.shooter.ShooterConstants.MAX_ACCERLERATION; |
| 5 | +import static frc.robot.subsystems.shooter.ShooterConstants.MAX_VELOCITY; |
4 | 6 |
|
5 | 7 | import com.revrobotics.RelativeEncoder; |
6 | 8 | import com.revrobotics.spark.SparkBase; |
7 | 9 | import com.revrobotics.spark.SparkFlex; |
8 | 10 | import com.revrobotics.spark.SparkLowLevel; |
9 | 11 | import com.revrobotics.spark.config.SparkBaseConfig; |
10 | 12 | import com.revrobotics.spark.config.SparkFlexConfig; |
11 | | -import edu.wpi.first.math.controller.PIDController; |
| 13 | +import edu.wpi.first.math.controller.ProfiledPIDController; |
| 14 | +import edu.wpi.first.math.trajectory.TrapezoidProfile; |
12 | 15 | import org.littletonrobotics.junction.Logger; |
13 | 16 |
|
14 | 17 | public class ShooterIOSparkMax implements ShooterIO { |
15 | 18 | private SparkFlex frontMotor; |
16 | 19 | private SparkFlex backMotor; |
17 | 20 | private RelativeEncoder encoder; |
18 | 21 |
|
19 | | - private PIDController pidController = new PIDController(0, 0, 0); |
| 22 | + private ProfiledPIDController pidController = new ProfiledPIDController(0, 0, 0, new TrapezoidProfile.Constraints(MAX_VELOCITY,MAX_ACCERLERATION)); |
20 | 23 |
|
21 | 24 | public ShooterIOSparkMax() { |
22 | 25 | frontMotor = new SparkFlex(ShooterConstants.FRONT_MOTOR_ID, SparkLowLevel.MotorType.kBrushless); |
@@ -84,7 +87,7 @@ public double getVoltage() { |
84 | 87 |
|
85 | 88 | @Override |
86 | 89 | public void setRPM(double rpm) { |
87 | | - double voltage = pidController.calculate(getRPM(), rpm); |
| 90 | + double voltage = pidController.calculate(getRPM(), rpm); //idk do i have to do anything special |
88 | 91 |
|
89 | 92 | frontMotor.setVoltage(voltage); |
90 | 93 | } |
|
0 commit comments