Skip to content

Commit

Permalink
Added Implementation of SparkMaxUtils.initWithRetry() (#33)
Browse files Browse the repository at this point in the history
* Implemented SparkMaxUtils.initWithRetry() Method

- added errors += SparkMaxUtils.check() stuff to the initSparks() method in Arm, Grabber, SwerveModule
- added SparkMaxUtils.initWithRetry() line to each constructor in Arm, Grabber, SwerveModule

* Did Ava figure out how to use git cherry-pick? Let's see

* Revisions to Added Init With Retry

* More Revisions to this PR 😔

* 🚨 FINAL CHANGES (HOPEFULLY 🤞) 🚨

* FINAL FINAL (no more comments, BASEL 🫵)

* Formatting fixes

---------

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
avakimc and github-actions[bot] authored Nov 2, 2023

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
1 parent bc1d85a commit 0821b43
Showing 9 changed files with 165 additions and 63 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Calibrations.java
Original file line number Diff line number Diff line change
@@ -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,
3 changes: 3 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
@@ -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
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/RobotMap.java
Original file line number Diff line number Diff line change
@@ -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;
}
67 changes: 46 additions & 21 deletions src/main/java/frc/robot/subsystems/arm/Arm.java
Original file line number Diff line number Diff line change
@@ -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<ArmPosition, Double>();
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;
}

/**
7 changes: 7 additions & 0 deletions src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
Original file line number Diff line number Diff line change
@@ -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;
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/drive/ModuleCal.java
Original file line number Diff line number Diff line change
@@ -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;
}
111 changes: 78 additions & 33 deletions src/main/java/frc/robot/subsystems/drive/SwerveModule.java
Original file line number Diff line number Diff line change
@@ -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;
}

/**
25 changes: 19 additions & 6 deletions src/main/java/frc/robot/subsystems/grabber/Grabber.java
Original file line number Diff line number Diff line change
@@ -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;
}

/**
9 changes: 9 additions & 0 deletions src/main/java/frc/robot/utils/SparkMaxUtils.java
Original file line number Diff line number Diff line change
@@ -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;

0 comments on commit 0821b43

Please sign in to comment.