From 360f8d6417b5a8a8708a91ed925c58f9d5ab2083 Mon Sep 17 00:00:00 2001 From: Yxhej Date: Thu, 26 Oct 2023 09:30:29 -0400 Subject: [PATCH] implement inline motor config w/ sparkutils --- .../lib/constants/SparkUtils.java | 16 +++--- .../org/sciborgs1155/robot/Constants.java | 1 - .../sciborgs1155/robot/arm/ArmConstants.java | 11 ---- .../org/sciborgs1155/robot/arm/RealElbow.java | 52 ++++++++++++------- .../sciborgs1155/robot/arm/RealElevator.java | 52 ++++++++++++------- .../org/sciborgs1155/robot/arm/RealWrist.java | 19 ++++--- .../robot/drive/DriveConstants.java | 9 ---- .../robot/drive/MAXSwerveModule.java | 31 +++++++---- .../org/sciborgs1155/robot/intake/Intake.java | 13 ++++- 9 files changed, 120 insertions(+), 84 deletions(-) diff --git a/src/main/java/org/sciborgs1155/lib/constants/SparkUtils.java b/src/main/java/org/sciborgs1155/lib/constants/SparkUtils.java index 567c8b8..ed63c8c 100644 --- a/src/main/java/org/sciborgs1155/lib/constants/SparkUtils.java +++ b/src/main/java/org/sciborgs1155/lib/constants/SparkUtils.java @@ -1,6 +1,5 @@ package org.sciborgs1155.lib.constants; -/** Add your docs here. */ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; @@ -17,12 +16,11 @@ public class SparkUtils { * Creates and configures a CANSparkMax. * * @param port - * @param type * @param config * @return The configured CANSparkMax */ - public static CANSparkMax create(int port, MotorType type, Consumer config) { - CANSparkMax spark = new CANSparkMax(port, type); + public static CANSparkMax create(int port, Consumer config) { + CANSparkMax spark = new CANSparkMax(port, MotorType.kBrushless); spark.restoreFactoryDefaults(); config.accept(spark); sparks.add(spark); @@ -30,10 +28,8 @@ public static CANSparkMax create(int port, MotorType type, Consumer } /** - * Burn to flash all motor configs at once, accounting for CAN bus delay. Use after fully + * Burn all motor configs to flash at the same time, accounting for CAN bus delay. Use after fully * configuring motors. - * - * @param sparks */ public static void safeBurnFlash() { Timer.delay(0.2); @@ -44,6 +40,12 @@ public static void safeBurnFlash() { Timer.delay(0.2); } + /** + * Disables a list of frames for a specific motor. + * + * @param spark + * @param frames + */ public static void disableFrames(CANSparkMax spark, int... frames) { for (int frame : frames) { spark.setPeriodicFramePeriod(PeriodicFrame.fromId(frame), 65535); diff --git a/src/main/java/org/sciborgs1155/robot/Constants.java b/src/main/java/org/sciborgs1155/robot/Constants.java index 82464f5..96ca032 100644 --- a/src/main/java/org/sciborgs1155/robot/Constants.java +++ b/src/main/java/org/sciborgs1155/robot/Constants.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import java.awt.Color; import java.util.Map; -import org.sciborgs1155.lib.constants.MotorConfig; import org.sciborgs1155.lib.constants.PIDConstants; /** diff --git a/src/main/java/org/sciborgs1155/robot/arm/ArmConstants.java b/src/main/java/org/sciborgs1155/robot/arm/ArmConstants.java index c5e48c6..de9bd8b 100644 --- a/src/main/java/org/sciborgs1155/robot/arm/ArmConstants.java +++ b/src/main/java/org/sciborgs1155/robot/arm/ArmConstants.java @@ -4,8 +4,6 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import org.sciborgs1155.lib.constants.ArmFFConstants; import org.sciborgs1155.lib.constants.ElevatorFFConstants; -import org.sciborgs1155.lib.constants.MotorConfig; -import org.sciborgs1155.lib.constants.MotorConfig.NeutralBehavior; import org.sciborgs1155.lib.constants.PIDConstants; import org.sciborgs1155.robot.Constants; import org.sciborgs1155.robot.Constants.Dimensions; @@ -15,9 +13,6 @@ public class ArmConstants { public static final class Wrist { - public static final MotorConfig MOTOR_CFG = - MotorConfig.base().withNeutralBehavior(NeutralBehavior.BRAKE); - public static final double MOTOR_GEARING = 53.125 / 1.0; // Gearing for motor : angle (radians) @@ -60,9 +55,6 @@ public static final class Wrist { } public static final class Elbow { - public static final MotorConfig MOTOR_CFG = - MotorConfig.base().withNeutralBehavior(NeutralBehavior.BRAKE).withCurrentLimit(50); - public static final double MOTOR_GEARING = 63.75 / 1.0; public static final double ENCODER_RATIO = 12.0 / 72.0; @@ -107,9 +99,6 @@ public static final class Elbow { } public static final class Elevator { - public static final MotorConfig MOTOR_CFG = - MotorConfig.base().withNeutralBehavior(NeutralBehavior.BRAKE).withCurrentLimit(35); - public static final double MOTOR_GEARING = 30.0 / 1.0; // Gearing for motor : height (meters) diff --git a/src/main/java/org/sciborgs1155/robot/arm/RealElbow.java b/src/main/java/org/sciborgs1155/robot/arm/RealElbow.java index 2d77c64..e47d05d 100644 --- a/src/main/java/org/sciborgs1155/robot/arm/RealElbow.java +++ b/src/main/java/org/sciborgs1155/robot/arm/RealElbow.java @@ -3,7 +3,7 @@ import static org.sciborgs1155.robot.Ports.Elbow.*; import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.CANSparkMax.IdleMode; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; @@ -13,16 +13,41 @@ import io.github.oblarg.oblog.annotations.Log; import java.util.List; import org.sciborgs1155.lib.BetterArmFeedforward; -import org.sciborgs1155.lib.constants.MotorConfig; +import org.sciborgs1155.lib.constants.SparkUtils; import org.sciborgs1155.lib.failure.FaultBuilder; import org.sciborgs1155.lib.failure.HardwareFault; import org.sciborgs1155.robot.Constants; import org.sciborgs1155.robot.arm.ArmConstants.Elbow; public class RealElbow implements JointIO { - private final CANSparkMax middleMotor; - private final CANSparkMax leftMotor; - private final CANSparkMax rightMotor; + private final CANSparkMax middleMotor = + SparkUtils.create( + MIDDLE_MOTOR, + s -> { + s.setIdleMode(IdleMode.kBrake); + s.setOpenLoopRampRate(0); + s.setSmartCurrentLimit(50); + }); + + private final CANSparkMax leftMotor = + SparkUtils.create( + LEFT_MOTOR, + s -> { + s.setIdleMode(IdleMode.kBrake); + s.setOpenLoopRampRate(0); + s.setSmartCurrentLimit(50); + s.follow(middleMotor); + }); + + private final CANSparkMax rightMotor = + SparkUtils.create( + RIGHT_MOTOR, + s -> { + s.setIdleMode(IdleMode.kBrake); + s.setOpenLoopRampRate(0); + s.setSmartCurrentLimit(50); + s.follow(middleMotor); + }); private final Encoder encoder; @@ -36,20 +61,9 @@ public class RealElbow implements JointIO { private double lastVoltage; public RealElbow(JointConfig config) { - middleMotor = Elbow.MOTOR_CFG.build(null, MIDDLE_MOTOR); - leftMotor = Elbow.MOTOR_CFG.build(MotorType.kBrushless, LEFT_MOTOR); - rightMotor = Elbow.MOTOR_CFG.build(MotorType.kBrushless, RIGHT_MOTOR); - - MotorConfig.disableFrames(middleMotor, 4, 5, 6); - MotorConfig.disableFrames(leftMotor, 1, 2, 3, 4, 5, 6); - MotorConfig.disableFrames(rightMotor, 1, 2, 3, 4, 5, 6); - - leftMotor.follow(middleMotor); - rightMotor.follow(middleMotor); - - middleMotor.burnFlash(); - leftMotor.burnFlash(); - rightMotor.burnFlash(); + SparkUtils.disableFrames(middleMotor, 4, 5, 6); + SparkUtils.disableFrames(leftMotor, 1, 2, 3, 4, 5, 6); + SparkUtils.disableFrames(rightMotor, 1, 2, 3, 4, 5, 6); encoder = new Encoder(ENCODER[0], ENCODER[1]); encoder.setDistancePerPulse(Elbow.CONVERSION); diff --git a/src/main/java/org/sciborgs1155/robot/arm/RealElevator.java b/src/main/java/org/sciborgs1155/robot/arm/RealElevator.java index 330c4f1..b3819f2 100644 --- a/src/main/java/org/sciborgs1155/robot/arm/RealElevator.java +++ b/src/main/java/org/sciborgs1155/robot/arm/RealElevator.java @@ -3,7 +3,7 @@ import static org.sciborgs1155.robot.Ports.Elevator.*; import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.CANSparkMax.IdleMode; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj.Encoder; import java.util.List; import org.sciborgs1155.lib.BetterElevatorFeedforward; -import org.sciborgs1155.lib.constants.MotorConfig; +import org.sciborgs1155.lib.constants.SparkUtils; import org.sciborgs1155.lib.failure.FaultBuilder; import org.sciborgs1155.lib.failure.HardwareFault; import org.sciborgs1155.robot.Constants; @@ -19,9 +19,34 @@ public class RealElevator implements ElevatorIO { - private final CANSparkMax lead; - private final CANSparkMax left; - private final CANSparkMax right; + private final CANSparkMax lead = + SparkUtils.create( + RIGHT_MOTOR, + s -> { + s.setIdleMode(IdleMode.kBrake); + s.setOpenLoopRampRate(0); + s.setSmartCurrentLimit(35); + }); + + private final CANSparkMax left = + SparkUtils.create( + LEFT_MOTOR, + s -> { + s.setIdleMode(IdleMode.kBrake); + s.setOpenLoopRampRate(0); + s.setSmartCurrentLimit(35); + s.follow(lead); + }); + + private final CANSparkMax right = + SparkUtils.create( + MIDDLE_MOTOR, + s -> { + s.setIdleMode(IdleMode.kBrake); + s.setOpenLoopRampRate(0); + s.setSmartCurrentLimit(35); + s.follow(lead); + }); private final Encoder encoder; @@ -35,20 +60,9 @@ public class RealElevator implements ElevatorIO { private double lastVoltage; public RealElevator(ElevatorConfig config) { - lead = Elevator.MOTOR_CFG.build(MotorType.kBrushless, RIGHT_MOTOR); - left = Elevator.MOTOR_CFG.build(MotorType.kBrushless, LEFT_MOTOR); - right = Elevator.MOTOR_CFG.build(MotorType.kBrushless, MIDDLE_MOTOR); - - left.follow(lead); - right.follow(lead); - - MotorConfig.disableFrames(lead, 4, 5, 6); - MotorConfig.disableFrames(left, 1, 2, 3, 4, 5, 6); - MotorConfig.disableFrames(right, 1, 2, 3, 4, 5, 6); - - lead.burnFlash(); - left.burnFlash(); - right.burnFlash(); + SparkUtils.disableFrames(lead, 4, 5, 6); + SparkUtils.disableFrames(left, 1, 2, 3, 4, 5, 6); + SparkUtils.disableFrames(right, 1, 2, 3, 4, 5, 6); encoder = new Encoder(ENCODER[0], ENCODER[1]); encoder.setDistancePerPulse(Elevator.CONVERSION_RELATIVE); diff --git a/src/main/java/org/sciborgs1155/robot/arm/RealWrist.java b/src/main/java/org/sciborgs1155/robot/arm/RealWrist.java index d73e6ce..fc68e2b 100644 --- a/src/main/java/org/sciborgs1155/robot/arm/RealWrist.java +++ b/src/main/java/org/sciborgs1155/robot/arm/RealWrist.java @@ -3,7 +3,7 @@ import static org.sciborgs1155.robot.Ports.Wrist.*; import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.CANSparkMax.IdleMode; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; @@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj.Encoder; import java.util.List; import org.sciborgs1155.lib.BetterArmFeedforward; -import org.sciborgs1155.lib.constants.MotorConfig; +import org.sciborgs1155.lib.constants.SparkUtils; import org.sciborgs1155.lib.failure.FaultBuilder; import org.sciborgs1155.lib.failure.HardwareFault; import org.sciborgs1155.robot.Constants; @@ -21,7 +21,15 @@ public class RealWrist implements JointIO { - private final CANSparkMax motor; + private final CANSparkMax motor = + SparkUtils.create( + MOTOR, + s -> { + s.setIdleMode(IdleMode.kBrake); + s.setOpenLoopRampRate(0); + s.setSmartCurrentLimit(80); + }); + private final DutyCycleEncoder absolute; private final Encoder relative; @@ -37,7 +45,6 @@ public class RealWrist implements JointIO { private double lastVoltage; public RealWrist(JointConfig config) { - motor = Wrist.MOTOR_CFG.build(MotorType.kBrushless, MOTOR); absolute = new DutyCycleEncoder(ABS_ENCODER); relative = new Encoder(RELATIVE_ENCODER[0], RELATIVE_ENCODER[1]); @@ -46,9 +53,7 @@ public RealWrist(JointConfig config) { relative.setDistancePerPulse(Wrist.CONVERSION_RELATIVE); relative.setReverseDirection(true); - MotorConfig.disableFrames(motor, 4, 5, 6); - - motor.burnFlash(); + SparkUtils.disableFrames(motor, 4, 5, 6); ff = config.ff().createFeedforward(); pid = config.pid().createPIDController(); diff --git a/src/main/java/org/sciborgs1155/robot/drive/DriveConstants.java b/src/main/java/org/sciborgs1155/robot/drive/DriveConstants.java index a095ad6..599f5c5 100644 --- a/src/main/java/org/sciborgs1155/robot/drive/DriveConstants.java +++ b/src/main/java/org/sciborgs1155/robot/drive/DriveConstants.java @@ -3,12 +3,9 @@ import com.pathplanner.lib.PathConstraints; import edu.wpi.first.math.geometry.Translation2d; import org.sciborgs1155.lib.constants.BasicFFConstants; -import org.sciborgs1155.lib.constants.MotorConfig; -import org.sciborgs1155.lib.constants.MotorConfig.NeutralBehavior; import org.sciborgs1155.lib.constants.PIDConstants; public final class DriveConstants { - public static final double MAX_SPEED = 4.8; // m / s public static final double MAX_ANGULAR_SPEED = 2 * Math.PI; // rad / s public static final double MAX_ACCEL = 6.5; // m / s^2 @@ -46,9 +43,6 @@ public final class DriveConstants { public static final class SwerveModule { public static final class Driving { - public static final MotorConfig MOTOR_CFG = - MotorConfig.base().withNeutralBehavior(NeutralBehavior.BRAKE).withCurrentLimit(50); - public static final double CIRCUMFERENCE = 2.0 * Math.PI * 0.0381; // Diameter of the wheel in meters (2 * π * R) @@ -61,9 +55,6 @@ public static final class Driving { } public static final class Turning { - public static final MotorConfig MOTOR_CFG = - MotorConfig.base().withNeutralBehavior(NeutralBehavior.BRAKE).withCurrentLimit(20); - public static final double MOTOR_GEARING = 1.0 / 4.0 / 3.0; public static final double CONVERSION = 2.0 * Math.PI; diff --git a/src/main/java/org/sciborgs1155/robot/drive/MAXSwerveModule.java b/src/main/java/org/sciborgs1155/robot/drive/MAXSwerveModule.java index 1b46235..205da0e 100644 --- a/src/main/java/org/sciborgs1155/robot/drive/MAXSwerveModule.java +++ b/src/main/java/org/sciborgs1155/robot/drive/MAXSwerveModule.java @@ -3,7 +3,7 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxAbsoluteEncoder.Type; import com.revrobotics.SparkMaxPIDController; @@ -12,8 +12,8 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import java.util.List; -import org.sciborgs1155.lib.constants.MotorConfig; import org.sciborgs1155.lib.constants.PIDConstants; +import org.sciborgs1155.lib.constants.SparkUtils; import org.sciborgs1155.lib.failure.FaultBuilder; import org.sciborgs1155.lib.failure.HardwareFault; import org.sciborgs1155.robot.drive.DriveConstants.SwerveModule.Driving; @@ -49,8 +49,24 @@ public class MAXSwerveModule implements ModuleIO { public MAXSwerveModule(String name, int drivePort, int turnPort, double angularOffset) { this.name = name; - driveMotor = Driving.MOTOR_CFG.build(MotorType.kBrushless, drivePort); - turnMotor = Turning.MOTOR_CFG.build(MotorType.kBrushless, turnPort); + driveMotor = + SparkUtils.create( + drivePort, + s -> { + s.setInverted(false); + s.setIdleMode(IdleMode.kBrake); + s.setOpenLoopRampRate(0); + s.setSmartCurrentLimit(50); + }); + turnMotor = + SparkUtils.create( + turnPort, + s -> { + s.setInverted(false); + s.setIdleMode(IdleMode.kBrake); + s.setOpenLoopRampRate(0); + s.setSmartCurrentLimit(20); + }); driveEncoder = driveMotor.getEncoder(); turningEncoder = turnMotor.getAbsoluteEncoder(Type.kDutyCycle); @@ -76,11 +92,8 @@ public MAXSwerveModule(String name, int drivePort, int turnPort, double angularO turnFeedback.setPositionPIDWrappingMinInput(0); turnFeedback.setPositionPIDWrappingMaxInput(Turning.CONVERSION); - MotorConfig.disableFrames(driveMotor, 4, 5, 6); - MotorConfig.disableFrames(turnMotor, 4, 6); - - driveMotor.burnFlash(); - turnMotor.burnFlash(); + SparkUtils.disableFrames(driveMotor, 4, 5, 6); + SparkUtils.disableFrames(turnMotor, 4, 6); driveEncoder.setPosition(0); this.angularOffset = Rotation2d.fromRadians(angularOffset); diff --git a/src/main/java/org/sciborgs1155/robot/intake/Intake.java b/src/main/java/org/sciborgs1155/robot/intake/Intake.java index 2133603..0d2d57e 100644 --- a/src/main/java/org/sciborgs1155/robot/intake/Intake.java +++ b/src/main/java/org/sciborgs1155/robot/intake/Intake.java @@ -5,7 +5,7 @@ import static org.sciborgs1155.robot.intake.IntakeConstants.*; import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; @@ -14,6 +14,7 @@ import io.github.oblarg.oblog.annotations.Log; import java.util.List; import java.util.function.Supplier; +import org.sciborgs1155.lib.constants.SparkUtils; import org.sciborgs1155.lib.failure.Fallible; import org.sciborgs1155.lib.failure.FaultBuilder; import org.sciborgs1155.lib.failure.HardwareFault; @@ -23,7 +24,15 @@ public class Intake extends SubsystemBase implements Fallible, Loggable, AutoClo @Log(name = "applied output", methodName = "getAppliedOutput") @Log(name = "current", methodName = "getOutputCurrent") - private final CANSparkMax wheels = MOTOR_CFG.build(MotorType.kBrushless, WHEEL_MOTOR); + private final CANSparkMax wheels = + SparkUtils.create( + WHEEL_MOTOR, + s -> { + s.setIdleMode(IdleMode.kBrake); + s.setSmartCurrentLimit(40); + s.setOpenLoopRampRate(0); + s.setInverted(true); + }); @Log(name = "velocity", methodName = "getVelocity") private final RelativeEncoder encoder = wheels.getEncoder();