diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7b9ac010..91813020 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -348,7 +348,7 @@ public void putShuffleboardCommands() { new SpinFeeder(feederSubsystem)); SmartDashboard.putData( - "Spin Shooter", + "Spins Shooter", new SpinShooter(shooterSubsystem, Constants.SHOOTER_SPEED)); SmartDashboard.putData( diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 09d8d678..8390a9af 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -9,6 +9,7 @@ import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; @@ -20,6 +21,7 @@ import frc.robot.utils.logging.io.pidmotor.SparkMaxPidConfig; import frc.robot.utils.logging.io.pidmotor.SparkMaxPidMotor; import frc.robot.utils.logging.io.pidmotor.SparkMaxPidMotorIo; +import frc.robot.utils.motor.TunablePIDManager; import frc.robot.utils.simulation.MotorSimulator; import frc.robot.utils.simulation.RobotVisualizer; @@ -30,16 +32,23 @@ public class ShooterSubsystem extends SubsystemBase { private final SparkMaxPidMotorIo io; private final SparkMax followerMotor; private final SparkMaxConfig followerConfig; - + private final TunablePIDManager pidManager; public ShooterSubsystem(SparkMaxPidMotorIo io) { + this.pidManager = new TunablePIDManager(LOGGING_NAME, io, createPidConfig()); this.io = io; - io.setPid(0.0000002, 0.000015, 0.000015); // Pid needs tuning + // io.setPid(0.0000002, 0.000015, 0.000015); // Pid needs tuning followerMotor = new SparkMax(Constants.SHOOTER_FOLLOWER_MOTOR_ID, SparkLowLevel.MotorType.kBrushless); followerConfig = new SparkMaxConfig(); - followerConfig.follow(Constants.SHOOTER_MOTOR_ID, true); + followerConfig.follow(Constants.SHOOTER_MOTOR_ID, true).idleMode(IdleMode.kCoast); followerMotor.configure(followerConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } + private static SparkMaxPidConfig createPidConfig() { + return new SparkMaxPidConfig(false) + .setCurrentLimit(Constants.NEO_CURRENT_LIMIT) + .setAllowedError(.1) + .setIdleMode(IdleMode.kCoast); + } // setSpeed expects a power value from -1 to 1 public void setSpeed(double speed) { @@ -57,6 +66,7 @@ public void setPidVelocity(double velocity) { @Override public void periodic() { + pidManager.periodic(); io.periodic(); } @@ -74,7 +84,7 @@ public static SparkMaxPidMotorIo createSimIo(RobotVisualizer visualizer) { } public static SparkMaxPidMotor createMotor() { - return new SparkMaxPidMotor(Constants.SHOOTER_MOTOR_ID, true); + return new SparkMaxPidMotor(Constants.SHOOTER_MOTOR_ID, createPidConfig()); } } diff --git a/src/main/java/frc/robot/utils/logging/io/pidmotor/MockSparkMaxPidMotorIo.java b/src/main/java/frc/robot/utils/logging/io/pidmotor/MockSparkMaxPidMotorIo.java index aed7bbd2..d42c3afe 100644 --- a/src/main/java/frc/robot/utils/logging/io/pidmotor/MockSparkMaxPidMotorIo.java +++ b/src/main/java/frc/robot/utils/logging/io/pidmotor/MockSparkMaxPidMotorIo.java @@ -4,6 +4,7 @@ package frc.robot.utils.logging.io.pidmotor; + import frc.robot.utils.logging.input.MotorLoggableInputs; import frc.robot.utils.logging.io.motor.MockSparkMaxIo; diff --git a/src/main/java/frc/robot/utils/logging/io/pidmotor/RealSparkMaxPidMotorIo.java b/src/main/java/frc/robot/utils/logging/io/pidmotor/RealSparkMaxPidMotorIo.java index 78122684..a14820b3 100644 --- a/src/main/java/frc/robot/utils/logging/io/pidmotor/RealSparkMaxPidMotorIo.java +++ b/src/main/java/frc/robot/utils/logging/io/pidmotor/RealSparkMaxPidMotorIo.java @@ -1,6 +1,10 @@ package frc.robot.utils.logging.io.pidmotor; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + import frc.robot.utils.logging.input.MotorLoggableInputs; import frc.robot.utils.logging.io.motor.RealSparkMaxIo; diff --git a/src/main/java/frc/robot/utils/logging/io/pidmotor/SparkMaxPidConfig.java b/src/main/java/frc/robot/utils/logging/io/pidmotor/SparkMaxPidConfig.java index 03ad4a50..989ae6a9 100644 --- a/src/main/java/frc/robot/utils/logging/io/pidmotor/SparkMaxPidConfig.java +++ b/src/main/java/frc/robot/utils/logging/io/pidmotor/SparkMaxPidConfig.java @@ -1,5 +1,7 @@ package frc.robot.utils.logging.io.pidmotor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + /** * Value container helper class for configuring a PidMotor. */ @@ -12,6 +14,7 @@ public class SparkMaxPidConfig { public static final double MAX_VELOCITY = 5000; public static final double MAX_ACCELERATION = 10000; public static final double ALLOWED_ERROR = 1.0; + public static final IdleMode DEFAULT_IDLE_MODE = IdleMode.kBrake; private double p = DEFAULT_P; private double i = DEFAULT_I; @@ -19,6 +22,7 @@ public class SparkMaxPidConfig { private double iZone = DEFAULT_IZONE; private double ff = DEFAULT_FF; private int currentLimit = 20; + private IdleMode mode = DEFAULT_MODE; /** * This is the cruise velocity for the MAX_MOTION config. */ @@ -130,4 +134,14 @@ public double getAllowedError() { public boolean getUsesMaxMotion() { return usesMaxMotion; } + + public SparkMaxPidConfig setIdleMode(IdleMode mode){ + this.mode = mode; + return this; + } + + public IdleMode getIdleMode(){ + return mode; + } } + diff --git a/src/main/java/frc/robot/utils/logging/io/pidmotor/SparkMaxPidMotor.java b/src/main/java/frc/robot/utils/logging/io/pidmotor/SparkMaxPidMotor.java index 5435c8e2..ccad39c8 100644 --- a/src/main/java/frc/robot/utils/logging/io/pidmotor/SparkMaxPidMotor.java +++ b/src/main/java/frc/robot/utils/logging/io/pidmotor/SparkMaxPidMotor.java @@ -9,6 +9,8 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import frc.robot.constants.Constants; + /** * A Wrapper utility to encapsulate the NEO motor with PID capability. This is simply a wrapper with * some convenient defaults and initializations that make programming the PID of the NEO easier. @@ -51,7 +53,7 @@ public SparkMaxPidMotor(int id, SparkMaxPidConfig pidConfig) { config .smartCurrentLimit(pidConfig.getCurrentLimit()) .closedLoopRampRate(RAMP_RATE) - .idleMode(IdleMode.kBrake); + .idleMode(pidConfig.getIdleMode()); config .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) @@ -78,7 +80,6 @@ public SparkMaxPidMotor(int id, SparkMaxPidConfig pidConfig) { neoMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } - /** * Reconfigure the PID fully using some of the values from motor params. * This method uses the PID, iZone, FF, maxVelocity, maxAcceleration and allowedError to reconfigure @@ -92,7 +93,7 @@ public void configurePID(SparkMaxPidConfig params) { .closedLoop .pid(params.getP(), params.getI(), params.getD()) .iZone(params.getIZone()) - .feedForward.kV(pidConfig.getFF()); + .feedForward.kV(params.getFF()); if (params.getUsesMaxMotion()) { config .closedLoop diff --git a/src/main/java/frc/robot/utils/logging/io/pidmotor/SparkMaxPidMotorIo.java b/src/main/java/frc/robot/utils/logging/io/pidmotor/SparkMaxPidMotorIo.java index 630e7f10..c33237f2 100644 --- a/src/main/java/frc/robot/utils/logging/io/pidmotor/SparkMaxPidMotorIo.java +++ b/src/main/java/frc/robot/utils/logging/io/pidmotor/SparkMaxPidMotorIo.java @@ -1,6 +1,7 @@ package frc.robot.utils.logging.io.pidmotor; import com.revrobotics.spark.SparkBase; + import frc.robot.utils.logging.io.motor.SparkMaxIo; /** @@ -56,4 +57,5 @@ public interface SparkMaxPidMotorIo extends SparkMaxIo { * @param pidFF the FF value to set */ void setPid(double pidP, double pidI, double pidD, double iZone, double pidFF); + }