From 3424678e025358a532557878f538db08e7465b3f Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Sun, 30 Mar 2025 13:24:45 -0400 Subject: [PATCH] MOre changes and an implementation of PivotSubsystem --- src/main/java/frc/robot/RobotContainer.java | 21 ++++++++ .../frc/robot/subsystems/PivotSubsystem.java | 20 ++++--- .../robot/utilities/EncoderMotorHelper.java | 54 +++++++++++++++++++ 3 files changed, 84 insertions(+), 11 deletions(-) create mode 100644 src/main/java/frc/robot/utilities/EncoderMotorHelper.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2de5e49..68dae2f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,7 +4,11 @@ package frc.robot; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -16,11 +20,16 @@ import frc.robot.constants.OIConstants; import frc.robot.interfaces.IDrivetrain; import frc.robot.subsystems.DifferentialDrivetrain; import frc.robot.subsystems.FrisbeeShooterWheels; +import frc.robot.subsystems.PivotSubsystem; +import frc.robot.utilities.EncoderMotorHelper; import frc.robot.utilities.OIProfileManager; +import frc.robot.utilities.PIDFFValues; +import frc.robot.utilities.ValueEnforcer; public class RobotContainer { private IDrivetrain drivetrain; private FrisbeeShooterWheels frisbeeShooterWheels; + private PivotSubsystem frisbeePlatformPivot; private CommandXboxController driver; @@ -35,6 +44,18 @@ public class RobotContainer { frisbeeShooterWheels = new FrisbeeShooterWheels(); + frisbeePlatformPivot = new PivotSubsystem( + new EncoderMotorHelper( + new SparkMax(0, MotorType.kBrushless), + false + ), + new PIDFFValues(0, 1, 0, 0), + new ValueEnforcer( + (new DigitalInput(0))::get, + 0 + ) + ); + driver = new CommandXboxController(OIConstants.kDriverUSB); oiProfiles = new OIProfileManager( diff --git a/src/main/java/frc/robot/subsystems/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/PivotSubsystem.java index 76599ef..9e0e752 100644 --- a/src/main/java/frc/robot/subsystems/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PivotSubsystem.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.interfaces.IPIDFMotorSubsystem; import frc.robot.interfaces.ISimpleMotorSubsystem; +import frc.robot.utilities.EncoderMotorHelper; import frc.robot.utilities.PIDFFValues; import frc.robot.utilities.ValueEnforcer; @@ -26,22 +27,19 @@ public class PivotSubsystem extends SubsystemBase implements ISimpleMotorSubsyst private ValueEnforcer[] enforcers; public PivotSubsystem(MotorController motor) { - this(motor, null, null, null); + this(new EncoderMotorHelper(motor, null, null), null); } - public PivotSubsystem(MotorController motor, DoubleSupplier positionSource, Consumer positionSetter, - PIDFFValues pidff) { - this(motor, positionSource, positionSetter, pidff, new ValueEnforcer[]{}); + public PivotSubsystem(EncoderMotorHelper motor, PIDFFValues pidff) { + this(motor, pidff, new ValueEnforcer[]{}); } - public PivotSubsystem(MotorController motor, DoubleSupplier positionSource, Consumer positionSetter, - PIDFFValues pidff, ValueEnforcer... enforcers) { + public PivotSubsystem(EncoderMotorHelper motor, PIDFFValues pidff, ValueEnforcer... enforcers) { + this.motor = motor.getMotorController(); - this.motor = motor; - - if(positionSource != null && positionSetter != null && pidff != null) { - this.positionSource = positionSource; - this.positionSetter = positionSetter; + if(motor.getPositionSource() != null && motor.getPositionSetter() != null && pidff != null) { + this.positionSource = motor.getPositionSource(); + this.positionSetter = motor.getPositionSetter(); controller = new PIDController( pidff.getP(), diff --git a/src/main/java/frc/robot/utilities/EncoderMotorHelper.java b/src/main/java/frc/robot/utilities/EncoderMotorHelper.java new file mode 100644 index 0000000..f9c2529 --- /dev/null +++ b/src/main/java/frc/robot/utilities/EncoderMotorHelper.java @@ -0,0 +1,54 @@ +package frc.robot.utilities; + +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; + +import com.revrobotics.spark.SparkBase; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; + +public class EncoderMotorHelper { + private MotorController controller; + + private DoubleSupplier positionSource; + + private Consumer positionSetter; + + public EncoderMotorHelper(MotorController controller, Encoder encoder) { + this.controller = controller; + + positionSource = encoder::getDistance; + positionSetter = (d) -> encoder.reset(); + } + + public EncoderMotorHelper(SparkBase spark, boolean useAbsolutePosition) { + controller = spark; + + if (useAbsolutePosition) { + positionSource = spark.getAbsoluteEncoder()::getPosition; + positionSetter = (d) -> {}; // TODO Do nothing position setter doesn't make a ton of sense... + } else { + positionSource = spark.getEncoder()::getPosition; + positionSetter = spark.getEncoder()::setPosition; + } + } + + public EncoderMotorHelper(MotorController controller, DoubleSupplier positionSource, Consumer positionSetter) { + this.controller = controller; + this.positionSource = positionSource; + this.positionSetter = positionSetter; + } + + public MotorController getMotorController() { + return controller; + } + + public DoubleSupplier getPositionSource() { + return positionSource; + } + + public Consumer getPositionSetter() { + return positionSetter; + } +}