From 47743f23abbaaf60843b3dab31b57a4ff1afefa8 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Thu, 12 Mar 2026 12:49:07 -0400 Subject: [PATCH] A bunch of changes + the first Subsystem --- .../iocore/abstracts/MotorControllerIO.java | 3 + .../iocore/commons/CommonEncoderConfig.java | 39 ++++++++++ .../commons/CommonMotorControllerConfig.java | 73 ++++++++++++++++++- .../team2648/iocore/realio/SparkMaxIO.java | 3 +- .../SimpleDifferentialDrivetrain.java | 43 +++++++++++ 5 files changed, 158 insertions(+), 3 deletions(-) create mode 100644 src/main/java/com/team2648/iocore/commons/CommonEncoderConfig.java create mode 100644 src/main/java/com/team2648/iocore/subsystems/SimpleDifferentialDrivetrain.java diff --git a/src/main/java/com/team2648/iocore/abstracts/MotorControllerIO.java b/src/main/java/com/team2648/iocore/abstracts/MotorControllerIO.java index edf532a..ebc57fd 100644 --- a/src/main/java/com/team2648/iocore/abstracts/MotorControllerIO.java +++ b/src/main/java/com/team2648/iocore/abstracts/MotorControllerIO.java @@ -6,6 +6,9 @@ import java.util.function.DoubleSupplier; import com.team2648.iocore.commons.CommonMotorControllerConfig; import com.team2648.iocore.enums.MotorControllerEncoderType; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; + public abstract class MotorControllerIO extends IOBase { public enum ControlTypes { kDutyCycle, diff --git a/src/main/java/com/team2648/iocore/commons/CommonEncoderConfig.java b/src/main/java/com/team2648/iocore/commons/CommonEncoderConfig.java new file mode 100644 index 0000000..010e4e6 --- /dev/null +++ b/src/main/java/com/team2648/iocore/commons/CommonEncoderConfig.java @@ -0,0 +1,39 @@ +package com.team2648.iocore.commons; + +public class CommonEncoderConfig { + private double positionConversionFactor; + private double velocityConversionFactor; + + public CommonEncoderConfig() { + this(1); + } + + public CommonEncoderConfig(double positionConversionFactor) { + this(positionConversionFactor, 1); + } + + public CommonEncoderConfig(double positionConversionFactor, double velocityConversionFactor) { + this.positionConversionFactor = positionConversionFactor; + this.velocityConversionFactor = velocityConversionFactor; + } + + public double getPositionConversionFactor() { + return positionConversionFactor; + } + + public double getVelocityConversionFactor() { + return velocityConversionFactor; + } + + public CommonEncoderConfig setPositionConversionFactor(double positionConversionFactor) { + this.positionConversionFactor = positionConversionFactor; + + return this; + } + + public CommonEncoderConfig setVelocityConversionFactor(double velocityConversionFactor) { + this.velocityConversionFactor = velocityConversionFactor; + + return this; + } +} diff --git a/src/main/java/com/team2648/iocore/commons/CommonMotorControllerConfig.java b/src/main/java/com/team2648/iocore/commons/CommonMotorControllerConfig.java index c04499f..ddeec84 100644 --- a/src/main/java/com/team2648/iocore/commons/CommonMotorControllerConfig.java +++ b/src/main/java/com/team2648/iocore/commons/CommonMotorControllerConfig.java @@ -1,7 +1,11 @@ package com.team2648.iocore.commons; +import java.util.HashMap; +import java.util.Optional; + import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.team2648.iocore.enums.MotorControllerEncoderType; import com.team2648.iocore.enums.MotorIdleMode; public class CommonMotorControllerConfig { @@ -12,12 +16,15 @@ public class CommonMotorControllerConfig { private MotorIdleMode mode; + private HashMap encoderConfigs; public CommonMotorControllerConfig() { isInverted = false; supplyCurrentLimit = -1; followCANID = -1; mode = MotorIdleMode.kCoast; + + encoderConfigs = new HashMap(); } public boolean isInverted() { @@ -36,6 +43,14 @@ public class CommonMotorControllerConfig { return mode; } + public Optional getEncoderConfig(MotorControllerEncoderType type) { + if(encoderConfigs.containsKey(type)) { + return Optional.of(encoderConfigs.get(type)); + } else { + return Optional.empty(); + } + } + public CommonMotorControllerConfig setInverted(boolean inverted) { this.isInverted = inverted; @@ -60,6 +75,44 @@ public class CommonMotorControllerConfig { return this; } + public CommonMotorControllerConfig setEncoderConfig(MotorControllerEncoderType type, CommonEncoderConfig config) { + encoderConfigs.put(type, config); + + return this; + } + + public CommonMotorControllerConfig setEncoderPositionConversionFactor(MotorControllerEncoderType type, double positionConversionFactor) { + if(!encoderConfigs.containsKey(type)) { + encoderConfigs.put(type, new CommonEncoderConfig()); + } + + encoderConfigs.get(type).setPositionConversionFactor(positionConversionFactor); + + return this; + } + + public CommonMotorControllerConfig setEncoderVelocityConversionFactor(MotorControllerEncoderType type, double velocityConversionFactor) { + if(!encoderConfigs.containsKey(type)) { + encoderConfigs.put(type, new CommonEncoderConfig()); + } + + encoderConfigs.get(type).setVelocityConversionFactor(velocityConversionFactor); + + return this; + } + + public CommonMotorControllerConfig setEncoderConversionFactors(MotorControllerEncoderType type, double positionConversionFactor, double velocityConversionFactor) { + if(!encoderConfigs.containsKey(type)) { + encoderConfigs.put(type, new CommonEncoderConfig()); + } + + encoderConfigs.get(type) + .setPositionConversionFactor(positionConversionFactor) + .setVelocityConversionFactor(velocityConversionFactor); + + return this; + } + public SparkMaxConfig getAsSparkMaxConfig() { // TODO More config SparkMaxConfig config = new SparkMaxConfig(); @@ -73,7 +126,25 @@ public class CommonMotorControllerConfig { if(followCANID >= 0) { config.follow(followCANID); } - + + if(encoderConfigs.containsKey(MotorControllerEncoderType.kInternal)) { + config.encoder + .positionConversionFactor(encoderConfigs.get(MotorControllerEncoderType.kInternal).getPositionConversionFactor()) + .velocityConversionFactor(encoderConfigs.get(MotorControllerEncoderType.kInternal).getVelocityConversionFactor()); + } + + if(encoderConfigs.containsKey(MotorControllerEncoderType.kAbsolute)) { + config.absoluteEncoder + .positionConversionFactor(encoderConfigs.get(MotorControllerEncoderType.kAbsolute).getPositionConversionFactor()) + .velocityConversionFactor(encoderConfigs.get(MotorControllerEncoderType.kAbsolute).getVelocityConversionFactor()); + } + + if(encoderConfigs.containsKey(MotorControllerEncoderType.kExternal)) { + config.alternateEncoder + .positionConversionFactor(encoderConfigs.get(MotorControllerEncoderType.kExternal).getPositionConversionFactor()) + .velocityConversionFactor(encoderConfigs.get(MotorControllerEncoderType.kExternal).getVelocityConversionFactor()); + } + return config; } } diff --git a/src/main/java/com/team2648/iocore/realio/SparkMaxIO.java b/src/main/java/com/team2648/iocore/realio/SparkMaxIO.java index 40b1116..8c273b6 100644 --- a/src/main/java/com/team2648/iocore/realio/SparkMaxIO.java +++ b/src/main/java/com/team2648/iocore/realio/SparkMaxIO.java @@ -79,6 +79,5 @@ public class SparkMaxIO extends MotorControllerIO { default: throw new UnsupportedOperationException("SparkMaxIO does not support MotorControllerEncoderType " + type.name()); } - } - + } } diff --git a/src/main/java/com/team2648/iocore/subsystems/SimpleDifferentialDrivetrain.java b/src/main/java/com/team2648/iocore/subsystems/SimpleDifferentialDrivetrain.java new file mode 100644 index 0000000..05fed71 --- /dev/null +++ b/src/main/java/com/team2648/iocore/subsystems/SimpleDifferentialDrivetrain.java @@ -0,0 +1,43 @@ +package com.team2648.iocore.subsystems; + +import java.util.function.DoubleSupplier; + +import com.team2648.iocore.abstracts.MotorControllerIO; +import com.team2648.iocore.abstracts.MotorControllerIO.ControlTypes; + +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class SimpleDifferentialDrivetrain extends SubsystemBase { + private MotorControllerIO leftIO; + private MotorControllerIO rightIO; + + private DifferentialDrive drive; + + public SimpleDifferentialDrivetrain(MotorControllerIO leftIO, MotorControllerIO rightIO) { + this.leftIO = leftIO; + this.rightIO = rightIO; + + drive = new DifferentialDrive( + (d) -> leftIO.run(ControlTypes.kDutyCycle, d), + (d) -> rightIO.run(ControlTypes.kDutyCycle, d) + ); + } + + public Command driveArcade(DoubleSupplier xSpeed, DoubleSupplier zRotation) { + return run(() -> { + drive.arcadeDrive(xSpeed.getAsDouble(), zRotation.getAsDouble()); + }); + } + + public Command driveTank(DoubleSupplier leftXSpeed, DoubleSupplier rightXSpeed) { + return run(() -> { + drive.tankDrive(leftXSpeed.getAsDouble(), rightXSpeed.getAsDouble()); + }); + } + + public Command stop() { + return driveTank(() -> 0, () -> 0); + } +}