diff --git a/src/main/java/frc/robot/Calibrations.java b/src/main/java/frc/robot/Calibrations.java index f94fcda..921db25 100644 --- a/src/main/java/frc/robot/Calibrations.java +++ b/src/main/java/frc/robot/Calibrations.java @@ -5,7 +5,7 @@ public final class Calibrations { public static final double PLACEHOLDER_DOUBLE = 0.0; public static final float PLACEHOLDER_FLOAT = 0; - public static final int SPARK_INIT_RETRY_ATTEMPTS = PLACEHOLDER_INT; + public static final int SPARK_INIT_RETRY_ATTEMPTS = 5; public static final class AutoBalance { public static final double CHARGE_STATION_PITCH_DEGREES_TO_NORM_VELOCITY = PLACEHOLDER_DOUBLE, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e49e7c4..bd3910b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -83,6 +83,9 @@ public RobotContainer() { } public void initialize() { + arm.initialize(); + grabber.initialize(); + drive.initialize(); autonChooser.setDefaultOption("Nothing", new RunCommand(() -> {}, drive, arm)); // Put the chooser on the dashboard diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 9327d61..f359d47 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -27,7 +27,7 @@ public class RobotMap { public static final int PIGEON_CAN_ID = Constants.PLACEHOLDER_INT; /** Channel for the game piece sensor */ - public static final int GRABBER_GAME_PIECE_SENSOR_DIO = Constants.PLACEHOLDER_INT; + public static final int GRABBER_GAME_PIECE_SENSOR_DIO = 0; public static final int DRIVER_CONTROLLER_PORT = 0, OPERATOR_CONTROLLER_PORT = 1; } diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 2232290..4684ba6 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -18,10 +18,12 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Calibrations; import frc.robot.RobotMap; import frc.robot.utils.ScoringLocationUtil; import frc.robot.utils.ScoringLocationUtil.ScoreHeight; import frc.robot.utils.SendableHelper; +import frc.robot.utils.SparkMaxUtils; import java.util.TreeMap; public class Arm extends SubsystemBase { @@ -57,7 +59,6 @@ public enum ArmPosition { ArmCal.ARM_MAX_ACCELERATION_DEG_PER_SECOND_SQUARED)); public Arm(ScoringLocationUtil scoreLoc) { - armPositionMap = new TreeMap(); armPositionMap.put(ArmPosition.STARTING, ArmCal.ARM_START_POSITION_DEG); armPositionMap.put(ArmPosition.INTAKE, ArmCal.ARM_INTAKE_POSITION_DEG); @@ -68,7 +69,9 @@ public Arm(ScoringLocationUtil scoreLoc) { this.scoreLoc = scoreLoc; } - public void initialize() {} + public void initialize() { + SparkMaxUtils.initWithRetry(this::initSparks, Calibrations.SPARK_INIT_RETRY_ATTEMPTS); + } /** Sets the desired position */ public void startScore() { @@ -161,20 +164,35 @@ public void zeroArmAtCurrentPos() { System.out.println("New Zero for Arm: " + ArmCal.armAbsoluteEncoderZeroPosDeg); } - public void setDegreesFromGearRatioAbsoluteEncoder( - AbsoluteEncoder sparkMaxEncoder, double ratio) { + /** + * @return the number of errors made when setting up the sparks + */ + public int setDegreesFromGearRatioAbsoluteEncoder(AbsoluteEncoder sparkMaxEncoder, double ratio) { + int errors = 0; double degreesPerRotation = 360.0 / ratio; double degreesPerRotationPerSecond = degreesPerRotation / 60.0; - sparkMaxEncoder.setPositionConversionFactor(degreesPerRotation); - sparkMaxEncoder.setVelocityConversionFactor(degreesPerRotationPerSecond); + errors += SparkMaxUtils.check(sparkMaxEncoder.setPositionConversionFactor(degreesPerRotation)); + errors += + SparkMaxUtils.check( + sparkMaxEncoder.setVelocityConversionFactor(degreesPerRotationPerSecond)); + return errors; } - public static void setDegreesFromGearRatioRelativeEncoder( + /** + * @return the number of errors made when setting up the sparks + */ + public static int setDegreesFromGearRatioRelativeEncoder( RelativeEncoder sparkMaxEncoder, double ratio) { + + int errors = 0; double degreesPerRotation = 360.0 / ratio; double degreesPerRotationPerSecond = degreesPerRotation / 60.0; - sparkMaxEncoder.setPositionConversionFactor(degreesPerRotation); - sparkMaxEncoder.setVelocityConversionFactor(degreesPerRotationPerSecond); + errors += SparkMaxUtils.check(sparkMaxEncoder.setPositionConversionFactor(degreesPerRotation)); + errors += + SparkMaxUtils.check( + sparkMaxEncoder.setVelocityConversionFactor(degreesPerRotationPerSecond)); + + return errors; } /** True if the arm is at the queried position. */ @@ -203,30 +221,37 @@ public boolean getCancelScore() { } /** Does all the initialization for the sparks */ - public void initSparks() { - - armMotor.restoreFactoryDefaults(); + public boolean initSparks() { + int errors = 0; + errors += SparkMaxUtils.check(armMotor.restoreFactoryDefaults()); // inverting stuff - armAbsoluteEncoder.setInverted(true); + errors += SparkMaxUtils.check(armAbsoluteEncoder.setInverted(true)); + errors += SparkMaxUtils.check(armEncoder.setInverted(true)); armMotor.setInverted(false); // Get positions and degrees of elevator through encoder in inches - setDegreesFromGearRatioRelativeEncoder(armEncoder, ArmConstants.ARM_MOTOR_GEAR_RATIO); + errors += setDegreesFromGearRatioRelativeEncoder(armEncoder, ArmConstants.ARM_MOTOR_GEAR_RATIO); + + errors += setDegreesFromGearRatioAbsoluteEncoder(armAbsoluteEncoder, 1.0); - setDegreesFromGearRatioAbsoluteEncoder(armAbsoluteEncoder, 1.0); + errors += + SparkMaxUtils.check( + armMotor.setSoftLimit(SoftLimitDirection.kForward, ArmCal.ARM_POSITIVE_LIMIT_DEGREES)); - armMotor.setSoftLimit(SoftLimitDirection.kForward, ArmCal.ARM_POSITIVE_LIMIT_DEGREES); + errors += SparkMaxUtils.check(armMotor.enableSoftLimit(SoftLimitDirection.kForward, true)); - armMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + errors += + SparkMaxUtils.check( + armMotor.setSoftLimit(SoftLimitDirection.kReverse, ArmCal.ARM_NEGATIVE_LIMIT_DEGREES)); - armMotor.setSoftLimit(SoftLimitDirection.kReverse, ArmCal.ARM_NEGATIVE_LIMIT_DEGREES); + errors += SparkMaxUtils.check(armMotor.enableSoftLimit(SoftLimitDirection.kReverse, true)); - armMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + errors += SparkMaxUtils.check(armMotor.setIdleMode(IdleMode.kBrake)); - armMotor.setIdleMode(IdleMode.kBrake); + errors += SparkMaxUtils.check(armMotor.setSmartCurrentLimit(ArmCal.ARM_CURRENT_LIMIT_AMPS)); - armMotor.setSmartCurrentLimit(ArmCal.ARM_CURRENT_LIMIT_AMPS); + return errors == 0; } /** diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 020c8a6..db1609a 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -94,6 +94,13 @@ public DriveSubsystem(Lights lightsSubsystem, BooleanSupplier isTimedMatchFunc) isTimedMatch = isTimedMatchFunc; } + public void initialize() { + frontLeft.initialize(); + frontRight.initialize(); + backLeft.initialize(); + backRight.initialize(); + } + public double getFilteredPitch() { return latestFilteredPitchDeg - DriveCal.IMU_PITCH_BIAS_DEG; } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleCal.java b/src/main/java/frc/robot/subsystems/drive/ModuleCal.java index a6bb4fa..eac5a1a 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleCal.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleCal.java @@ -12,6 +12,6 @@ public class ModuleCal { /** Values from 2023 */ public static final double TURNING_P = 0.8, TURNING_I = 0.0, TURNING_D = 0.1, TURNING_FF = 0.0; - public static final double DRIVING_MIN_OUPTUT = -1.0, DRIVING_MAX_OUTPUT = 1.0; + public static final double DRIVING_MIN_OUTPUT = -1.0, DRIVING_MAX_OUTPUT = 1.0; public static final double TURNING_MIN_OUTPUT = -1.0, TURNING_MAX_OUTPUT = 1.0; } diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java index f4fcd2e..b214e6c 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -16,6 +16,7 @@ import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.Timer; +import frc.robot.Calibrations; import frc.robot.utils.AbsoluteEncoderChecker; import frc.robot.utils.SparkMaxUtils; @@ -54,58 +55,102 @@ public SwerveModule(int drivingCanId, int turningCanId, double chassisAngularOff drivingEncoder.setPosition(0); } + public void initialize() { + SparkMaxUtils.initWithRetry(this::initDriveSpark, Calibrations.SPARK_INIT_RETRY_ATTEMPTS); + SparkMaxUtils.initWithRetry(this::initTurnSpark, Calibrations.SPARK_INIT_RETRY_ATTEMPTS); + } + /** Does all the initialization for the spark */ - void initTurnSpark() { + boolean initTurnSpark() { + int errors = 0; + + errors += SparkMaxUtils.check(turningSparkMax.restoreFactoryDefaults()); - turningSparkMax.restoreFactoryDefaults(); turningSparkMax.setInverted(ModuleConstants.TURNING_SPARK_MAX_INVERTED); AbsoluteEncoder turningEncoderTmp = turningSparkMax.getAbsoluteEncoder(Type.kDutyCycle); SparkMaxPIDController turningPidTmp = turningSparkMax.getPIDController(); - turningPidTmp.setFeedbackDevice(turningEncoderTmp); + + errors += SparkMaxUtils.check(turningPidTmp.setFeedbackDevice(turningEncoderTmp)); + + errors += SparkMaxUtils.check(turningPidTmp.setP(ModuleCal.TURNING_P)); + errors += SparkMaxUtils.check(turningPidTmp.setI(ModuleCal.TURNING_I)); + errors += SparkMaxUtils.check(turningPidTmp.setD(ModuleCal.TURNING_D)); + errors += SparkMaxUtils.check(turningPidTmp.setFF(ModuleCal.TURNING_FF)); /* Gear ratio 1.0 because the encoder is 1:1 with the module (doesn't involve the actual turning / gear ratio)*/ SparkMaxUtils.UnitConversions.setRadsFromGearRatio( turningEncoderTmp, ModuleConstants.TURNING_ENCODER_GEAR_RATIO); - turningEncoderTmp.setInverted(ModuleConstants.TURNING_ENCODER_INVERTED); - turningPidTmp.setPositionPIDWrappingEnabled(true); - turningPidTmp.setPositionPIDWrappingMinInput( - ModuleConstants.TURNING_ENCODER_POSITION_PID_MIN_INPUT_RADIANS); - turningPidTmp.setPositionPIDWrappingMaxInput( - ModuleConstants.TURNING_ENCODER_POSITION_PID_MAX_INPUT_RADIANS); - - turningPidTmp.setP(ModuleCal.TURNING_P); - turningPidTmp.setI(ModuleCal.TURNING_I); - turningPidTmp.setD(ModuleCal.TURNING_D); - turningPidTmp.setFF(ModuleCal.TURNING_FF); - turningPidTmp.setOutputRange(ModuleCal.TURNING_MIN_OUTPUT, ModuleCal.TURNING_MAX_OUTPUT); - turningSparkMax.setIdleMode(ModuleConstants.TURNING_MOTOR_IDLE_MODE); - turningSparkMax.setSmartCurrentLimit(ModuleConstants.TURNING_MOTOR_CURRENT_LIMIT_AMPS); + errors += + SparkMaxUtils.check( + turningEncoderTmp.setInverted(ModuleConstants.TURNING_ENCODER_INVERTED)); + errors += SparkMaxUtils.check(turningPidTmp.setPositionPIDWrappingEnabled(true)); + errors += + SparkMaxUtils.check( + turningPidTmp.setPositionPIDWrappingMinInput( + ModuleConstants.TURNING_ENCODER_POSITION_PID_MIN_INPUT_RADIANS)); + errors += + SparkMaxUtils.check( + turningPidTmp.setPositionPIDWrappingMaxInput( + ModuleConstants.TURNING_ENCODER_POSITION_PID_MAX_INPUT_RADIANS)); + + errors += + SparkMaxUtils.check( + turningPidTmp.setOutputRange( + ModuleCal.TURNING_MIN_OUTPUT, ModuleCal.TURNING_MAX_OUTPUT)); + errors += + SparkMaxUtils.check(turningSparkMax.setIdleMode(ModuleConstants.TURNING_MOTOR_IDLE_MODE)); + errors += + SparkMaxUtils.check( + turningSparkMax.setSmartCurrentLimit(ModuleConstants.TURNING_MOTOR_CURRENT_LIMIT_AMPS)); + + return errors == 0; } /** Does all the initialization for the spark */ - void initDriveSpark() { - drivingSparkMax.restoreFactoryDefaults(); + boolean initDriveSpark() { + int errors = 0; + errors += SparkMaxUtils.check(drivingSparkMax.restoreFactoryDefaults()); drivingSparkMax.setInverted(ModuleConstants.DRIVING_SPARK_MAX_INVERTED); RelativeEncoder drivingEncoderTmp = drivingSparkMax.getEncoder(); SparkMaxPIDController drivingPidTmp = drivingSparkMax.getPIDController(); - drivingPidTmp.setFeedbackDevice(drivingEncoderTmp); - - drivingPidTmp.setP(ModuleCal.DRIVING_P); - drivingPidTmp.setI(ModuleCal.DRIVING_I); - drivingPidTmp.setD(ModuleCal.DRIVING_D); - drivingPidTmp.setFF(ModuleCal.DRIVING_FF); - drivingPidTmp.setOutputRange(ModuleCal.DRIVING_MIN_OUPTUT, ModuleCal.DRIVING_MAX_OUTPUT); - drivingEncoderTmp.setPositionConversionFactor( - ModuleConstants.DRIVING_ENCODER_POSITION_FACTOR_METERS); - drivingEncoderTmp.setVelocityConversionFactor( - ModuleConstants.DRIVING_ENCODER_VELOCITY_FACTOR_METERS_PER_SECOND); - - drivingSparkMax.setIdleMode(ModuleConstants.DRIVING_MOTOR_IDLE_MODE); - drivingSparkMax.setSmartCurrentLimit(ModuleConstants.DRIVING_MOTOR_CURRENT_LIMIT_AMPS); + errors += SparkMaxUtils.check(drivingPidTmp.setFeedbackDevice(drivingEncoderTmp)); + + errors += SparkMaxUtils.check(drivingPidTmp.setP(ModuleCal.DRIVING_P)); + errors += SparkMaxUtils.check(drivingPidTmp.setI(ModuleCal.DRIVING_I)); + errors += SparkMaxUtils.check(drivingPidTmp.setD(ModuleCal.DRIVING_D)); + errors += SparkMaxUtils.check(drivingPidTmp.setFF(ModuleCal.DRIVING_FF)); + + errors += + SparkMaxUtils.check( + drivingPidTmp.setOutputRange( + ModuleCal.DRIVING_MIN_OUTPUT, ModuleCal.DRIVING_MAX_OUTPUT)); + + errors += + SparkMaxUtils.check( + drivingPidTmp.setOutputRange( + ModuleCal.DRIVING_MIN_OUTPUT, ModuleCal.DRIVING_MAX_OUTPUT)); + + errors += + SparkMaxUtils.check( + drivingEncoderTmp.setPositionConversionFactor( + ModuleConstants.DRIVING_ENCODER_POSITION_FACTOR_METERS)); + + errors += + SparkMaxUtils.check( + drivingEncoderTmp.setVelocityConversionFactor( + ModuleConstants.DRIVING_ENCODER_VELOCITY_FACTOR_METERS_PER_SECOND)); + + errors += + SparkMaxUtils.check(drivingSparkMax.setIdleMode(ModuleConstants.DRIVING_MOTOR_IDLE_MODE)); + errors += + SparkMaxUtils.check( + drivingSparkMax.setSmartCurrentLimit(ModuleConstants.DRIVING_MOTOR_CURRENT_LIMIT_AMPS)); + + return errors == 0; } /** diff --git a/src/main/java/frc/robot/subsystems/grabber/Grabber.java b/src/main/java/frc/robot/subsystems/grabber/Grabber.java index 7566dca..5c6fc27 100644 --- a/src/main/java/frc/robot/subsystems/grabber/Grabber.java +++ b/src/main/java/frc/robot/subsystems/grabber/Grabber.java @@ -7,8 +7,10 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Calibrations; import frc.robot.RobotMap; import frc.robot.utils.ScoringLocationUtil.ScoreHeight; +import frc.robot.utils.SparkMaxUtils; public class Grabber extends SubsystemBase { @@ -31,6 +33,10 @@ public Grabber(Command rumbleBrieflyCmd) { this.rumbleBriefly = rumbleBrieflyCmd; } + public void initialize() { + SparkMaxUtils.initWithRetry(this::initSparks, Calibrations.SPARK_INIT_RETRY_ATTEMPTS); + } + public void spinMotors(double power) { runningCommand = !(Math.abs(power) < 0.01); frontMotor.set(power); @@ -71,12 +77,19 @@ public boolean seeGamePiece() { return seeGamePieceNow; } - public void initSparks() { - frontMotor.restoreFactoryDefaults(); - backMotor.restoreFactoryDefaults(); - backMotor.follow(frontMotor); - frontMotor.setSmartCurrentLimit(GrabberCalibrations.MOTOR_CURRENT_LIMIT); - backMotor.setSmartCurrentLimit(GrabberCalibrations.MOTOR_CURRENT_LIMIT); + public boolean initSparks() { + int errors = 0; + errors += SparkMaxUtils.check(frontMotor.restoreFactoryDefaults()); + errors += SparkMaxUtils.check(backMotor.restoreFactoryDefaults()); + + errors += + SparkMaxUtils.check( + frontMotor.setSmartCurrentLimit(GrabberCalibrations.MOTOR_CURRENT_LIMIT)); + errors += + SparkMaxUtils.check( + backMotor.setSmartCurrentLimit(GrabberCalibrations.MOTOR_CURRENT_LIMIT)); + + return errors == 0; } /** diff --git a/src/main/java/frc/robot/utils/SparkMaxUtils.java b/src/main/java/frc/robot/utils/SparkMaxUtils.java index dca8c85..127ca36 100644 --- a/src/main/java/frc/robot/utils/SparkMaxUtils.java +++ b/src/main/java/frc/robot/utils/SparkMaxUtils.java @@ -1,10 +1,19 @@ package frc.robot.utils; import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.REVLibError; import java.util.function.BooleanSupplier; public class SparkMaxUtils { + /** + * @param error API return value + * @return + */ + public static int check(REVLibError error) { + return error == REVLibError.kOk ? 0 : 1; + } + public static class UnitConversions { public static void setRadsFromGearRatio(AbsoluteEncoder sparkMaxEncoder, double ratio) { double degreesPerRotation = 360.0 / ratio;