diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 450ee6e..f4d8d61 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -1,6 +1,5 @@ package frc.robot.constants; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java new file mode 100644 index 0000000..0d56175 --- /dev/null +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -0,0 +1,112 @@ +package frc.robot.constants; + +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.math.util.Units; + +public class ShooterConstants { + public enum ShooterSpeeds { + kHubSpeed(0, 0), + kFeedSpeed(0, 0); + + private double frontRollerMPS; + private double rearRollerMPS; + + private ShooterSpeeds(double frontRollerMPS, double rearRollerMPS) { + this.frontRollerMPS = frontRollerMPS; + this.rearRollerMPS = rearRollerMPS; + } + + public double getFrontRollerMPS() { + return frontRollerMPS; + } + + public double getRearRollerMPS() { + return rearRollerMPS; + } + } + + public static final double kWheelDiameter = Units.inchesToMeters(6); + + // TODO Real values + public static final int kFrontShooterMotor1CANID = 0; + public static final int kFrontShooterMotor2CANID = 0; + public static final int kRearShooterMotor1CANID = 0; + public static final int kRearShooterMotor2CANID = 0; + + public static final boolean kFrontShooterMotor1Inverted = false; + public static final boolean kFrontShooterMotor2Inverted = false; + public static final boolean kRearShooterMotor1Inverted = false; + public static final boolean kRearShooterMotor2Inverted = false; + + public static final double kFrontP = 0; + public static final double kFrontI = 0; + public static final double kFrontD = 0; + public static final double kFrontS = 0; + public static final double kFrontV = 0; + public static final double kFrontA = 0; + + public static final double kRearP = 0; + public static final double kRearI = 0; + public static final double kRearD = 0; + public static final double kRearS = 0; + public static final double kRearV = 0; + public static final double kRearA = 0; + + // TODO Is this value sane? + public static final int kCurrentLimit = 30; + + public static final IdleMode kShooterIdleMode = IdleMode.kCoast; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final SparkMaxConfig kFrontMotor1Config = new SparkMaxConfig(); + public static final SparkMaxConfig kFrontMotor2Config = new SparkMaxConfig(); + public static final SparkMaxConfig kRearMotor1Config = new SparkMaxConfig(); + public static final SparkMaxConfig kRearMotor2Config = new SparkMaxConfig(); + + + static { + kFrontMotor1Config + .idleMode(kShooterIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(kFrontShooterMotor1Inverted); + kFrontMotor1Config.absoluteEncoder + .positionConversionFactor(kWheelDiameter * Math.PI) + .velocityConversionFactor(kWheelDiameter * Math.PI / 60); + kFrontMotor1Config.closedLoop + .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .pid(kFrontP, kFrontI, kFrontD) + .outputRange(-1, 1) + .feedForward + .sva(kFrontS, kFrontV, kFrontA); + + kFrontMotor2Config + .idleMode(kShooterIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(kFrontShooterMotor2Inverted) + .follow(kFrontShooterMotor1CANID); + + kRearMotor1Config + .idleMode(kShooterIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(kRearShooterMotor1Inverted); + kRearMotor1Config.absoluteEncoder + .positionConversionFactor(kWheelDiameter * Math.PI) + .velocityConversionFactor(kWheelDiameter * Math.PI / 60); + kRearMotor1Config.closedLoop + .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .pid(kRearP, kRearI, kRearD) + .outputRange(-1, 1) + .feedForward + .sva(kRearS, kRearV, kRearA); + + kRearMotor2Config + .idleMode(kShooterIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(kRearShooterMotor2Inverted) + .follow(kRearShooterMotor1CANID); + } +} diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java new file mode 100644 index 0000000..804e1e5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -0,0 +1,135 @@ +package frc.robot.subsystems; + +import java.util.Optional; + +import org.littletonrobotics.junction.Logger; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.ShooterConstants; +import frc.robot.constants.ShooterConstants.ShooterSpeeds; + +public class Shooter extends SubsystemBase { + private SparkMax frontMotor1; + private SparkMax frontMotor2; + private SparkMax rearMotor1; + private SparkMax rearMotor2; + + private AbsoluteEncoder frontEncoder; + private AbsoluteEncoder rearEncoder; + + private SparkClosedLoopController frontClosedLoopController; + private SparkClosedLoopController rearClosedLoopController; + + private ShooterSpeeds currentSpeeds; + + public Shooter() { + frontMotor1 = new SparkMax(ShooterConstants.kFrontShooterMotor1CANID, MotorType.kBrushless); + frontMotor2 = new SparkMax(ShooterConstants.kFrontShooterMotor2CANID, MotorType.kBrushless); + rearMotor1 = new SparkMax(ShooterConstants.kRearShooterMotor1CANID, MotorType.kBrushless); + rearMotor2 = new SparkMax(ShooterConstants.kRearShooterMotor2CANID, MotorType.kBrushless); + + frontMotor1.configure( + ShooterConstants.kFrontMotor1Config, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + rearMotor1.configure( + ShooterConstants.kRearMotor1Config, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + frontMotor2.configure( + ShooterConstants.kFrontMotor2Config, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + rearMotor2.configure( + ShooterConstants.kRearMotor2Config, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + frontEncoder = frontMotor1.getAbsoluteEncoder(); + rearEncoder = rearMotor1.getAbsoluteEncoder(); + + frontClosedLoopController = frontMotor1.getClosedLoopController(); + rearClosedLoopController = rearMotor1.getClosedLoopController(); + + // TODO Set this to the initial startup speed + currentSpeeds = null; + } + + @Override + public void periodic() { + if(currentSpeeds == null) { + if(frontClosedLoopController.getSetpoint() != 0) { + frontClosedLoopController.setSetpoint( + 0, + ControlType.kVelocity + ); + } + + if(rearClosedLoopController.getSetpoint() != 0) { + rearClosedLoopController.setSetpoint( + 0, + ControlType.kVelocity + ); + } + } else { + frontClosedLoopController.setSetpoint( + currentSpeeds.getFrontRollerMPS(), + ControlType.kVelocity + ); + + rearClosedLoopController.setSetpoint( + currentSpeeds.getRearRollerMPS(), + ControlType.kVelocity + ); + } + + Logger.recordOutput( + "Shooter/FrontRollers/TargetMPS", + currentSpeeds == null ? 0 : currentSpeeds.getFrontRollerMPS() + ); + + Logger.recordOutput( + "Shooter/RearRollers/TargetMPS", + currentSpeeds == null ? 0 : currentSpeeds.getRearRollerMPS() + ); + + Logger.recordOutput("Shooter/FrontRollers/CurrentMPS", frontEncoder.getVelocity()); + Logger.recordOutput("Shooter/RearRollers/CurrentMPS", rearEncoder.getVelocity()); + + // TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance? + Logger.recordOutput("Shooter/FrontRollers/AtSetpoint", frontClosedLoopController.isAtSetpoint()); + Logger.recordOutput("Shooter/RearRollers/AtSetpoint", rearClosedLoopController.isAtSetpoint()); + } + + public Command setCurrentSpeeds(ShooterSpeeds speeds) { + return runOnce(() -> { + currentSpeeds = speeds; + }); + } + + public Command stop() { + return runOnce(() -> { + currentSpeeds = null; + }); + } + + public Optional getCurrentSpeeds() { + return Optional.ofNullable(currentSpeeds); + } +} diff --git a/src/main/java/frc/robot/utilities/SwerveModule.java b/src/main/java/frc/robot/utilities/SwerveModule.java index 6e7af98..ccc0169 100644 --- a/src/main/java/frc/robot/utilities/SwerveModule.java +++ b/src/main/java/frc/robot/utilities/SwerveModule.java @@ -12,7 +12,6 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkLowLevel.MotorType; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState;