Skip to content

Commit 8533005

Browse files
committed
Switched from PIDController to ProfiledPIDController
1 parent b586905 commit 8533005

2 files changed

Lines changed: 8 additions & 5 deletions

File tree

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,6 @@ public class ShooterConstants {
77
public static final int BACK_MOTOR_ID = 0;
88

99
// 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
10+
public static final double MAX_VELOCITY = 8; // m/s i think, not sure if this is fast or slow for a shooter
11+
public static final double MAX_ACCERLERATION=16;
1212
}

src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,22 +1,25 @@
11
package frc.robot.subsystems.shooter;
22

33
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;
46

57
import com.revrobotics.RelativeEncoder;
68
import com.revrobotics.spark.SparkBase;
79
import com.revrobotics.spark.SparkFlex;
810
import com.revrobotics.spark.SparkLowLevel;
911
import com.revrobotics.spark.config.SparkBaseConfig;
1012
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;
1215
import org.littletonrobotics.junction.Logger;
1316

1417
public class ShooterIOSparkMax implements ShooterIO {
1518
private SparkFlex frontMotor;
1619
private SparkFlex backMotor;
1720
private RelativeEncoder encoder;
1821

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));
2023

2124
public ShooterIOSparkMax() {
2225
frontMotor = new SparkFlex(ShooterConstants.FRONT_MOTOR_ID, SparkLowLevel.MotorType.kBrushless);
@@ -84,7 +87,7 @@ public double getVoltage() {
8487

8588
@Override
8689
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
8891

8992
frontMotor.setVoltage(voltage);
9093
}

0 commit comments

Comments
 (0)