From 41b06f5636eb9bd65fe9fcd34278a150140afaba Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Sat, 29 Mar 2025 21:23:48 -0400 Subject: [PATCH] Preliminary PivotSubsystem and a bunch of other junk --- .../robot/interfaces/IPIDFMotorSubsystem.java | 23 ++++ ...eMotor.java => ISimpleMotorSubsystem.java} | 6 +- .../subsystems/FrisbeeShooterWheels.java | 4 +- .../frc/robot/subsystems/PivotSubsystem.java | 107 ++++++++++++++++++ .../java/frc/robot/utilities/PIDFFValues.java | 77 +++++++++++++ .../java/frc/robot/utilities/PIDValues.java | 66 +++++++++++ .../frc/robot/utilities/ValueEnforcer.java | 24 ++++ 7 files changed, 303 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/interfaces/IPIDFMotorSubsystem.java rename src/main/java/frc/robot/interfaces/{ISimpleMotor.java => ISimpleMotorSubsystem.java} (60%) create mode 100644 src/main/java/frc/robot/subsystems/PivotSubsystem.java create mode 100644 src/main/java/frc/robot/utilities/PIDFFValues.java create mode 100644 src/main/java/frc/robot/utilities/PIDValues.java create mode 100644 src/main/java/frc/robot/utilities/ValueEnforcer.java diff --git a/src/main/java/frc/robot/interfaces/IPIDFMotorSubsystem.java b/src/main/java/frc/robot/interfaces/IPIDFMotorSubsystem.java new file mode 100644 index 0000000..d8d0b8b --- /dev/null +++ b/src/main/java/frc/robot/interfaces/IPIDFMotorSubsystem.java @@ -0,0 +1,23 @@ +package frc.robot.interfaces; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.Command; + +public interface IPIDFMotorSubsystem { + public boolean atSetpoint(); + + public Command goToSetpoint(DoubleSupplier setpoint); + + public default Command goToSetpointAndStop(DoubleSupplier setpoint) { + return goToSetpoint(setpoint) + .until(this::atSetpoint); + } + + public default Command goToSetpointAndStop(DoubleSupplier setpoint, double timeout) { + return goToSetpointAndStop(setpoint) + .withTimeout(timeout); + } + + public Command maintainPosition(); +} diff --git a/src/main/java/frc/robot/interfaces/ISimpleMotor.java b/src/main/java/frc/robot/interfaces/ISimpleMotorSubsystem.java similarity index 60% rename from src/main/java/frc/robot/interfaces/ISimpleMotor.java rename to src/main/java/frc/robot/interfaces/ISimpleMotorSubsystem.java index bd68f39..7fb2cf2 100644 --- a/src/main/java/frc/robot/interfaces/ISimpleMotor.java +++ b/src/main/java/frc/robot/interfaces/ISimpleMotorSubsystem.java @@ -4,8 +4,10 @@ import java.util.function.DoubleSupplier; import edu.wpi.first.wpilibj2.command.Command; -public interface ISimpleMotor { +public interface ISimpleMotorSubsystem { public Command setSpeed(DoubleSupplier speed); - public Command stop(); + public default Command stop() { + return setSpeed(() -> 0); + } } diff --git a/src/main/java/frc/robot/subsystems/FrisbeeShooterWheels.java b/src/main/java/frc/robot/subsystems/FrisbeeShooterWheels.java index c004a19..f3dc4bd 100644 --- a/src/main/java/frc/robot/subsystems/FrisbeeShooterWheels.java +++ b/src/main/java/frc/robot/subsystems/FrisbeeShooterWheels.java @@ -8,9 +8,9 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.FrisbeeShooterWheelsConstants; -import frc.robot.interfaces.ISimpleMotor; +import frc.robot.interfaces.ISimpleMotorSubsystem; -public class FrisbeeShooterWheels extends SubsystemBase implements ISimpleMotor{ +public class FrisbeeShooterWheels extends SubsystemBase implements ISimpleMotorSubsystem{ private SparkMax frontMotor; private SparkMax rearMotor; diff --git a/src/main/java/frc/robot/subsystems/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/PivotSubsystem.java new file mode 100644 index 0000000..76599ef --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PivotSubsystem.java @@ -0,0 +1,107 @@ +package frc.robot.subsystems; + +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import edu.wpi.first.wpilibj2.command.Command; +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.PIDFFValues; +import frc.robot.utilities.ValueEnforcer; + +public class PivotSubsystem extends SubsystemBase implements ISimpleMotorSubsystem,IPIDFMotorSubsystem{ + private MotorController motor; + + private DoubleSupplier positionSource; + private Consumer positionSetter; + + private PIDController controller; + private ArmFeedforward armFF; + + private ValueEnforcer[] enforcers; + + public PivotSubsystem(MotorController motor) { + this(motor, null, null, null); + } + + public PivotSubsystem(MotorController motor, DoubleSupplier positionSource, Consumer positionSetter, + PIDFFValues pidff) { + this(motor, positionSource, positionSetter, pidff, new ValueEnforcer[]{}); + } + + public PivotSubsystem(MotorController motor, DoubleSupplier positionSource, Consumer positionSetter, + PIDFFValues pidff, ValueEnforcer... enforcers) { + + this.motor = motor; + + if(positionSource != null && positionSetter != null && pidff != null) { + this.positionSource = positionSource; + this.positionSetter = positionSetter; + + controller = new PIDController( + pidff.getP(), + pidff.getI(), + pidff.getD() + ); + controller.setTolerance(pidff.getTolerance()); + + armFF = new ArmFeedforward( + pidff.getS(), + pidff.getG(), + pidff.getV(), + pidff.getA() + ); + } + + this.enforcers = enforcers; + } + + @Override + public void periodic() { + for(ValueEnforcer enforcer : enforcers) { + enforcer.checkAndEnforce(positionSetter); + } + } + + @Override + public boolean atSetpoint() { + return controller.atSetpoint(); + } + + @Override + public Command goToSetpoint(DoubleSupplier setpoint) { + if(controller != null && armFF != null) { + return run(() -> { + motor.setVoltage( + controller.calculate(positionSource.getAsDouble(), setpoint.getAsDouble()) + + armFF.calculate(setpoint.getAsDouble(), 0) + ); + }); + } else { + return new PrintCommand("This subsystem doesn't support PID Control"); + } + } + + @Override + public Command maintainPosition() { + if (controller != null && armFF != null) { + return run(() -> { + motor.setVoltage(armFF.calculate(positionSource.getAsDouble(), 0)); + }); + } else { + return new PrintCommand("This subsystem doesn't support PID Control"); + } + } + + @Override + public Command setSpeed(DoubleSupplier speed) { + return run(() -> { + motor.set(speed.getAsDouble()); + }); + } +} diff --git a/src/main/java/frc/robot/utilities/PIDFFValues.java b/src/main/java/frc/robot/utilities/PIDFFValues.java new file mode 100644 index 0000000..d5e7bcf --- /dev/null +++ b/src/main/java/frc/robot/utilities/PIDFFValues.java @@ -0,0 +1,77 @@ +package frc.robot.utilities; + +import edu.wpi.first.util.sendable.SendableBuilder; + +public class PIDFFValues extends PIDValues { + private double s; + private double g; + private double v; + private double a; + + public PIDFFValues(double p, double tolerance, double s, double v) { + this(p, 0, tolerance, s, v); + } + + public PIDFFValues(double p, double d, double tolerance, double s, double v) { + this(p, 0, d, tolerance, s, v); + } + + public PIDFFValues(double p, double i, double d, double tolerance, double s, double v) { + this(p, i, d, tolerance, s, v, 0); + } + + public PIDFFValues(double p, double i, double d, double tolerance, double s, double v, double a) { + this(p, i, d, tolerance, s, 0, v, a); + } + + public PIDFFValues(double p, double i, double d, double tolerance, double s, double g, double v, double a) { + super(p, i, d, tolerance); + + this.s = s; + this.g = g; + this.v = v; + this.a = a; + } + + public double getS() { + return s; + } + + public double getG() { + return g; + } + + public double getV() { + return v; + } + + public double getA() { + return a; + } + + public void setS(double s) { + this.s = s; + } + + public void setG(double g) { + this.g = g; + } + + public void setV(double v) { + this.v = v; + } + + public void setA(double a) { + this.a = a; + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + + builder.addDoubleProperty("S", this::getS, this::setS); + builder.addDoubleProperty("G", this::getG, this::setG); + builder.addDoubleProperty("V", this::getV, this::setV); + builder.addDoubleProperty("A", this::getA, this::setA); + } +} diff --git a/src/main/java/frc/robot/utilities/PIDValues.java b/src/main/java/frc/robot/utilities/PIDValues.java new file mode 100644 index 0000000..b994f97 --- /dev/null +++ b/src/main/java/frc/robot/utilities/PIDValues.java @@ -0,0 +1,66 @@ +package frc.robot.utilities; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; + +public class PIDValues implements Sendable { + private double p; + private double i; + private double d; + private double tolerance; + + public PIDValues(double p, double tolerance) { + this(p, 0, tolerance); + } + + public PIDValues(double p, double d, double tolerance) { + this(p, 0, d, tolerance); + } + + public PIDValues(double p, double i, double d, double tolerance) { + this.p = p; + this.i = i; + this.d = d; + this.tolerance = tolerance; + } + + public void setP(double p) { + this.p = p; + } + + public void setI(double i) { + this.i = i; + } + + public void setD(double d) { + this.d = d; + } + + public void setTolerance(double tolerance) { + this.tolerance = tolerance; + } + + public double getP() { + return p; + } + + public double getI() { + return i; + } + + public double getD() { + return d; + } + + public double getTolerance() { + return tolerance; + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.addDoubleProperty("P", this::getP, this::setP); + builder.addDoubleProperty("I", this::getI, this::setI); + builder.addDoubleProperty("D", this::getD, this::setD); + builder.addDoubleProperty("Tolerance", this::getTolerance, this::setTolerance); + } +} diff --git a/src/main/java/frc/robot/utilities/ValueEnforcer.java b/src/main/java/frc/robot/utilities/ValueEnforcer.java new file mode 100644 index 0000000..a3eed8e --- /dev/null +++ b/src/main/java/frc/robot/utilities/ValueEnforcer.java @@ -0,0 +1,24 @@ +package frc.robot.utilities; + +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; + +public class ValueEnforcer { + private BooleanSupplier shouldEnforce; + + private double valueToEnforce; + + public ValueEnforcer(BooleanSupplier shouldEnforce, double valueToEnforce) { + this.shouldEnforce = shouldEnforce; + this.valueToEnforce = valueToEnforce; + } + + public boolean checkAndEnforce(Consumer setValue) { + if (shouldEnforce.getAsBoolean()) { + setValue.accept(valueToEnforce); + return true; + } + + return false; + } +}