From 6932da6d11a591712b94a5a0b37b7eaf3dc2f8dc Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Mon, 29 Jan 2024 17:41:57 -0500 Subject: [PATCH] Consolidated Spark Flex Motor Factories --- .../com/team1701/robot/RobotContainer.java | 32 ++++----- ...actory.java => SparkFlexMotorFactory.java} | 52 +++++++++++++- .../shooter/ShooterMotorFactory.java | 69 ------------------- 3 files changed, 64 insertions(+), 89 deletions(-) rename src/main/java/com/team1701/robot/{subsystems/drive/DriveMotorFactory.java => SparkFlexMotorFactory.java} (62%) delete mode 100644 src/main/java/com/team1701/robot/subsystems/shooter/ShooterMotorFactory.java diff --git a/src/main/java/com/team1701/robot/RobotContainer.java b/src/main/java/com/team1701/robot/RobotContainer.java index 8c6b3b17..d0721847 100644 --- a/src/main/java/com/team1701/robot/RobotContainer.java +++ b/src/main/java/com/team1701/robot/RobotContainer.java @@ -20,15 +20,13 @@ import com.team1701.lib.util.GeometryUtil; import com.team1701.lib.util.LoggedTunableNumber; import com.team1701.robot.Configuration.Mode; +import com.team1701.robot.SparkFlexMotorFactory.ShooterMotorUsage; import com.team1701.robot.commands.AutonomousCommands; import com.team1701.robot.commands.DriveCommands; import com.team1701.robot.states.RobotState; import com.team1701.robot.subsystems.drive.Drive; -import com.team1701.robot.subsystems.drive.DriveMotorFactory; import com.team1701.robot.subsystems.drive.SwerveModule.SwerveModuleIO; import com.team1701.robot.subsystems.shooter.Shooter; -import com.team1701.robot.subsystems.shooter.ShooterMotorFactory; -import com.team1701.robot.subsystems.shooter.ShooterMotorFactory.ShooterMotorUsage; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; @@ -65,31 +63,31 @@ public RobotContainer() { new GyroIOPigeon2(10), new SwerveModuleIO[] { new SwerveModuleIO( - DriveMotorFactory.createDriveMotorIOSparkMax(10), - DriveMotorFactory.createSteerMotorIOSparkMax(11), + SparkFlexMotorFactory.createDriveMotorIOSparkMax(10), + SparkFlexMotorFactory.createSteerMotorIOSparkMax(11), new EncoderIOAnalog(0)), new SwerveModuleIO( - DriveMotorFactory.createDriveMotorIOSparkMax(12), - DriveMotorFactory.createSteerMotorIOSparkMax(13), + SparkFlexMotorFactory.createDriveMotorIOSparkMax(12), + SparkFlexMotorFactory.createSteerMotorIOSparkMax(13), new EncoderIOAnalog(1)), new SwerveModuleIO( - DriveMotorFactory.createDriveMotorIOSparkMax(16), - DriveMotorFactory.createSteerMotorIOSparkMax(17), + SparkFlexMotorFactory.createDriveMotorIOSparkMax(16), + SparkFlexMotorFactory.createSteerMotorIOSparkMax(17), new EncoderIOAnalog(3)), new SwerveModuleIO( - DriveMotorFactory.createDriveMotorIOSparkMax(14), - DriveMotorFactory.createSteerMotorIOSparkMax(15), + SparkFlexMotorFactory.createDriveMotorIOSparkMax(14), + SparkFlexMotorFactory.createSteerMotorIOSparkMax(15), new EncoderIOAnalog(2)), }, mRobotState)); // TODO: update IDs shooter = Optional.of(new Shooter( - ShooterMotorFactory.createShooterMotorIOSparkFlex( + SparkFlexMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER), - ShooterMotorFactory.createShooterMotorIOSparkFlex( + SparkFlexMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER), - ShooterMotorFactory.createShooterMotorIOSparkFlex( + SparkFlexMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterRotationMotorId, ShooterMotorUsage.ROTATION), new EncoderIOAnalog(Constants.Shooter.kShooterThroughBoreEncoderId))); break; @@ -138,11 +136,11 @@ public RobotContainer() { new AprilTagCameraIO() {})); */ this.mShooter = shooter.orElseGet(() -> new Shooter( - ShooterMotorFactory.createShooterMotorIOSparkFlex( + SparkFlexMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER), - ShooterMotorFactory.createShooterMotorIOSparkFlex( + SparkFlexMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER), - ShooterMotorFactory.createShooterMotorIOSparkFlex( + SparkFlexMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterRotationMotorId, ShooterMotorUsage.ROTATION), new EncoderIOAnalog(Constants.Shooter.kShooterThroughBoreEncoderId))); diff --git a/src/main/java/com/team1701/robot/subsystems/drive/DriveMotorFactory.java b/src/main/java/com/team1701/robot/SparkFlexMotorFactory.java similarity index 62% rename from src/main/java/com/team1701/robot/subsystems/drive/DriveMotorFactory.java rename to src/main/java/com/team1701/robot/SparkFlexMotorFactory.java index 279fb507..db541dad 100644 --- a/src/main/java/com/team1701/robot/subsystems/drive/DriveMotorFactory.java +++ b/src/main/java/com/team1701/robot/SparkFlexMotorFactory.java @@ -1,15 +1,56 @@ -package com.team1701.robot.subsystems.drive; +package com.team1701.robot; import java.util.function.Supplier; +import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.team1701.lib.alerts.REVAlert; +import com.team1701.lib.drivers.motors.MotorIOSparkFlex; import com.team1701.lib.drivers.motors.MotorIOSparkMax; -import com.team1701.robot.Constants; -public final class DriveMotorFactory { +public class SparkFlexMotorFactory { + public static MotorIOSparkFlex createShooterMotorIOSparkFlex(int deviceId, ShooterMotorUsage motorUse) { + var motor = new CANSparkFlex(deviceId, MotorType.kBrushless); + var encoder = motor.getEncoder(); + var controller = motor.getPIDController(); + var errorAlert = new REVAlert(motor, deviceId); + + motor.setCANTimeout(200); + + // TODO: Update values for actual shooter + configureWithRetry(() -> motor.restoreFactoryDefaults(), errorAlert); + + configureWithRetry(() -> motor.setSmartCurrentLimit(80), errorAlert); + configureWithRetry(() -> motor.enableVoltageCompensation(12), errorAlert); + + configureWithRetry(() -> encoder.setPosition(0), errorAlert); + configureWithRetry(() -> encoder.setMeasurementPeriod(10), errorAlert); + configureWithRetry(() -> encoder.setAverageDepth(2), errorAlert); + + switch (motorUse) { + case ROLLER: + configureWithRetry(() -> controller.setP(Constants.Shooter.kRollerKp.get()), errorAlert); + configureWithRetry(() -> controller.setD(Constants.Shooter.kRollerKd.get()), errorAlert); + configureWithRetry(() -> controller.setFF(Constants.Shooter.kRollerKff.get()), errorAlert); + break; + case ROTATION: + configureWithRetry(() -> controller.setP(Constants.Shooter.kRotationKp.get()), errorAlert); + configureWithRetry(() -> controller.setD(Constants.Shooter.kRotationKd.get()), errorAlert); + configureWithRetry(() -> controller.setFF(Constants.Shooter.kRotationKff.get()), errorAlert); + break; + default: + break; + } + + configureWithRetry(() -> motor.burnFlash(), errorAlert); + + motor.setCANTimeout(0); + + return new MotorIOSparkFlex(motor, Constants.Shooter.kShooterReduction); + } + public static MotorIOSparkMax createDriveMotorIOSparkMax(int deviceId) { var motor = new CANSparkMax(deviceId, MotorType.kBrushless); var encoder = motor.getEncoder(); @@ -83,4 +124,9 @@ private static void configureWithRetry(Supplier config, REVAlert fa failureAlert.enable(error); } + + public enum ShooterMotorUsage { + ROLLER, + ROTATION + } } diff --git a/src/main/java/com/team1701/robot/subsystems/shooter/ShooterMotorFactory.java b/src/main/java/com/team1701/robot/subsystems/shooter/ShooterMotorFactory.java deleted file mode 100644 index 5d9da214..00000000 --- a/src/main/java/com/team1701/robot/subsystems/shooter/ShooterMotorFactory.java +++ /dev/null @@ -1,69 +0,0 @@ -package com.team1701.robot.subsystems.shooter; - -import java.util.function.Supplier; - -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.REVLibError; -import com.team1701.lib.alerts.REVAlert; -import com.team1701.lib.drivers.motors.MotorIOSparkFlex; -import com.team1701.robot.Constants; - -public class ShooterMotorFactory { - public static MotorIOSparkFlex createShooterMotorIOSparkFlex(int deviceId, ShooterMotorUsage motorUse) { - var motor = new CANSparkFlex(deviceId, MotorType.kBrushless); - var encoder = motor.getEncoder(); - var controller = motor.getPIDController(); - var errorAlert = new REVAlert(motor, deviceId); - - motor.setCANTimeout(200); - - // TODO: Update values for actual shooter - configureWithRetry(() -> motor.restoreFactoryDefaults(), errorAlert); - - configureWithRetry(() -> motor.setSmartCurrentLimit(80), errorAlert); - configureWithRetry(() -> motor.enableVoltageCompensation(12), errorAlert); - - configureWithRetry(() -> encoder.setPosition(0), errorAlert); - configureWithRetry(() -> encoder.setMeasurementPeriod(10), errorAlert); - configureWithRetry(() -> encoder.setAverageDepth(2), errorAlert); - - switch (motorUse) { - case ROLLER: - configureWithRetry(() -> controller.setP(Constants.Shooter.kRollerKp.get()), errorAlert); - configureWithRetry(() -> controller.setD(Constants.Shooter.kRollerKd.get()), errorAlert); - configureWithRetry(() -> controller.setFF(Constants.Shooter.kRollerKff.get()), errorAlert); - break; - case ROTATION: - configureWithRetry(() -> controller.setP(Constants.Shooter.kRotationKp.get()), errorAlert); - configureWithRetry(() -> controller.setD(Constants.Shooter.kRotationKd.get()), errorAlert); - configureWithRetry(() -> controller.setFF(Constants.Shooter.kRotationKff.get()), errorAlert); - break; - default: - break; - } - - configureWithRetry(() -> motor.burnFlash(), errorAlert); - - motor.setCANTimeout(0); - - return new MotorIOSparkFlex(motor, Constants.Shooter.kShooterReduction); - } - - private static void configureWithRetry(Supplier config, REVAlert failureAlert) { - REVLibError error = REVLibError.kUnknown; - for (var i = 0; i < 4; i++) { - error = config.get(); - if (error == REVLibError.kOk) { - return; - } - } - - failureAlert.enable(error); - } - - public enum ShooterMotorUsage { - ROLLER, - ROTATION - } -}