From 7da8751376fd8cddaefbdec6b466bd2e92d88017 Mon Sep 17 00:00:00 2001 From: TapChap Date: Tue, 13 May 2025 18:46:05 +0300 Subject: [PATCH 01/17] implemented GenericFF to allow applyGains method for easy gains application for FF and PID controller --- .../GenericFF/FeedForwardGainsSetter.java | 11 +++ .../excalib/control/GenericFF/GenericFF.java | 96 +++++++++++++++++++ .../java/frc/excalib/control/gains/Gains.java | 30 +++++- .../motor/{controllers => }/Motor.java | 2 +- .../control/motor/controllers/FlexMotor.java | 1 + .../control/motor/controllers/MotorGroup.java | 1 + .../motor/controllers/SparkMaxMotor.java | 1 + .../motor/controllers/TalonFXMotor.java | 1 + .../frc/excalib/mechanisms/Mechanism.java | 2 +- .../java/frc/excalib/mechanisms/arm/Arm.java | 2 +- .../mechanisms/fly_wheel/FlyWheel.java | 2 +- .../linear_extension/LinearExtension.java | 2 +- .../frc/excalib/mechanisms/turret/Turret.java | 2 +- .../java/frc/excalib/swerve/SwerveModule.java | 2 +- 14 files changed, 147 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java create mode 100644 src/main/java/frc/excalib/control/GenericFF/GenericFF.java rename src/main/java/frc/excalib/control/motor/{controllers => }/Motor.java (98%) diff --git a/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java b/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java new file mode 100644 index 0000000..91ee861 --- /dev/null +++ b/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java @@ -0,0 +1,11 @@ +package frc.excalib.control.GenericFF; + +/** + * Interface for feedforward controllers that have kS, kV, and kA gains. + */ +public interface FeedForwardGainsSetter { + void setKs(double kS); + void setKv(double kV); + void setKa(double kA); + void setKg(double kG); +} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/GenericFF/GenericFF.java b/src/main/java/frc/excalib/control/GenericFF/GenericFF.java new file mode 100644 index 0000000..8cc41b3 --- /dev/null +++ b/src/main/java/frc/excalib/control/GenericFF/GenericFF.java @@ -0,0 +1,96 @@ +package frc.excalib.control.GenericFF; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; + +public class GenericFF { + public static class ArmFF extends ArmFeedforward implements FeedForwardGainsSetter { + public ArmFF() { + super(0, 0, 0, 0); + } + + @Override + public void setKs(double kS) { + super.setKs(kS); + } + + + @Override + public void setKv(double kV) { + super.setKv(kV); + } + + + @Override + public void setKa(double kA) { + super.setKa(kA); + } + + + @Override + public void setKg(double kG) { + super.setKg(kG); + } + } + + public static class SimpleFF extends SimpleMotorFeedforward implements FeedForwardGainsSetter { + public SimpleFF() { + super(0, 0, 0); + } + + + @Override + public void setKs(double kS) { + super.setKs(kS); + } + + + @Override + public void setKv(double kV) { + super.setKv(kV); + } + + + @Override + public void setKa(double kA) { + super.setKa(kA); + } + + + @Override + public void setKg(double kG) { + return; + } + } + + public static class ElevatorFF extends ElevatorFeedforward implements FeedForwardGainsSetter { + public ElevatorFF() { + super(0, 0, 0, 0); + } + + + @Override + public void setKs(double kS) { + super.setKs(kS); + } + + + @Override + public void setKv(double kV) { + super.setKv(kV); + } + + + @Override + public void setKa(double kA) { + super.setKa(kA); + } + + + @Override + public void setKg(double kG) { + super.setKg(kG); + } + } +} diff --git a/src/main/java/frc/excalib/control/gains/Gains.java b/src/main/java/frc/excalib/control/gains/Gains.java index a5be5ed..5b1de09 100644 --- a/src/main/java/frc/excalib/control/gains/Gains.java +++ b/src/main/java/frc/excalib/control/gains/Gains.java @@ -1,5 +1,10 @@ package frc.excalib.control.gains; +import edu.wpi.first.math.controller.PIDController; +import frc.excalib.control.GenericFF.FeedForwardGainsSetter; +import frc.excalib.control.GenericFF.FeedforwardGainsSetter; + + /** * This class bundles together multiple types of gains, including PID gains and feedforward gains. */ @@ -9,6 +14,7 @@ public class Gains { // Feedforward gains: static, gravity, velocity, and acceleration public double ks, kg, kv, ka; + /** * Constructs a Gains object with specified PID and feedforward gains. * @@ -30,6 +36,7 @@ public Gains(double kp, double ki, double kd, double ks, double kv, double ka, d this.ka = ka; } + /** * Constructs a Gains object with only PID gains. * Feedforward gains are set to 0. @@ -42,6 +49,7 @@ public Gains(double kp, double ki, double kd) { this(kp, ki, kd, 0, 0, 0, 0); } + /** * Constructs a Gains object with only feedforward gains. * PID gains are set to 0. @@ -55,6 +63,7 @@ public Gains(double ks, double kg, double kv, double ka) { this(0, 0, 0, ks, kv, ka, kg); } + /** * Constructs a Gains object by combining PID gains and feedforward gains from two separate Gains objects. * @@ -65,6 +74,7 @@ public Gains(Gains PIDgains, Gains FFgains) { this(PIDgains.kp, PIDgains.ki, PIDgains.kd, FFgains.ks, FFgains.kv, FFgains.ka, FFgains.kg); } + /** * Constructs a Gains object with all gains set to 0. */ @@ -72,6 +82,7 @@ public Gains() { this(0, 0, 0, 0, 0, 0, 0); } + /** * Constructs a Gains object by copying the values from another Gains object. * @@ -81,6 +92,7 @@ public Gains(Gains gains) { this(gains.kp, gains.ki, gains.kd, gains.ks, gains.kg, gains.kv, gains.ka); } + /** * Sets the PID gains. * @@ -94,6 +106,7 @@ public void setPIDgains(double kp, double ki, double kd) { this.kd = kd; } + /** * Sets the feedforward gains. * @@ -108,4 +121,19 @@ public void setFeedForwardGains(double ks, double kg, double kv, double ka) { this.kv = kv; this.ka = ka; } -} \ No newline at end of file + + + public PIDController getPIDcontroller(){ + return new PIDController(this.kp, this.ki, this.kd); + } + + + public FF applyGains(FF ff){ + ff.setKv(this.kv); + ff.setKs(this.ks); + ff.setKa(this.ka); + ff.setKg(this.kg); + + return ff; + } +} diff --git a/src/main/java/frc/excalib/control/motor/controllers/Motor.java b/src/main/java/frc/excalib/control/motor/Motor.java similarity index 98% rename from src/main/java/frc/excalib/control/motor/controllers/Motor.java rename to src/main/java/frc/excalib/control/motor/Motor.java index 278dd45..4e9a712 100644 --- a/src/main/java/frc/excalib/control/motor/controllers/Motor.java +++ b/src/main/java/frc/excalib/control/motor/Motor.java @@ -1,4 +1,4 @@ -package frc.excalib.control.motor.controllers; +package frc.excalib.control.motor; import frc.excalib.control.motor.motor_specs.DirectionState; import frc.excalib.control.motor.motor_specs.IdleState; diff --git a/src/main/java/frc/excalib/control/motor/controllers/FlexMotor.java b/src/main/java/frc/excalib/control/motor/controllers/FlexMotor.java index 2d1a4c3..c6e44cf 100644 --- a/src/main/java/frc/excalib/control/motor/controllers/FlexMotor.java +++ b/src/main/java/frc/excalib/control/motor/controllers/FlexMotor.java @@ -2,6 +2,7 @@ import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.config.SparkFlexConfig; +import frc.excalib.control.motor.Motor; import frc.excalib.control.motor.motor_specs.DirectionState; import frc.excalib.control.motor.motor_specs.IdleState; diff --git a/src/main/java/frc/excalib/control/motor/controllers/MotorGroup.java b/src/main/java/frc/excalib/control/motor/controllers/MotorGroup.java index 90bbc74..7549d59 100644 --- a/src/main/java/frc/excalib/control/motor/controllers/MotorGroup.java +++ b/src/main/java/frc/excalib/control/motor/controllers/MotorGroup.java @@ -1,5 +1,6 @@ package frc.excalib.control.motor.controllers; +import frc.excalib.control.motor.Motor; import frc.excalib.control.motor.motor_specs.DirectionState; import frc.excalib.control.motor.motor_specs.IdleState; diff --git a/src/main/java/frc/excalib/control/motor/controllers/SparkMaxMotor.java b/src/main/java/frc/excalib/control/motor/controllers/SparkMaxMotor.java index 9db6826..3530c41 100644 --- a/src/main/java/frc/excalib/control/motor/controllers/SparkMaxMotor.java +++ b/src/main/java/frc/excalib/control/motor/controllers/SparkMaxMotor.java @@ -2,6 +2,7 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkFlexConfig; +import frc.excalib.control.motor.Motor; import frc.excalib.control.motor.motor_specs.DirectionState; import frc.excalib.control.motor.motor_specs.IdleState; diff --git a/src/main/java/frc/excalib/control/motor/controllers/TalonFXMotor.java b/src/main/java/frc/excalib/control/motor/controllers/TalonFXMotor.java index a5dd60b..eca0de0 100644 --- a/src/main/java/frc/excalib/control/motor/controllers/TalonFXMotor.java +++ b/src/main/java/frc/excalib/control/motor/controllers/TalonFXMotor.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.excalib.control.motor.Motor; import frc.excalib.control.motor.motor_specs.DirectionState; import frc.excalib.control.motor.motor_specs.IdleState; diff --git a/src/main/java/frc/excalib/mechanisms/Mechanism.java b/src/main/java/frc/excalib/mechanisms/Mechanism.java index 6db6e89..b36cd9b 100644 --- a/src/main/java/frc/excalib/mechanisms/Mechanism.java +++ b/src/main/java/frc/excalib/mechanisms/Mechanism.java @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.excalib.control.gains.SysidConfig; -import frc.excalib.control.motor.controllers.Motor; +import frc.excalib.control.motor.Motor; import frc.excalib.control.motor.motor_specs.IdleState; import monologue.Logged; diff --git a/src/main/java/frc/excalib/mechanisms/arm/Arm.java b/src/main/java/frc/excalib/mechanisms/arm/Arm.java index 4354282..d4efc43 100644 --- a/src/main/java/frc/excalib/mechanisms/arm/Arm.java +++ b/src/main/java/frc/excalib/mechanisms/arm/Arm.java @@ -7,7 +7,7 @@ import frc.excalib.control.gains.Gains; import frc.excalib.control.limits.SoftLimit; import frc.excalib.control.math.physics.Mass; -import frc.excalib.control.motor.controllers.Motor; +import frc.excalib.control.motor.Motor; import frc.excalib.mechanisms.Mechanism; import java.util.function.Consumer; diff --git a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java b/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java index 28160d8..ae48f2e 100644 --- a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java +++ b/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.excalib.control.gains.Gains; -import frc.excalib.control.motor.controllers.Motor; +import frc.excalib.control.motor.Motor; import frc.excalib.mechanisms.Mechanism; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java b/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java index f6d4892..c9dcdcd 100644 --- a/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java +++ b/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.excalib.control.gains.Gains; -import frc.excalib.control.motor.controllers.Motor; +import frc.excalib.control.motor.Motor; import frc.excalib.mechanisms.Mechanism; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/excalib/mechanisms/turret/Turret.java b/src/main/java/frc/excalib/mechanisms/turret/Turret.java index 7803b86..2d25c31 100644 --- a/src/main/java/frc/excalib/mechanisms/turret/Turret.java +++ b/src/main/java/frc/excalib/mechanisms/turret/Turret.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.excalib.control.gains.Gains; import frc.excalib.control.limits.ContinuousSoftLimit; -import frc.excalib.control.motor.controllers.Motor; +import frc.excalib.control.motor.Motor; import frc.excalib.mechanisms.Mechanism; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/excalib/swerve/SwerveModule.java b/src/main/java/frc/excalib/swerve/SwerveModule.java index e21768d..cb1efd4 100644 --- a/src/main/java/frc/excalib/swerve/SwerveModule.java +++ b/src/main/java/frc/excalib/swerve/SwerveModule.java @@ -12,7 +12,7 @@ import frc.excalib.control.gains.SysidConfig; import frc.excalib.control.limits.ContinuousSoftLimit; import frc.excalib.control.math.Vector2D; -import frc.excalib.control.motor.controllers.Motor; +import frc.excalib.control.motor.Motor; import frc.excalib.mechanisms.fly_wheel.FlyWheel; import frc.excalib.mechanisms.turret.Turret; import monologue.Logged; From 5a251e1f14394f9e7b63455006f9b808babde1a6 Mon Sep 17 00:00:00 2001 From: TapChap Date: Tue, 13 May 2025 18:53:08 +0300 Subject: [PATCH 02/17] added DifferentialMotor class to allow separate control over two motors. --- .../java/frc/excalib/control/gains/Gains.java | 1 - .../motor/motorType/DifferentialMotor.java | 28 +++++++++++++++++++ .../MotorGroup.java | 19 ++++++++++--- 3 files changed, 43 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/excalib/control/motor/motorType/DifferentialMotor.java rename src/main/java/frc/excalib/control/motor/{controllers => motorType}/MotorGroup.java (91%) diff --git a/src/main/java/frc/excalib/control/gains/Gains.java b/src/main/java/frc/excalib/control/gains/Gains.java index 5b1de09..aff759e 100644 --- a/src/main/java/frc/excalib/control/gains/Gains.java +++ b/src/main/java/frc/excalib/control/gains/Gains.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.controller.PIDController; import frc.excalib.control.GenericFF.FeedForwardGainsSetter; -import frc.excalib.control.GenericFF.FeedforwardGainsSetter; /** diff --git a/src/main/java/frc/excalib/control/motor/motorType/DifferentialMotor.java b/src/main/java/frc/excalib/control/motor/motorType/DifferentialMotor.java new file mode 100644 index 0000000..5cf7a07 --- /dev/null +++ b/src/main/java/frc/excalib/control/motor/motorType/DifferentialMotor.java @@ -0,0 +1,28 @@ +package frc.excalib.control.motor.motorType; + +import edu.wpi.first.math.Pair; +import frc.excalib.control.motor.Motor; +import frc.excalib.control.motor.motorType.MotorGroup; + +import static frc.excalib.control.motor.motor_specs.DirectionState.*; + +public class DifferentialMotor extends MotorGroup { + private final Motor motorA, motorB; + + public DifferentialMotor(Motor motorA, Motor motorB, boolean negate) { + super(motorA, motorB); + + this.motorA = motorA; + this.motorB = motorB; + + motorB.setInverted(negate? REVERSE : FORWARD); + } + + public void setDifferentialVoltage(double motorAvoltage, double motorBvoltage){ + super.setDifferentialVoltage(motorAvoltage, motorBvoltage); + } + + public Pair getMotorPositions(){ + return Pair.of(motorA.getMotorPosition(), motorB.getMotorPosition()); + } +} diff --git a/src/main/java/frc/excalib/control/motor/controllers/MotorGroup.java b/src/main/java/frc/excalib/control/motor/motorType/MotorGroup.java similarity index 91% rename from src/main/java/frc/excalib/control/motor/controllers/MotorGroup.java rename to src/main/java/frc/excalib/control/motor/motorType/MotorGroup.java index 7549d59..1ef7993 100644 --- a/src/main/java/frc/excalib/control/motor/controllers/MotorGroup.java +++ b/src/main/java/frc/excalib/control/motor/motorType/MotorGroup.java @@ -1,4 +1,4 @@ -package frc.excalib.control.motor.controllers; +package frc.excalib.control.motor.motorType; import frc.excalib.control.motor.Motor; import frc.excalib.control.motor.motor_specs.DirectionState; @@ -9,7 +9,6 @@ * Implements the Motor interface to provide unified control over multiple motors. */ public class MotorGroup implements Motor { - private final Motor[] m_motors; /** @@ -174,7 +173,7 @@ public double getTemperature() { * Sets a soft limit for all motors in the group. * * @param directionState The direction state for the limit. - * @param limit The soft limit value. + * @param limit The soft limit value. */ @Override public void setSoftLimit(DirectionState directionState, float limit) { @@ -223,7 +222,7 @@ public void setVelocityConversionFactor(double conversionFactor) { * Sets the current limits for all motors in the group. * * @param stallLimit The stall current limit. - * @param freeLimit The free current limit. + * @param freeLimit The free current limit. */ @Override public void setCurrentLimit(int stallLimit, int freeLimit) { @@ -243,4 +242,16 @@ public void setMotorPosition(double position) { motor.setMotorPosition(position); } } + + protected void setDifferentialVoltage(double motorAvoltage, double motorBvoltage) { + // this check isn't necessary right now, but might be in the future + if (!(this instanceof DifferentialMotor)) { + System.out.println("setDifferentialVoltage only works with a DifferentialMotor"); + return; + } + + + m_motors[0].setVoltage(motorAvoltage); + m_motors[1].setVoltage(motorBvoltage); + } } \ No newline at end of file From 6e19fa9e11521de59ca895c7345147890c759ba9 Mon Sep 17 00:00:00 2001 From: TapChap Date: Wed, 14 May 2025 00:23:07 +0300 Subject: [PATCH 03/17] changed all mechanisms pid & FF controller to get their gains form the applyGains methods instead of manually --- src/main/java/frc/excalib/control/gains/Gains.java | 2 +- src/main/java/frc/excalib/mechanisms/arm/Arm.java | 2 +- src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java | 5 +++-- .../excalib/mechanisms/linear_extension/LinearExtension.java | 2 +- src/main/java/frc/excalib/mechanisms/turret/Turret.java | 5 +++-- 5 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/excalib/control/gains/Gains.java b/src/main/java/frc/excalib/control/gains/Gains.java index aff759e..8ba4136 100644 --- a/src/main/java/frc/excalib/control/gains/Gains.java +++ b/src/main/java/frc/excalib/control/gains/Gains.java @@ -127,7 +127,7 @@ public PIDController getPIDcontroller(){ } - public FF applyGains(FF ff){ + public GenericFF applyGains(GenericFF ff){ ff.setKv(this.kv); ff.setKs(this.ks); ff.setKa(this.ka); diff --git a/src/main/java/frc/excalib/mechanisms/arm/Arm.java b/src/main/java/frc/excalib/mechanisms/arm/Arm.java index d4efc43..bcb1006 100644 --- a/src/main/java/frc/excalib/mechanisms/arm/Arm.java +++ b/src/main/java/frc/excalib/mechanisms/arm/Arm.java @@ -31,7 +31,7 @@ public Arm(Motor motor, DoubleSupplier angleSupplier, m_kg = gains.kg; m_kv = gains.kv; m_ks = gains.ks; - m_PIDController = new PIDController(gains.kp, gains.ki, gains.kd); + m_PIDController = gains.getPIDcontroller(); m_mass = mass; } diff --git a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java b/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java index ae48f2e..fd9f3b5 100644 --- a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java +++ b/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.motor.Motor; import frc.excalib.mechanisms.Mechanism; @@ -35,8 +36,8 @@ public FlyWheel(Motor motor, double maxAcceleration, double maxJerk, Gains gains m_gains = gains; this.maxAcceleration = maxAcceleration; this.maxJerk = maxJerk; - this.m_pidController = new PIDController(m_gains.kp, m_gains.ki, m_gains.kd); - this.m_FF_CONTROLLER = new SimpleMotorFeedforward(gains.ks, gains.kv, gains.ka); + this.m_pidController = gains.getPIDcontroller(); + this.m_FF_CONTROLLER = gains.applyGains(new GenericFF.SimpleFF()); } /** diff --git a/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java b/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java index c9dcdcd..be7cb3e 100644 --- a/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java +++ b/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java @@ -26,7 +26,7 @@ public LinearExtension(Motor motor, DoubleSupplier positionSupplier, DoubleSuppl m_positionSupplier = positionSupplier; m_angleSupplier = angleSupplier; m_gains = gains; - m_PIDController = new PIDController(gains.kp, gains.ki, gains.kd); + m_PIDController = gains.getPIDcontroller(); m_constraints = constraints; m_tolerance = tolerance; } diff --git a/src/main/java/frc/excalib/mechanisms/turret/Turret.java b/src/main/java/frc/excalib/mechanisms/turret/Turret.java index 2d25c31..9a84fb5 100644 --- a/src/main/java/frc/excalib/mechanisms/turret/Turret.java +++ b/src/main/java/frc/excalib/mechanisms/turret/Turret.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.limits.ContinuousSoftLimit; import frc.excalib.control.motor.Motor; @@ -35,8 +36,8 @@ public Turret(Motor motor, ContinuousSoftLimit rotationLimit, Gains angleGains, super(motor); m_rotationLimit = rotationLimit; - m_anglePIDcontroller = new PIDController(angleGains.kp, angleGains.ki, angleGains.kd); - m_angleFFcontroller = new SimpleMotorFeedforward(angleGains.ks, angleGains.kv, angleGains.ka); + m_anglePIDcontroller = angleGains.getPIDcontroller(); + m_angleFFcontroller = angleGains.applyGains(new GenericFF.SimpleFF()); m_anglePIDcontroller.setTolerance(PIDtolerance); m_anglePIDcontroller.enableContinuousInput(-Math.PI, Math.PI); From 3dff1d8261f3fe650cd1b58b4693c82b1d3b1e04 Mon Sep 17 00:00:00 2001 From: TapChap Date: Wed, 14 May 2025 00:27:19 +0300 Subject: [PATCH 04/17] added DifferentialMotor class to allow independent control over two motors with one motor object, and provide util methods specifically for a Differential mechanism updated Differential mechanism to use the DiffMotor and added functionality to control the mechanism with and w/o advanced control --- .../motor/motorType/DifferentialMotor.java | 22 ++++ .../control/motor/motorType/MotorGroup.java | 13 +- .../frc/excalib/mechanisms/Mechanism.java | 9 ++ .../mechanisms/diffrential/Differential.java | 113 ++++++++++++++++++ .../mechanisms/diffrential/Diffrential.java | 26 ---- 5 files changed, 147 insertions(+), 36 deletions(-) create mode 100644 src/main/java/frc/excalib/mechanisms/diffrential/Differential.java delete mode 100644 src/main/java/frc/excalib/mechanisms/diffrential/Diffrential.java diff --git a/src/main/java/frc/excalib/control/motor/motorType/DifferentialMotor.java b/src/main/java/frc/excalib/control/motor/motorType/DifferentialMotor.java index 5cf7a07..efb398d 100644 --- a/src/main/java/frc/excalib/control/motor/motorType/DifferentialMotor.java +++ b/src/main/java/frc/excalib/control/motor/motorType/DifferentialMotor.java @@ -9,6 +9,12 @@ public class DifferentialMotor extends MotorGroup { private final Motor motorA, motorB; + /** + * + * @param motorA first motor in the differential mechanism + * @param motorB second motor in the differential mechanism + * @param negate make sure that applying the same voltage causes rotation and not translation. + */ public DifferentialMotor(Motor motorA, Motor motorB, boolean negate) { super(motorA, motorB); @@ -25,4 +31,20 @@ public void setDifferentialVoltage(double motorAvoltage, double motorBvoltage){ public Pair getMotorPositions(){ return Pair.of(motorA.getMotorPosition(), motorB.getMotorPosition()); } + + /** + * computes the avg of the two motors + * @return the rotation of the differential axle + */ + public double getMotorAvgPosition(){ + return (motorA.getMotorPosition() + motorB.getMotorPosition()) / 2.0; + } + + public double getMotorDifference(){ + return motorA.getMotorPosition() - motorB.getMotorPosition(); + } + + public double getMotorSum(){ + return motorA.getMotorPosition() + motorB.getMotorPosition(); + } } diff --git a/src/main/java/frc/excalib/control/motor/motorType/MotorGroup.java b/src/main/java/frc/excalib/control/motor/motorType/MotorGroup.java index 1ef7993..7af497c 100644 --- a/src/main/java/frc/excalib/control/motor/motorType/MotorGroup.java +++ b/src/main/java/frc/excalib/control/motor/motorType/MotorGroup.java @@ -243,15 +243,8 @@ public void setMotorPosition(double position) { } } - protected void setDifferentialVoltage(double motorAvoltage, double motorBvoltage) { - // this check isn't necessary right now, but might be in the future - if (!(this instanceof DifferentialMotor)) { - System.out.println("setDifferentialVoltage only works with a DifferentialMotor"); - return; - } - - - m_motors[0].setVoltage(motorAvoltage); - m_motors[1].setVoltage(motorBvoltage); + protected void setDifferentialVoltage(double voltageA, double voltageB) { + m_motors[0].setVoltage(voltageA); + m_motors[1].setVoltage(voltageB); } } \ No newline at end of file diff --git a/src/main/java/frc/excalib/mechanisms/Mechanism.java b/src/main/java/frc/excalib/mechanisms/Mechanism.java index b36cd9b..a0f5d26 100644 --- a/src/main/java/frc/excalib/mechanisms/Mechanism.java +++ b/src/main/java/frc/excalib/mechanisms/Mechanism.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.excalib.control.gains.SysidConfig; import frc.excalib.control.motor.Motor; +import frc.excalib.control.motor.motorType.DifferentialMotor; import frc.excalib.control.motor.motor_specs.IdleState; import monologue.Logged; @@ -55,6 +56,14 @@ public void setVoltage(double voltage) { m_motor.setVoltage(voltage); } + protected void setDifferentialVoltage(double voltageA, double voltageB){ + if (!(this.m_motor instanceof DifferentialMotor)) { + System.out.println("setDifferentialVoltage only works with a DifferentialMotor"); + return; + } + ((DifferentialMotor) this.m_motor).setDifferentialVoltage(voltageA, voltageB); + } + /** * @param outputSupplier the dynamic setpoint for the mechanism (voltage) * @return a command which controls the mechanism manually diff --git a/src/main/java/frc/excalib/mechanisms/diffrential/Differential.java b/src/main/java/frc/excalib/mechanisms/diffrential/Differential.java new file mode 100644 index 0000000..fc6e0c7 --- /dev/null +++ b/src/main/java/frc/excalib/mechanisms/diffrential/Differential.java @@ -0,0 +1,113 @@ +package frc.excalib.mechanisms.diffrential; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.GenericFF.GenericFF; +import frc.excalib.control.gains.Gains; +import frc.excalib.control.motor.motorType.DifferentialMotor; +import frc.excalib.control.motor.motor_specs.IdleState; +import frc.excalib.mechanisms.Mechanism; +import monologue.Annotations; +import monologue.Annotations.Log; + +import java.util.function.DoubleSupplier; + +public class Differential extends Mechanism { + private final Gains gains; + private final PIDController pidController_A, pidController_B; + private final ArmFeedforward ffController; + + private final DifferentialMotor motor; + + public final Trigger atSetpointTrigger; + + private final double differentialMul; + + private double deltaA, deltaB; + private double motorApos, motorBpos; + +// private DoubleSupplier angleSetpointSupplier, linearSetpointSupplier; +// DoubleSupplier angleSetpointSupplier, DoubleSupplier linearSetpointSupplier + + /** + * @param differentialMotor a differential motor object of both mechanism motors + * @param gains gains for pid & ff for the two motors + * @param differentialMul an optional multiplier to correct acc error in certain types of differential mechanism + * @param tolerance tolerance for the accumulated error from BOTH motors combined + */ + public Differential(DifferentialMotor differentialMotor, Gains gains, double differentialMul, double tolerance) { + super(differentialMotor); + + this.motor = differentialMotor; + this.motor.setIdleState(IdleState.BRAKE); + + this.gains = gains; + this.atSetpointTrigger = new Trigger(()-> (Math.abs(deltaA - motorApos) + Math.abs(deltaB - motorBpos)) < tolerance); + + this.pidController_A = this.gains.getPIDcontroller(); + this.pidController_B = this.gains.getPIDcontroller(); + this.ffController = this.gains.applyGains(new GenericFF.ArmFF()); + + this.differentialMul = differentialMul; + } + + /** + * constructor for a manual diff, w/o advanced control capabilities + * @param differentialMotor + */ + public Differential(DifferentialMotor differentialMotor) { + this(differentialMotor, new Gains(), 1, 20); + } + + public Command moveToState(double angle, double position, double ff, SubsystemBase... requirements){ + return Commands.startRun( + ()-> { + double deltaTheta = angle - getMechanismAngle(); + double deltaPos = position - getMechanismPosition(); + + deltaA = deltaTheta + (deltaPos / (2 * differentialMul)); + deltaB = deltaTheta - (deltaPos / (2 * differentialMul)); + }, + ()-> { + this.motorApos = this.motor.getMotorPositions().getFirst(); + this.motorBpos = this.motor.getMotorPositions().getSecond(); + + setDifferentialVoltage( + pidController_A.calculate(motorApos, this.deltaA) + ff, + pidController_B.calculate(motorBpos, this.deltaB) + ff + ); + }, + requirements); + } + + public Command setDifferentialVoltage(DoubleSupplier voltageA, DoubleSupplier voltageB, SubsystemBase... requirements){ + return new RunCommand( + ()-> setDifferentialVoltage(voltageA.getAsDouble(), voltageB.getAsDouble()), + requirements); + } + + public void move(double voltage) { + super.setDifferentialVoltage(voltage, -voltage); + } + + public void rotate(double voltage) { + super.setDifferentialVoltage(voltage, voltage); + } + + public void setDifferentialVoltage(double voltageA, double voltageB) { + super.setDifferentialVoltage(voltageA, voltageB); + } + + @Log.NT + public double getMechanismAngle() { + return this.motor.getMotorDifference(); + } + + @Log.NT + public double getMechanismPosition(){ + return this.differentialMul * this.motor.getMotorDifference(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/excalib/mechanisms/diffrential/Diffrential.java b/src/main/java/frc/excalib/mechanisms/diffrential/Diffrential.java deleted file mode 100644 index 9942f23..0000000 --- a/src/main/java/frc/excalib/mechanisms/diffrential/Diffrential.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.excalib.mechanisms.diffrential; - -import frc.excalib.control.motor.controllers.MotorGroup; -import frc.excalib.control.motor.motor_specs.IdleState; -import frc.excalib.mechanisms.Mechanism; - -import java.util.function.DoubleSupplier; - -public class Diffrential extends Mechanism { - private double setpoint; - public Diffrential(MotorGroup motorGroup, DoubleSupplier angleSetpointSupplier, DoubleSupplier angleSupplier, DoubleSupplier setpointSupplier) { - super(motorGroup); - motorGroup.setIdleState(IdleState.BRAKE); - } - - public void setVoltage(DoubleSupplier firstMotorVoltage, DoubleSupplier secondMotorVoltage) { - super.setVoltage((firstMotorVoltage.getAsDouble() + secondMotorVoltage.getAsDouble()) /2); - } - - public void setAngleSupplier(DoubleSupplier angleSupplier){ - - } - - - -} From 3dc23d0bd2941cbac55816ee3c46ecd24b72fa9c Mon Sep 17 00:00:00 2001 From: Yehuda Rothstein Date: Wed, 14 May 2025 18:53:07 +0300 Subject: [PATCH 05/17] Refactor FeedForwardGainsSetter to include setValue method for streamlined gain setting --- .../GenericFF/FeedForwardGainsSetter.java | 2 ++ .../excalib/control/GenericFF/GenericFF.java | 28 +++++++++++++++++++ .../java/frc/excalib/control/gains/Gains.java | 10 ++----- 3 files changed, 33 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java b/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java index 91ee861..f3e77cc 100644 --- a/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java +++ b/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java @@ -8,4 +8,6 @@ public interface FeedForwardGainsSetter { void setKv(double kV); void setKa(double kA); void setKg(double kG); + void setValue(double kS, double kV, double kA, double kG); + } \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/GenericFF/GenericFF.java b/src/main/java/frc/excalib/control/GenericFF/GenericFF.java index 8cc41b3..4244ad2 100644 --- a/src/main/java/frc/excalib/control/GenericFF/GenericFF.java +++ b/src/main/java/frc/excalib/control/GenericFF/GenericFF.java @@ -32,6 +32,14 @@ public void setKa(double kA) { public void setKg(double kG) { super.setKg(kG); } + + @Override + public void setValue(double kS, double kV, double kA, double kG) { + setKa(kA); + setKs(kS); + setKv(kV); + setKg(kG); + } } public static class SimpleFF extends SimpleMotorFeedforward implements FeedForwardGainsSetter { @@ -62,6 +70,17 @@ public void setKa(double kA) { public void setKg(double kG) { return; } + + @Override + public void setValue(double kS, double kV, double kA, double kG) { + if (kG != 0) { + throw new IllegalArgumentException("SimpleMotorFeedforward does not support gravity gain (kG), please make it zero!"); + } + setKa(kA); + setKs(kS); + setKv(kV); + setKg(kG); + } } public static class ElevatorFF extends ElevatorFeedforward implements FeedForwardGainsSetter { @@ -92,5 +111,14 @@ public void setKa(double kA) { public void setKg(double kG) { super.setKg(kG); } + + @Override + public void setValue(double kS, double kV, double kA, double kG) { + setKa(kA); + setKs(kS); + setKv(kV); + setKg(kG); + } + } } diff --git a/src/main/java/frc/excalib/control/gains/Gains.java b/src/main/java/frc/excalib/control/gains/Gains.java index 8ba4136..b716eb3 100644 --- a/src/main/java/frc/excalib/control/gains/Gains.java +++ b/src/main/java/frc/excalib/control/gains/Gains.java @@ -127,12 +127,8 @@ public PIDController getPIDcontroller(){ } - public GenericFF applyGains(GenericFF ff){ - ff.setKv(this.kv); - ff.setKs(this.ks); - ff.setKa(this.ka); - ff.setKg(this.kg); - - return ff; + public GenericFeedFoward applyGains(GenericFeedFoward feedFoward){ + feedFoward.setValue(this.ks, this.kv, this.ka, this.kg); + return feedFoward; } } From cf33b605d67198924684ac3a73e5bd1e46461aa6 Mon Sep 17 00:00:00 2001 From: yoav cohen Date: Sun, 18 May 2025 22:16:30 +0300 Subject: [PATCH 06/17] ADD: DifferentialGearBox class --- .../DifferentialGearBox.java | 225 ++++++++++++++++++ 1 file changed, 225 insertions(+) create mode 100644 src/main/java/frc/excalib/control/differential_gearbox/DifferentialGearBox.java diff --git a/src/main/java/frc/excalib/control/differential_gearbox/DifferentialGearBox.java b/src/main/java/frc/excalib/control/differential_gearbox/DifferentialGearBox.java new file mode 100644 index 0000000..18a9d36 --- /dev/null +++ b/src/main/java/frc/excalib/control/differential_gearbox/DifferentialGearBox.java @@ -0,0 +1,225 @@ +package frc.excalib.control.differential_gearbox; + +import frc.excalib.control.motor.Motor; +import frc.excalib.control.motor.motor_specs.DirectionState; +import frc.excalib.control.motor.motor_specs.IdleState; + +public class DifferentialGearBox { + private final Motor input1, input2; + private final Motor diffOutput, sumOutput; + + private double diffVoltage = 0, sumVoltage = 0; + + private double diffPositionOffset = 0, sumPositionOffset = 0; + private double diffPositionConversion = 1, diffVelocityConversion = 1; + private double sumPositionConversion = 1, sumVelocityConversion = 1; + + public DifferentialGearBox(Motor input1, Motor input2) { + this.input1 = input1; + this.input2 = input2; + + diffOutput = new Motor() { + @Override + public void stopMotor() { + setVoltage(0); + } + + @Override + public void setVoltage(double voltage) { + diffVoltage = voltage; + setVoltages(); + } + + @Override + public void setPercentage(double percentage) { + setVoltage(percentage * 12); + } + + @Override + public void setFollower(int mainMotorID) {} + + @Override + public void setIdleState(IdleState idleMode) { + input1.setIdleState(idleMode); + input2.setIdleState(idleMode); + } + + @Override + public IdleState getIdleState() { + return input1.getIdleState(); + } + + @Override + public int getDeviceID() { + return -1; + } + + @Override + public double getMotorPosition() { + return halfDiff(input1.getMotorPosition(), input2.getMotorPosition()) * diffPositionConversion + diffPositionOffset; + } + + @Override + public double getMotorVelocity() { + return halfDiff(input1.getMotorVelocity(), input2.getMotorVelocity()) * diffVelocityConversion; + } + + @Override + public double getCurrent() { + return halfDiff(input1.getCurrent(), input2.getCurrent()); + } + + @Override + public double getVoltage() { + return halfDiff(input1.getVoltage(), input2.getVoltage()); + } + + @Override + public double getTemperature() { + return -1; + } + + @Override + public void setSoftLimit(DirectionState directionState, float limit) {} + + @Override + public void setInverted(DirectionState mode) {} + + @Override + public void setPositionConversionFactor(double conversionFactor) { + diffPositionConversion = conversionFactor; + } + + @Override + public void setVelocityConversionFactor(double conversionFactor) { + diffVelocityConversion = conversionFactor; + } + + @Override + public void setCurrentLimit(int stallLimit, int freeLimit) {} + + @Override + public void setMotorPosition(double position) { + double raw = (position - diffPositionOffset) / diffPositionConversion; + double pos1 = input1.getMotorPosition(); + double pos2 = input2.getMotorPosition(); + double avg = (pos1 + pos2) / 2; + double diff = raw; + input1.setMotorPosition(avg + diff); + input2.setMotorPosition(avg - diff); + } + }; + + sumOutput = new Motor() { + @Override + public void stopMotor() { + setVoltage(0); + } + + @Override + public void setVoltage(double voltage) { + sumVoltage = voltage; + setVoltages(); + } + + @Override + public void setPercentage(double percentage) { + setVoltage(percentage * 12); + } + + @Override + public void setFollower(int mainMotorID) {} + + @Override + public void setIdleState(IdleState idleMode) { + input1.setIdleState(idleMode); + input2.setIdleState(idleMode); + } + + @Override + public IdleState getIdleState() { + return input1.getIdleState(); + } + + @Override + public int getDeviceID() { + return -1; + } + + @Override + public double getMotorPosition() { + return average(input1.getMotorPosition(), input2.getMotorPosition()) * sumPositionConversion + sumPositionOffset; + } + + @Override + public double getMotorVelocity() { + return average(input1.getMotorVelocity(), input2.getMotorVelocity()) * sumVelocityConversion; + } + + @Override + public double getCurrent() { + return average(input1.getCurrent(), input2.getCurrent()); + } + + @Override + public double getVoltage() { + return average(input1.getVoltage(), input2.getVoltage()); + } + + @Override + public double getTemperature() { + return -1; + } + + @Override + public void setSoftLimit(DirectionState directionState, float limit) {} + + @Override + public void setInverted(DirectionState mode) {} + + @Override + public void setPositionConversionFactor(double conversionFactor) { + sumPositionConversion = conversionFactor; + } + + @Override + public void setVelocityConversionFactor(double conversionFactor) { + sumVelocityConversion = conversionFactor; + } + + @Override + public void setCurrentLimit(int stallLimit, int freeLimit) {} + + @Override + public void setMotorPosition(double position) { + double raw = (position - sumPositionOffset) / sumPositionConversion; + double diff = input1.getMotorPosition() - input2.getMotorPosition(); + double pos1 = raw + diff / 2; + double pos2 = raw - diff / 2; + input1.setMotorPosition(pos1); + input2.setMotorPosition(pos2); + } + }; + } + + private static double average(double a, double b) { + return (a + b) / 2; + } + + private static double halfDiff(double a, double b) { + return (a - b) / 2; + } + + private void setVoltages() { + input1.setVoltage(sumVoltage - diffVoltage / 2); + input2.setVoltage(sumVoltage + diffVoltage / 2); + } + + public Motor getDiffOutput() { + return diffOutput; + } + + public Motor getSumOutput() { + return sumOutput; + } +} From a81d30b1b6004a7f8f7598afbaab4de2d5642cf1 Mon Sep 17 00:00:00 2001 From: TapChap Date: Tue, 20 May 2025 13:08:43 +0300 Subject: [PATCH 07/17] Excalib - shai's version --- .../excalib/additional_utilities/Color.java | 52 -- .../excalib/additional_utilities/Elastic.java | 390 -------------- .../excalib/additional_utilities/LEDs.java | 308 ----------- .../ContinuouslyConditionalCommand.java | 47 -- .../java/frc/excalib/commands/MapCommand.java | 74 --- .../GenericFF/FeedForwardGainsSetter.java | 10 +- .../excalib/control/GenericFF/GenericFF.java | 76 +-- .../DifferentialGearBox.java | 225 -------- .../control/encoder/DutyCycleEncoder.java | 35 ++ .../frc/excalib/control/encoder/Encoder.java | 16 + .../java/frc/excalib/control/gains/Gains.java | 2 +- .../java/frc/excalib/control/math/Circle.java | 102 ---- .../java/frc/excalib/control/math/Line.java | 52 -- .../frc/excalib/control/math/MathUtils.java | 78 --- .../frc/excalib/control/math/Vector2D.java | 164 ------ .../excalib/control/math/physics/Mass.java | 51 -- .../frc/excalib/control/motor/Piston.java | 27 + .../frc/excalib/mechanisms/Mechanism.java | 34 +- .../java/frc/excalib/mechanisms/arm/Arm.java | 68 +-- .../mechanisms/diffrential/Differential.java | 19 +- .../mechanisms/fly_wheel/FlyWheel.java | 74 +-- .../linear_extension/LinearExtension.java | 75 +-- .../frc/excalib/mechanisms/turret/Turret.java | 78 ++- .../frc/excalib/slam/mapper/AuroraClient.java | 110 ---- .../frc/excalib/slam/mapper/Odometry.java | 39 -- .../slam/mapper/PhotonAprilTagsCamera.java | 91 ---- .../excalib/slam/mapper/PoseEstimator.java | 21 - .../frc/excalib/swerve/ModulesHolder.java | 222 -------- src/main/java/frc/excalib/swerve/Swerve.java | 501 ------------------ .../frc/excalib/swerve/SwerveAccUtils.java | 79 --- .../java/frc/excalib/swerve/SwerveModule.java | 231 -------- 31 files changed, 240 insertions(+), 3111 deletions(-) delete mode 100644 src/main/java/frc/excalib/additional_utilities/Color.java delete mode 100644 src/main/java/frc/excalib/additional_utilities/Elastic.java delete mode 100644 src/main/java/frc/excalib/additional_utilities/LEDs.java delete mode 100644 src/main/java/frc/excalib/commands/ContinuouslyConditionalCommand.java delete mode 100644 src/main/java/frc/excalib/commands/MapCommand.java delete mode 100644 src/main/java/frc/excalib/control/differential_gearbox/DifferentialGearBox.java create mode 100644 src/main/java/frc/excalib/control/encoder/DutyCycleEncoder.java create mode 100644 src/main/java/frc/excalib/control/encoder/Encoder.java delete mode 100644 src/main/java/frc/excalib/control/math/Circle.java delete mode 100644 src/main/java/frc/excalib/control/math/Line.java delete mode 100644 src/main/java/frc/excalib/control/math/MathUtils.java delete mode 100644 src/main/java/frc/excalib/control/math/Vector2D.java delete mode 100644 src/main/java/frc/excalib/control/math/physics/Mass.java create mode 100644 src/main/java/frc/excalib/control/motor/Piston.java delete mode 100644 src/main/java/frc/excalib/slam/mapper/AuroraClient.java delete mode 100644 src/main/java/frc/excalib/slam/mapper/Odometry.java delete mode 100644 src/main/java/frc/excalib/slam/mapper/PhotonAprilTagsCamera.java delete mode 100644 src/main/java/frc/excalib/slam/mapper/PoseEstimator.java delete mode 100644 src/main/java/frc/excalib/swerve/ModulesHolder.java delete mode 100644 src/main/java/frc/excalib/swerve/Swerve.java delete mode 100644 src/main/java/frc/excalib/swerve/SwerveAccUtils.java delete mode 100644 src/main/java/frc/excalib/swerve/SwerveModule.java diff --git a/src/main/java/frc/excalib/additional_utilities/Color.java b/src/main/java/frc/excalib/additional_utilities/Color.java deleted file mode 100644 index 74ced9b..0000000 --- a/src/main/java/frc/excalib/additional_utilities/Color.java +++ /dev/null @@ -1,52 +0,0 @@ -package frc.excalib.additional_utilities; - -/** - * This Class represents a single RGB color for led usage - */ -public class Color extends edu.wpi.first.wpilibj.util.Color { - public Color(double green, double red, double blue) { - super(green, red, blue); - } - - public Color(int green, int red, int blue) { - super(red, green, blue); - } - - public Color(){ - super(0, 0, 0); - } - - public static Color balance(Color color) { - double v = Math.max(Math.max(color.red, color.green), color.blue); - Color newColor = new Color(color.red, color.green / 2, color.blue / 4); - double newV = Math.max(Math.max(newColor.red, newColor.green), newColor.blue); - double ratio = v / newV; - return new Color(newColor.red * ratio, newColor.green * ratio, newColor.blue * ratio); - } - - public enum Colors { - OFF(new Color(0, 0, 0)), - - TEAM_GOLD(new Color(255, 170, 0)), - TEAM_BLUE(new Color(1, 30, 202)), - - RED(new Color(0, 255, 0)), - GREEN(new Color(0, 255, 0)), - BLUE(new Color(0, 0, 255)), - - WHITE(new Color(255, 255, 255)), - - YELLOW(new Color(255, 255, 0)), - CYAN(new Color(0, 255, 255)), - PINK(new Color(255, 0, 255)), - - ORANGE(new Color(255, 50, 0)), - PURPLE(new Color(75, 0, 130)); - - public final Color color; - - Colors(Color color) { - this.color = color; - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/additional_utilities/Elastic.java b/src/main/java/frc/excalib/additional_utilities/Elastic.java deleted file mode 100644 index 24a796c..0000000 --- a/src/main/java/frc/excalib/additional_utilities/Elastic.java +++ /dev/null @@ -1,390 +0,0 @@ -// Copyright (c) 2023-2025 Gold87 and other Elastic contributors -// This software can be modified and/or shared under the terms -// defined by the Elastic license: -// https://github.com/Gold872/elastic-dashboard/blob/main/LICENSE - -package frc.excalib.additional_utilities; - -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.PubSubOption; -import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.networktables.StringTopic; - -public final class Elastic { - private static final StringTopic notificationTopic = - NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications"); - private static final StringPublisher notificationPublisher = - notificationTopic.publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true)); - private static final StringTopic selectedTabTopic = - NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab"); - private static final StringPublisher selectedTabPublisher = - selectedTabTopic.publish(PubSubOption.keepDuplicates(true)); - private static final ObjectMapper objectMapper = new ObjectMapper(); - - /** - * Sends an notification to the Elastic dashboard. The notification is serialized as a JSON string - * before being published. - * - * @param notification the {@link Notification} object containing notification details - */ - public static void sendNotification(Notification notification) { - try { - notificationPublisher.set(objectMapper.writeValueAsString(notification)); - } catch (JsonProcessingException e) { - e.printStackTrace(); - } - } - - /** - * Selects the tab of the dashboard with the given name. If no tab matches the name, this will - * have no effect on the widgets or tabs in view. - * - *

If the given name is a number, Elastic will select the tab whose index equals the number - * provided. - * - * @param tabName the name of the tab to select - */ - public static void selectTab(String tabName) { - selectedTabPublisher.set(tabName); - } - - /** - * Selects the tab of the dashboard at the given index. If this index is greater than or equal to - * the number of tabs, this will have no effect. - * - * @param tabIndex the index of the tab to select. - */ - public static void selectTab(int tabIndex) { - selectTab(Integer.toString(tabIndex)); - } - - /** - * Represents an notification object to be sent to the Elastic dashboard. This object holds - * properties such as level, title, description, display time, and dimensions to control how the - * notification is displayed on the dashboard. - */ - public static class Notification { - @JsonProperty("level") - private NotificationLevel level; - - @JsonProperty("title") - private String title; - - @JsonProperty("description") - private String description; - - @JsonProperty("displayTime") - private int displayTimeMillis; - - @JsonProperty("width") - private double width; - - @JsonProperty("height") - private double height; - - /** - * Creates a new Notification with all default parameters. This constructor is intended to be - * used with the chainable decorator methods - * - *

Title and description fields are empty. - */ - public Notification() { - this(NotificationLevel.INFO, "", ""); - } - - /** - * Creates a new Notification with all properties specified. - * - * @param level the level of the notification (e.g., INFO, WARNING, ERROR) - * @param title the title text of the notification - * @param description the descriptive text of the notification - * @param displayTimeMillis the time in milliseconds for which the notification is displayed - * @param width the width of the notification display area - * @param height the height of the notification display area, inferred if below zero - */ - public Notification( - NotificationLevel level, - String title, - String description, - int displayTimeMillis, - double width, - double height) { - this.level = level; - this.title = title; - this.displayTimeMillis = displayTimeMillis; - this.description = description; - this.height = height; - this.width = width; - } - - /** - * Creates a new Notification with default display time and dimensions. - * - * @param level the level of the notification - * @param title the title text of the notification - * @param description the descriptive text of the notification - */ - public Notification(NotificationLevel level, String title, String description) { - this(level, title, description, 3000, 350, -1); - } - - /** - * Creates a new Notification with a specified display time and default dimensions. - * - * @param level the level of the notification - * @param title the title text of the notification - * @param description the descriptive text of the notification - * @param displayTimeMillis the display time in milliseconds - */ - public Notification( - NotificationLevel level, String title, String description, int displayTimeMillis) { - this(level, title, description, displayTimeMillis, 350, -1); - } - - /** - * Creates a new Notification with specified dimensions and default display time. If the height - * is below zero, it is automatically inferred based on screen size. - * - * @param level the level of the notification - * @param title the title text of the notification - * @param description the descriptive text of the notification - * @param width the width of the notification display area - * @param height the height of the notification display area, inferred if below zero - */ - public Notification( - NotificationLevel level, String title, String description, double width, double height) { - this(level, title, description, 3000, width, height); - } - - /** - * Updates the level of this notification - * - * @param level the level to set the notification to - */ - public void setLevel(NotificationLevel level) { - this.level = level; - } - - /** - * @return the level of this notification - */ - public NotificationLevel getLevel() { - return level; - } - - /** - * Updates the title of this notification - * - * @param title the title to set the notification to - */ - public void setTitle(String title) { - this.title = title; - } - - /** - * Gets the title of this notification - * - * @return the title of this notification - */ - public String getTitle() { - return title; - } - - /** - * Updates the description of this notification - * - * @param description the description to set the notification to - */ - public void setDescription(String description) { - this.description = description; - } - - public String getDescription() { - return description; - } - - /** - * Updates the display time of the notification - * - * @param seconds the number of seconds to display the notification for - */ - public void setDisplayTimeSeconds(double seconds) { - setDisplayTimeMillis((int) Math.round(seconds * 1000)); - } - - /** - * Updates the display time of the notification in milliseconds - * - * @param displayTimeMillis the number of milliseconds to display the notification for - */ - public void setDisplayTimeMillis(int displayTimeMillis) { - this.displayTimeMillis = displayTimeMillis; - } - - /** - * Gets the display time of the notification in milliseconds - * - * @return the number of milliseconds the notification is displayed for - */ - public int getDisplayTimeMillis() { - return displayTimeMillis; - } - - /** - * Updates the width of the notification - * - * @param width the width to set the notification to - */ - public void setWidth(double width) { - this.width = width; - } - - /** - * Gets the width of the notification - * - * @return the width of the notification - */ - public double getWidth() { - return width; - } - - /** - * Updates the height of the notification - * - *

If the height is set to -1, the height will be determined automatically by the dashboard - * - * @param height the height to set the notification to - */ - public void setHeight(double height) { - this.height = height; - } - - /** - * Gets the height of the notification - * - * @return the height of the notification - */ - public double getHeight() { - return height; - } - - /** - * Modifies the notification's level and returns itself to allow for method chaining - * - * @param level the level to set the notification to - * @return the current notification - */ - public Notification withLevel(NotificationLevel level) { - this.level = level; - return this; - } - - /** - * Modifies the notification's title and returns itself to allow for method chaining - * - * @param title the title to set the notification to - * @return the current notification - */ - public Notification withTitle(String title) { - setTitle(title); - return this; - } - - /** - * Modifies the notification's description and returns itself to allow for method chaining - * - * @param description the description to set the notification to - * @return the current notification - */ - public Notification withDescription(String description) { - setDescription(description); - return this; - } - - /** - * Modifies the notification's display time and returns itself to allow for method chaining - * - * @param seconds the number of seconds to display the notification for - * @return the current notification - */ - public Notification withDisplaySeconds(double seconds) { - return withDisplayMilliseconds((int) Math.round(seconds * 1000)); - } - - /** - * Modifies the notification's display time and returns itself to allow for method chaining - * - * @param displayTimeMillis the number of milliseconds to display the notification for - * @return the current notification - */ - public Notification withDisplayMilliseconds(int displayTimeMillis) { - setDisplayTimeMillis(displayTimeMillis); - return this; - } - - /** - * Modifies the notification's width and returns itself to allow for method chaining - * - * @param width the width to set the notification to - * @return the current notification - */ - public Notification withWidth(double width) { - setWidth(width); - return this; - } - - /** - * Modifies the notification's height and returns itself to allow for method chaining - * - * @param height the height to set the notification to - * @return the current notification - */ - public Notification withHeight(double height) { - setHeight(height); - return this; - } - - /** - * Modifies the notification's height and returns itself to allow for method chaining - * - *

This will set the height to -1 to have it automatically determined by the dashboard - * - * @return the current notification - */ - public Notification withAutomaticHeight() { - setHeight(-1); - return this; - } - - /** - * Modifies the notification to disable the auto dismiss behavior - * - *

This sets the display time to 0 milliseconds - * - *

The auto dismiss behavior can be re-enabled by setting the display time to a number - * greater than 0 - * - * @return the current notification - */ - public Notification withNoAutoDismiss() { - setDisplayTimeMillis(0); - return this; - } - - /** - * Represents the possible levels of notifications for the Elastic dashboard. These levels are - * used to indicate the severity or type of notification. - */ - public enum NotificationLevel { - /** Informational Message */ - INFO, - /** Warning message */ - WARNING, - /** Error message */ - ERROR - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/additional_utilities/LEDs.java b/src/main/java/frc/excalib/additional_utilities/LEDs.java deleted file mode 100644 index 34d6284..0000000 --- a/src/main/java/frc/excalib/additional_utilities/LEDs.java +++ /dev/null @@ -1,308 +0,0 @@ -package frc.excalib.additional_utilities; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.*; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -import java.util.Arrays; -import java.util.Random; -import java.util.concurrent.atomic.AtomicBoolean; -import java.util.concurrent.atomic.AtomicInteger; -import java.util.concurrent.atomic.AtomicReference; - -import static frc.excalib.additional_utilities.Color.Colors.*; - -/** - * The LEDs class is responsible for controlling an addressable LED strip. - * It provides various patterns and commands to manipulate the LED strip's colors and animations. - */ -public class LEDs extends SubsystemBase { - private final AddressableLED LedStrip = new AddressableLED(LEDS_PORT); // LED strip object - private final AddressableLEDBuffer buffer = new AddressableLEDBuffer(LENGTH); // Buffer to hold LED colors - - private static LEDs instance = null; // Singleton instance of the LEDs class - private final Random rnd = new Random(); // Random generator for random patterns - - private int tailIndex = 0; // Index for train patterns - private double offset = 0; // Offset for animations - - // PWM port for the LED strip - public static final int LEDS_PORT = 9; - // The length of the LED strip - public static final int LENGTH = 50; - - // Predefined color arrays - Color[] orange = new Color[LENGTH]; - Color[] black = new Color[LENGTH]; - - /** - * Private constructor to initialize the LED strip and set the default command. - */ - private LEDs() { - LedStrip.setLength(LENGTH); - LedStrip.start(); - - Arrays.fill(orange, ORANGE.color); - Arrays.fill(black, OFF.color); - - setDefaultCommand(setPattern(LEDPattern.EXPAND, BLUE.color, WHITE.color)); - } - - /** - * Returns the singleton instance of the LEDs class. - * - * @return The LEDs instance. - */ - public static LEDs getInstance() { - if (instance == null) { - instance = new LEDs(); - } - return instance; - } - - /** - * Creates a command to set a specific LED pattern with main and accent colors. - * - * @param pattern The LED pattern to set. - * @param mainColor The main color for the pattern. - * @param accentColor The accent color for the pattern. - * @return A command to execute the pattern. - */ - public Command setPattern(LEDPattern pattern, Color mainColor, Color accentColor) { - Command command = new InstantCommand(); // Default command - Color[] colors = new Color[LENGTH]; // Array to hold LED colors - int trainLength = (int) (LENGTH / 5.0); // Length of the train pattern - final AtomicBoolean invert = new AtomicBoolean(false); // Direction for train patterns - - switch (pattern) { - case OFF: - // Turns off all LEDs - Arrays.fill(colors, OFF.color); - command = new RunCommand(() -> setLedStrip(colors), this).withName("OFF"); - break; - - case SOLID: - // Sets all LEDs to the main color - Arrays.fill(colors, mainColor); - command = new RunCommand(() -> setLedStrip(colors), this).withName("SOLID: " + mainColor.toString()); - break; - - case ALTERNATING_STATIC: - // Creates a static alternating pattern of main and accent colors - for (int i = 0; i < LENGTH; i++) { - colors[i] = mainColor; - colors[i + 1] = accentColor; - i++; - } - command = new RunCommand(() -> setLedStrip(colors), this) - .withName("ALTERNATING, main: " + mainColor.toString() + ", accent: " + accentColor.toString()); - break; - - case ALTERNATING_MOVING: - // Creates a moving alternating pattern of main and accent colors - AtomicReference mainAlternatingColor = new AtomicReference<>(mainColor); - AtomicReference accentAlternatingColor = new AtomicReference<>(accentColor); - AtomicReference tempAlternatingColor = new AtomicReference<>(mainColor); - - command = this.runOnce(() -> { - for (int i = 0; i < LENGTH - 1; i++) { - colors[i] = mainAlternatingColor.get(); - colors[i + 1] = accentAlternatingColor.get(); - i++; - } - setLedStrip(colors); - - tempAlternatingColor.set(mainAlternatingColor.get()); - mainAlternatingColor.set(accentAlternatingColor.get()); - accentAlternatingColor.set(tempAlternatingColor.get()); - }) - .andThen(new WaitCommand(0.25)).repeatedly() - .withName("ALTERNATING_MOVING, main: " + mainColor.toString() + ", accent: " + accentColor.toString()); - break; - - case RANDOM: - // Sets LEDs to random colors - command = this.runOnce(() -> { - Arrays.fill(colors, new Color(rnd.nextInt(255), rnd.nextInt(255), rnd.nextInt(255))); - setLedStrip(colors); - }).andThen(new WaitCommand(1)) - .repeatedly().withName("RANDOM"); - break; - - case BLINKING: - // Creates a blinking pattern between main and accent colors - command = Commands.repeatingSequence( - new InstantCommand(() -> { - Arrays.fill(colors, mainColor); - setLedStrip(colors); - }, this), - new WaitCommand(0.15), - new InstantCommand(() -> { - Arrays.fill(colors, accentColor); - setLedStrip(colors); - }, this), - new WaitCommand(0.15) - ).withName("BLINKING, main: " + mainColor.toString() + ", accent: " + accentColor.toString()); - break; - - case EXPAND: - // Creates an expanding pattern from the center - final AtomicInteger expandStep = new AtomicInteger(0); - command = new InstantCommand(() -> { - Color[] LedColors = new Color[LENGTH]; - Arrays.fill(LedColors, mainColor); - int LEDS_LENGTH = 5; - - int step = expandStep.get(); - - if (LENGTH % 2 == 0) { - int leftCenter = (LENGTH / 2) - 1; - int rightCenter = LENGTH / 2; - int leftIndex = leftCenter - step; - int rightIndex = rightCenter + step; - - for (int i = 0; i < LEDS_LENGTH; i++) { // Light up 3 LEDs per side - if (leftIndex - i >= 0) { - LedColors[leftIndex - i] = accentColor; - } - if (rightIndex + i < LENGTH) { - LedColors[rightIndex + i] = accentColor; - } - } - } else { - int center = LENGTH / 2; - int leftIndex = center - step; - int rightIndex = center + step; - - for (int i = 0; i < LEDS_LENGTH; i++) { // Light up 3 LEDs per side - if (leftIndex - i >= 0) { - LedColors[leftIndex - i] = accentColor; - } - if (rightIndex + i < LENGTH) { - LedColors[rightIndex + i] = accentColor; - } - } - } - - int newStep = expandStep.get() + 1; - if (newStep > (LENGTH / 2)) { - newStep = 0; - } - expandStep.set(newStep); - - setLedStrip(LedColors); - }, this) - .andThen(new WaitCommand(0.01)) - .repeatedly() - .withName("EXPAND, main: " + mainColor.toString() + ", accent: " + accentColor.toString()); - break; - - // Additional cases omitted for brevity - default: - break; - } - - return command.ignoringDisable(true); - } - - public Command setPattern(LEDPattern pattern, Color color) { - return setPattern(pattern, color, OFF.color); - } - - - public Command setLEDsCommand(Color[] colors) { - return this.runOnce(() -> setLedStrip(colors)).ignoringDisable(true); - } - - public Command twoStatesCommand(LEDPattern firstPattern, Color firstColor, LEDPattern secondPattern, Color secondColor, Trigger switchStates) { - return setPattern(firstPattern, firstColor).until(switchStates).andThen(setPattern(secondPattern, secondColor)); - } - - public Command deadLineLEDcommand(Command ledCommand) { - return new StartEndCommand( - ledCommand::schedule, - restoreLEDs()::schedule); - } - - public Command scheduleLEDcommand(Command ledCommand) { - return new InstantCommand(ledCommand::schedule); - } - - public Command circleLEDs(Color[] colors) { - return Commands.repeatingSequence( - setLEDsCommand(colors), - new WaitCommand(0.1), - new InstantCommand(() -> shiftLeds(colors))) - .ignoringDisable(true); - } - - public Command restoreLEDs() { - return new InstantCommand(() -> - CommandScheduler.getInstance().cancel(CommandScheduler.getInstance().requiring(this))); - } - - public enum LEDPattern { - OFF, - RAINBOW, - SOLID, - ALTERNATING_STATIC, - ALTERNATING_MOVING, - TRAIN_CIRCLE, - TRAIN, - RANDOM, - EXPAND, - BLINKING, - RSL; - } - - public Color getAllianceColor() { - if (DriverStation.getAlliance().equals(DriverStation.Alliance.Blue)) return Color.Colors.TEAM_BLUE.color; - else if (DriverStation.getAlliance().equals(DriverStation.Alliance.Red)) return Color.Colors.RED.color; - else return Color.Colors.WHITE.color; - } - - private void setLedStrip(Color[] colors) { - for (int i = 0; i < colors.length; i++) buffer.setLED(i, colors[i]); - LedStrip.setData(buffer); - } - - private void shiftTrain(Color[] colors, Color mainColor, Color trainColor, int trainLength, int offset) { - tailIndex = findTailIndex(colors, trainColor); - Arrays.fill(colors, mainColor); - for (int i = 0; i < trainLength; i++) { - colors[stayInBounds(i + tailIndex + offset, colors.length)] = trainColor; - } - } - - private int findHeadIndex(Color[] colors, Color trainColor) { - for (int i = colors.length - 1; i >= 0; i--) { - if (colors[i].equals(trainColor)) return i; - } - return -1; - } - - private int findTailIndex(Color[] colors, Color trainColor) { - for (int i = 0; i < colors.length; i++) { - if (colors[i].equals(trainColor)) return i; - } - return -1; - } - - private void shiftLeds(Color[] colors) { - Color lastColor = colors[colors.length - 1]; - - for (int i = colors.length - 2; i >= 0; i--) { - colors[i + 1] = colors[i]; - } - - colors[0] = lastColor; - } - - private int stayInBounds(int value, int length) { - if (value >= length) return stayInBounds(value - length, length); - if (value < 0) return stayInBounds(value + length, length); - return value; - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/commands/ContinuouslyConditionalCommand.java b/src/main/java/frc/excalib/commands/ContinuouslyConditionalCommand.java deleted file mode 100644 index a6855ba..0000000 --- a/src/main/java/frc/excalib/commands/ContinuouslyConditionalCommand.java +++ /dev/null @@ -1,47 +0,0 @@ -package frc.excalib.commands; - -import edu.wpi.first.util.ErrorMessages; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; - -import java.util.function.BooleanSupplier; - -/** - * - */ -public class ContinuouslyConditionalCommand extends Command { - private final Command m_onTrue; - private final Command m_onFalse; - private final BooleanSupplier m_condition; - private boolean negated; - - public ContinuouslyConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condition) { - this.m_onTrue = ErrorMessages.requireNonNullParam(onTrue, "onTrue", "ConditionalCommand"); - this.m_onFalse = ErrorMessages.requireNonNullParam(onFalse, "onFalse", "ConditionalCommand"); - this.m_condition = ErrorMessages.requireNonNullParam(condition, "condition", "ConditionalCommand"); - CommandScheduler.getInstance().registerComposedCommands(onTrue, onFalse); - negated = condition.getAsBoolean(); - } - - private Command getCurrentCommand(){ - if (m_condition.getAsBoolean()) return m_onTrue; - return m_onFalse; - } - - private Command getOtherCommand(){ - if (m_condition.getAsBoolean()) return m_onFalse; - return m_onTrue; - } - - public void initialize() { - getCurrentCommand().schedule(); - } - - public void execute() { - if (negated != m_condition.getAsBoolean()) { - getOtherCommand().cancel(); - getCurrentCommand().schedule(); - negated = !negated; - } - } -} diff --git a/src/main/java/frc/excalib/commands/MapCommand.java b/src/main/java/frc/excalib/commands/MapCommand.java deleted file mode 100644 index d7dc995..0000000 --- a/src/main/java/frc/excalib/commands/MapCommand.java +++ /dev/null @@ -1,74 +0,0 @@ -package frc.excalib.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; - -import java.util.Map; -import java.util.function.Supplier; - -/** - * A command that maps a key of type T to a corresponding command. - * This allows dynamic selection and execution of commands based on a supplied key. - * - * @param The type of the key used to map to commands. - */ -public class MapCommand extends Command { - private final Map m_map; // A map that associates keys of type T with commands. - private Command m_command; // The currently selected command to execute. - private Supplier m_t; // A supplier that provides the key to select the command. - - /** - * Constructs a new MapCommand. - * - * @param map A map associating keys of type T with commands. - * @param t A supplier that provides the key to select the command. - */ - public MapCommand(Map map, Supplier t) { - m_map = map; - m_t = t; - // Add the requirements of all commands in the map to this command. - map.forEach((key, value) -> this.addRequirements(value.getRequirements())); - } - - /** - * Initializes the command by selecting the appropriate command from the map - * based on the key provided by the supplier. - * If no command is found for the key, a default "none" command is used. - */ - @Override - public void initialize() { - m_command = m_map.get(m_t.get()); - if (m_command == null) { - m_command = Commands.none(); // Use a default "none" command if no match is found. - } - m_command.initialize(); // Initialize the selected command. - } - - /** - * Executes the currently selected command. - */ - @Override - public void execute() { - m_command.execute(); - } - - /** - * Ends the currently selected command. - * - * @param inturupted Whether the command was interrupted. - */ - @Override - public void end(boolean inturupted) { - m_command.end(inturupted); - } - - /** - * Checks if the currently selected command has finished execution. - * - * @return True if the selected command is finished, false otherwise. - */ - @Override - public boolean isFinished() { - return this.m_command.isFinished(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java b/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java index f3e77cc..f9c208f 100644 --- a/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java +++ b/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java @@ -4,10 +4,8 @@ * Interface for feedforward controllers that have kS, kV, and kA gains. */ public interface FeedForwardGainsSetter { - void setKs(double kS); - void setKv(double kV); - void setKa(double kA); - void setKg(double kG); - void setValue(double kS, double kV, double kA, double kG); - + void setKs(double Ks); + void setKv(double Kv); + void setKa(double Ka); + void setKg(double Kg); } \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/GenericFF/GenericFF.java b/src/main/java/frc/excalib/control/GenericFF/GenericFF.java index 4244ad2..be87a8b 100644 --- a/src/main/java/frc/excalib/control/GenericFF/GenericFF.java +++ b/src/main/java/frc/excalib/control/GenericFF/GenericFF.java @@ -11,34 +11,26 @@ public ArmFF() { } @Override - public void setKs(double kS) { - super.setKs(kS); + public void setKs(double Ks) { + super.setKs(Ks); } @Override - public void setKv(double kV) { - super.setKv(kV); + public void setKv(double Kv) { + super.setKv(Kv); } @Override - public void setKa(double kA) { - super.setKa(kA); + public void setKa(double Ka) { + super.setKa(Ka); } @Override - public void setKg(double kG) { - super.setKg(kG); - } - - @Override - public void setValue(double kS, double kV, double kA, double kG) { - setKa(kA); - setKs(kS); - setKv(kV); - setKg(kG); + public void setKg(double Kg) { + super.setKg(Kg); } } @@ -49,37 +41,28 @@ public SimpleFF() { @Override - public void setKs(double kS) { - super.setKs(kS); + public void setKs(double Ks) { + super.setKs(Ks); } @Override - public void setKv(double kV) { - super.setKv(kV); + public void setKv(double Kv) { + super.setKv(Kv); } @Override - public void setKa(double kA) { - super.setKa(kA); + public void setKa(double Ka) { + super.setKa(Ka); } @Override - public void setKg(double kG) { - return; - } + public void setKg(double Kg) { + if (Kg != 0) throw new IllegalArgumentException("SimpleMotorFeedforward does not support gravity gain (Kg), please make it zero!"); - @Override - public void setValue(double kS, double kV, double kA, double kG) { - if (kG != 0) { - throw new IllegalArgumentException("SimpleMotorFeedforward does not support gravity gain (kG), please make it zero!"); - } - setKa(kA); - setKs(kS); - setKv(kV); - setKg(kG); + return; } } @@ -90,35 +73,26 @@ public ElevatorFF() { @Override - public void setKs(double kS) { - super.setKs(kS); + public void setKs(double Ks) { + super.setKs(Ks); } @Override - public void setKv(double kV) { - super.setKv(kV); + public void setKv(double Kv) { + super.setKv(Kv); } @Override - public void setKa(double kA) { - super.setKa(kA); + public void setKa(double Ka) { + super.setKa(Ka); } @Override - public void setKg(double kG) { - super.setKg(kG); + public void setKg(double Kg) { + super.setKg(Kg); } - - @Override - public void setValue(double kS, double kV, double kA, double kG) { - setKa(kA); - setKs(kS); - setKv(kV); - setKg(kG); - } - } } diff --git a/src/main/java/frc/excalib/control/differential_gearbox/DifferentialGearBox.java b/src/main/java/frc/excalib/control/differential_gearbox/DifferentialGearBox.java deleted file mode 100644 index 18a9d36..0000000 --- a/src/main/java/frc/excalib/control/differential_gearbox/DifferentialGearBox.java +++ /dev/null @@ -1,225 +0,0 @@ -package frc.excalib.control.differential_gearbox; - -import frc.excalib.control.motor.Motor; -import frc.excalib.control.motor.motor_specs.DirectionState; -import frc.excalib.control.motor.motor_specs.IdleState; - -public class DifferentialGearBox { - private final Motor input1, input2; - private final Motor diffOutput, sumOutput; - - private double diffVoltage = 0, sumVoltage = 0; - - private double diffPositionOffset = 0, sumPositionOffset = 0; - private double diffPositionConversion = 1, diffVelocityConversion = 1; - private double sumPositionConversion = 1, sumVelocityConversion = 1; - - public DifferentialGearBox(Motor input1, Motor input2) { - this.input1 = input1; - this.input2 = input2; - - diffOutput = new Motor() { - @Override - public void stopMotor() { - setVoltage(0); - } - - @Override - public void setVoltage(double voltage) { - diffVoltage = voltage; - setVoltages(); - } - - @Override - public void setPercentage(double percentage) { - setVoltage(percentage * 12); - } - - @Override - public void setFollower(int mainMotorID) {} - - @Override - public void setIdleState(IdleState idleMode) { - input1.setIdleState(idleMode); - input2.setIdleState(idleMode); - } - - @Override - public IdleState getIdleState() { - return input1.getIdleState(); - } - - @Override - public int getDeviceID() { - return -1; - } - - @Override - public double getMotorPosition() { - return halfDiff(input1.getMotorPosition(), input2.getMotorPosition()) * diffPositionConversion + diffPositionOffset; - } - - @Override - public double getMotorVelocity() { - return halfDiff(input1.getMotorVelocity(), input2.getMotorVelocity()) * diffVelocityConversion; - } - - @Override - public double getCurrent() { - return halfDiff(input1.getCurrent(), input2.getCurrent()); - } - - @Override - public double getVoltage() { - return halfDiff(input1.getVoltage(), input2.getVoltage()); - } - - @Override - public double getTemperature() { - return -1; - } - - @Override - public void setSoftLimit(DirectionState directionState, float limit) {} - - @Override - public void setInverted(DirectionState mode) {} - - @Override - public void setPositionConversionFactor(double conversionFactor) { - diffPositionConversion = conversionFactor; - } - - @Override - public void setVelocityConversionFactor(double conversionFactor) { - diffVelocityConversion = conversionFactor; - } - - @Override - public void setCurrentLimit(int stallLimit, int freeLimit) {} - - @Override - public void setMotorPosition(double position) { - double raw = (position - diffPositionOffset) / diffPositionConversion; - double pos1 = input1.getMotorPosition(); - double pos2 = input2.getMotorPosition(); - double avg = (pos1 + pos2) / 2; - double diff = raw; - input1.setMotorPosition(avg + diff); - input2.setMotorPosition(avg - diff); - } - }; - - sumOutput = new Motor() { - @Override - public void stopMotor() { - setVoltage(0); - } - - @Override - public void setVoltage(double voltage) { - sumVoltage = voltage; - setVoltages(); - } - - @Override - public void setPercentage(double percentage) { - setVoltage(percentage * 12); - } - - @Override - public void setFollower(int mainMotorID) {} - - @Override - public void setIdleState(IdleState idleMode) { - input1.setIdleState(idleMode); - input2.setIdleState(idleMode); - } - - @Override - public IdleState getIdleState() { - return input1.getIdleState(); - } - - @Override - public int getDeviceID() { - return -1; - } - - @Override - public double getMotorPosition() { - return average(input1.getMotorPosition(), input2.getMotorPosition()) * sumPositionConversion + sumPositionOffset; - } - - @Override - public double getMotorVelocity() { - return average(input1.getMotorVelocity(), input2.getMotorVelocity()) * sumVelocityConversion; - } - - @Override - public double getCurrent() { - return average(input1.getCurrent(), input2.getCurrent()); - } - - @Override - public double getVoltage() { - return average(input1.getVoltage(), input2.getVoltage()); - } - - @Override - public double getTemperature() { - return -1; - } - - @Override - public void setSoftLimit(DirectionState directionState, float limit) {} - - @Override - public void setInverted(DirectionState mode) {} - - @Override - public void setPositionConversionFactor(double conversionFactor) { - sumPositionConversion = conversionFactor; - } - - @Override - public void setVelocityConversionFactor(double conversionFactor) { - sumVelocityConversion = conversionFactor; - } - - @Override - public void setCurrentLimit(int stallLimit, int freeLimit) {} - - @Override - public void setMotorPosition(double position) { - double raw = (position - sumPositionOffset) / sumPositionConversion; - double diff = input1.getMotorPosition() - input2.getMotorPosition(); - double pos1 = raw + diff / 2; - double pos2 = raw - diff / 2; - input1.setMotorPosition(pos1); - input2.setMotorPosition(pos2); - } - }; - } - - private static double average(double a, double b) { - return (a + b) / 2; - } - - private static double halfDiff(double a, double b) { - return (a - b) / 2; - } - - private void setVoltages() { - input1.setVoltage(sumVoltage - diffVoltage / 2); - input2.setVoltage(sumVoltage + diffVoltage / 2); - } - - public Motor getDiffOutput() { - return diffOutput; - } - - public Motor getSumOutput() { - return sumOutput; - } -} diff --git a/src/main/java/frc/excalib/control/encoder/DutyCycleEncoder.java b/src/main/java/frc/excalib/control/encoder/DutyCycleEncoder.java new file mode 100644 index 0000000..f3f03d4 --- /dev/null +++ b/src/main/java/frc/excalib/control/encoder/DutyCycleEncoder.java @@ -0,0 +1,35 @@ +package frc.excalib.control.encoder; + +public class DutyCycleEncoder extends edu.wpi.first.wpilibj.DutyCycleEncoder implements Encoder { + private double positionConversionFactor, velocityConversionFactor; + private double position; + + public DutyCycleEncoder(int port){ + super(port); + } + + @Override + public void setPositionConversionFactor(double conversionFactor) { + this.positionConversionFactor = conversionFactor; + } + + @Override + public void setVelocityConversionFactor(double conversionFactor) { + this.velocityConversionFactor = conversionFactor; + } + + @Override + public double getPosition() { + return super.get() * this.positionConversionFactor + this.position; + } + + @Override + public double getVelocity() { + return 0; + } + + @Override + public void setPosition(double position) { + this.position = position; + } +} diff --git a/src/main/java/frc/excalib/control/encoder/Encoder.java b/src/main/java/frc/excalib/control/encoder/Encoder.java new file mode 100644 index 0000000..ddeb6d4 --- /dev/null +++ b/src/main/java/frc/excalib/control/encoder/Encoder.java @@ -0,0 +1,16 @@ +package frc.excalib.control.encoder; + +public interface Encoder { + + void setPositionConversionFactor(double positionConversionFactor); + + void setVelocityConversionFactor(double velocityConversionFactor); + + double getPosition(); + + double getVelocity(); + + void setPosition(double position); + + void setInverted(boolean inverted); +} diff --git a/src/main/java/frc/excalib/control/gains/Gains.java b/src/main/java/frc/excalib/control/gains/Gains.java index b716eb3..a706818 100644 --- a/src/main/java/frc/excalib/control/gains/Gains.java +++ b/src/main/java/frc/excalib/control/gains/Gains.java @@ -128,7 +128,7 @@ public PIDController getPIDcontroller(){ public GenericFeedFoward applyGains(GenericFeedFoward feedFoward){ - feedFoward.setValue(this.ks, this.kv, this.ka, this.kg); + feedFoward.setValues(this.ks, this.kv, this.ka, this.kg); return feedFoward; } } diff --git a/src/main/java/frc/excalib/control/math/Circle.java b/src/main/java/frc/excalib/control/math/Circle.java deleted file mode 100644 index 30c3f7b..0000000 --- a/src/main/java/frc/excalib/control/math/Circle.java +++ /dev/null @@ -1,102 +0,0 @@ -package frc.excalib.control.math; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -/** - * Represents a circle in a 2D plane, defined by its center and radius. - */ -public class Circle { - // The radius of the circle - public final double r; - // The center of the circle as a Translation2d object - public final Translation2d center; - - /** - * Constructs a Circle object with the specified center coordinates and radius. - * - * @param a The x-coordinate of the circle's center. - * @param b The y-coordinate of the circle's center. - * @param r The radius of the circle. - */ - public Circle(double a, double b, double r) { - this.center = new Translation2d(a, b); - this.r = r; - } - - /** - * Calculates the tangent line to the circle at a given point on its circumference. - * - * @param pointOnCircle A point on the circle's circumference. - * @return A Line object representing the tangent line. - */ - public Line getTangent(Translation2d pointOnCircle) { - return new Line( - pointOnCircle.getX() - center.getX(), - pointOnCircle.getY() - center.getY(), - -center.getX() * (pointOnCircle.getX() - center.getX()) - - center.getY() * (pointOnCircle.getY() - center.getY()) - - this.r * this.r - ); - } - - /** - * Calculates the tangent lines from a given external point to the circle. - * - * @param point A point outside or on the circle. - * @return An array of Line objects representing the tangent lines. - * Returns an empty array if the point is inside the circle. - */ - public Line[] getTangents(Translation2d point) { - if (point.getDistance(this.center) < this.r) { - return new Line[0]; - } else if (point.getDistance(this.center) == this.r) { - return new Line[]{ - getTangent(point) - }; - } - double centersDistance = this.center.getDistance(point); - double newRad = Math.sqrt(Math.pow(centersDistance, 2) - Math.pow(this.r, 2)); - Circle newCircle = new Circle(point.getX(), point.getY(), newRad); - Translation2d[] intersections = getInterSections(newCircle); - Translation2d firstTanPoint = intersections[0]; - Translation2d secondTanPoint = intersections[1]; - - return new Line[]{ - getTangent(firstTanPoint), - getTangent(secondTanPoint) - }; - } - - /** - * Calculates the intersection points between this circle and another circle. - * - * @param other The other circle to find intersections with. - * @return An array of Translation2d objects representing the intersection points. - * Returns an empty array if there are no intersections. - */ - public Translation2d[] getInterSections(Circle other) { - - if (other.center.getDistance(this.center) > other.r + r) return new Translation2d[0]; - if (other.center.getDistance(this.center) < Math.abs(other.r - this.r)) return new Translation2d[0]; - - if (other.center.getDistance(this.center) == other.r + r) { - return new Translation2d[]{ - this.center.plus(new Translation2d(this.r, other.center.minus(this.center).getAngle())) - }; - } - if (other.center.getDistance(this.center) < Math.abs(other.r - this.r)) { - return new Translation2d[]{ // Check for edge case - this.center.plus(new Translation2d(this.r, other.center.minus(this.center).getAngle())) - }; - } - Rotation2d alpha = new Rotation2d(Math.acos( - (Math.pow(other.r, 2) - Math.pow(this.r, 2) - Math.pow(this.center.getDistance(other.center), 2)) - / (-2 * this.center.getDistance(other.center) * this.r) - )); - return new Translation2d[]{ - this.center.plus(new Translation2d(this.r, other.center.minus(this.center).getAngle().plus(alpha))), - this.center.plus(new Translation2d(this.r, other.center.minus(this.center).getAngle().minus(alpha))) - }; - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/math/Line.java b/src/main/java/frc/excalib/control/math/Line.java deleted file mode 100644 index 1613b96..0000000 --- a/src/main/java/frc/excalib/control/math/Line.java +++ /dev/null @@ -1,52 +0,0 @@ -package frc.excalib.control.math; - -import edu.wpi.first.math.geometry.Translation2d; - -/** - * Represents a line in a 2D plane, defined by the equation ax + by + c = 0. - */ -public class Line { - // Coefficient of x in the line equation - public final double a; - // Coefficient of y in the line equation - public final double b; - // Constant term in the line equation - public final double c; - - /** - * Constructs a Line object with the specified coefficients. - * - * @param a The coefficient of x in the line equation. - * @param b The coefficient of y in the line equation. - * @param c The constant term in the line equation. - */ - public Line(double a, double b, double c) { - this.a = a; - this.b = b; - this.c = c; - } - - /** - * Finds the intersection point of this line with another line. - * - * @param other The other line to find the intersection with. - * @return A Translation2d object representing the intersection point. - * If the lines are parallel, the result may be undefined. - */ - public Translation2d findIntersection(Line other) { - return new Translation2d( - (other.b * c - b * other.c) / (b * other.a - other.b * a), - (other.a * c - a * other.c) / (a * other.b - other.a * b) - ); - } - - /** - * Returns a string representation of the line in the format "a: [a], b: [b], c: [c]". - * - * @return A string representation of the line. - */ - @Override - public String toString() { - return "a: " + a + ", b: " + b + ", c: " + c; - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/math/MathUtils.java b/src/main/java/frc/excalib/control/math/MathUtils.java deleted file mode 100644 index a6c993e..0000000 --- a/src/main/java/frc/excalib/control/math/MathUtils.java +++ /dev/null @@ -1,78 +0,0 @@ -package frc.excalib.control.math; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -/** - * Utility class for mathematical operations and geometry-related calculations. - */ -public class MathUtils { - - /** - * Ensures that the absolute value of the given number does not exceed the specified limit. - * - * @param val The value to be limited. - * @param sizeLimit The maximum allowable absolute value. - * @return The value limited to the specified maximum absolute value, preserving its sign. - */ - public static double minSize(double val, double sizeLimit) { - return Math.min(sizeLimit, Math.abs(val)) * Math.signum(val); - } - - /** - * Limits the given value to the specified limit. If the value exceeds the limit, it is clamped to the limit. - * - * @param limit The maximum allowable value (positive or negative). - * @param value The value to be limited. - * @return The value clamped to the specified limit. - */ - public static double limitTo(double limit, double value) { - if ((limit > 0 && limit < value) || (limit < 0 && limit > value)) { - return limit; - } - return value; - } - - /** - * Calculates the optimal target position for a robot to reach a target while avoiding a reef. - * - * @param robot The current position of the robot as a Translation2d. - * @param target The target position as a Translation2d. - * @param reefCenter The center of the reef as a Translation2d. - * @return The optimal target position as a Translation2d. - */ - public static Translation2d getTargetPose(Translation2d robot, Translation2d target, Translation2d reefCenter) { - double radius = reefCenter.getDistance(target); - Circle c = new Circle(reefCenter.getX(), reefCenter.getY(), radius); - Line[] tangents = c.getTangents(robot); - Rotation2d alpha = target.minus(reefCenter).getAngle(); - Rotation2d theta = robot.minus(reefCenter).getAngle(); - - // If the angle between the target and robot is less than 60 degrees, return the target directly. - if (Math.abs(alpha.minus(theta).getRadians()) < Math.PI / 3) { - return target; - } - - // If no tangents exist, calculate a point on the reef perimeter and find the intersection. - if (tangents.length == 0) { - Translation2d onPerimeter = reefCenter.plus(new Translation2d(radius, robot.minus(reefCenter).getAngle())); - Line tangent = c.getTangent(onPerimeter); - return tangent.findIntersection(c.getTangent(target)); - } - - // If tangents exist, find the intersection of the target tangent with the robot tangents. - Line targetTangent = c.getTangent(target); - if (tangents.length == 1) { - return targetTangent.findIntersection(tangents[0]); - } - - // Calculate the two possible intersection points and return the closer one to the target. - Translation2d translation1 = targetTangent.findIntersection(tangents[0]); - Translation2d translation2 = targetTangent.findIntersection(tangents[1]); - - if (target.getDistance(translation1) < target.getDistance(translation2)) { - return translation1; - } - return translation2; - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/math/Vector2D.java b/src/main/java/frc/excalib/control/math/Vector2D.java deleted file mode 100644 index 9680aa4..0000000 --- a/src/main/java/frc/excalib/control/math/Vector2D.java +++ /dev/null @@ -1,164 +0,0 @@ -package frc.excalib.control.math; - -import edu.wpi.first.math.geometry.Rotation2d; - -/** - * A class representing a Vector in two dimensions; - * the Vector is defined by x value and y value - * - * @author Itay - */ -public class Vector2D { - private double m_x, m_y; - - /** - * A constructor that takes two doubles representing the x and y components: - * - * @param x The x component of the Vector - * @param y The y component of the Vector - */ - public Vector2D(double x, double y) { - this.m_x = x; - this.m_y = y; - } - - /** - * A constructor that takes a double representing the distance, - * and a Rotation2D representing the angle: - * - * @param magnitude The distance from the origin - * @param direction The angle of the Vector - */ - public Vector2D(double magnitude, Rotation2d direction) { - this.m_x = magnitude * direction.getCos(); - this.m_y = magnitude * direction.getSin(); - } - - /** - * A function to get the x component of the Vector - * - * @return x component - */ - public double getX() { - return m_x; - } - - /** - * A function to get the y component of the Vector - * - * @return y component - */ - public double getY() { - return m_y; - } - - /** - * A function to get the distance from the origin - * - * @return distance - */ - public double getDistance() { - return Math.hypot(m_x, m_y); - } - - /** - * A function to get the direction of the Vector - * - * @return direction - */ - public Rotation2d getDirection() { - return new Rotation2d(Math.atan2(m_y, m_x)); - } - - /** - * A function that adds another Vector to this Vector - * - * @param other The other vector to add - * @return a new Vector2D that represents the sum of the Vectors - */ - public Vector2D plus(Vector2D other) { - return new Vector2D(m_x + other.m_x, m_y + other.m_y); - } - - /** - * A function that multiplies the Vector by a scalar - * - * @param scalar The scalar to multiply by - * @return a new Vector2D that represents the scaled Vector - */ - public Vector2D mul(double scalar) { - return new Vector2D(m_x * scalar, m_y * scalar); - } - - /** - * A function that rotates the Vector by a given angle - * - * @param deltaDirection The angle to rotate by - * @return a new Vector2D that represents the rotated Vector - */ - public Vector2D rotate(Rotation2d deltaDirection) { - double cosTheta = deltaDirection.getCos(); - double sinTheta = deltaDirection.getSin(); - - double newX = m_x * cosTheta - m_y * sinTheta; - double newY = m_x * sinTheta + m_y * cosTheta; - - return new Vector2D(newX, newY); - } - - /** - * Sets the x component of the Vector - * - * @param x The new x component - */ - public void setX(double x) { - this.m_x = x; - } - - /** - * Sets the y component of the Vector - * - * @param y The new y component - */ - public void setY(double y) { - this.m_y = y; - } - - /** - * Sets the magnitude of the Vector while keeping its direction - * - * @param magnitude The new magnitude - */ - public void setMagnitude(double magnitude) { - double currentMagnitude = getDistance(); - if (currentMagnitude != 0) { - double scale = magnitude / currentMagnitude; - this.m_x *= scale; - this.m_y *= scale; - } else { - // When the current magnitude is zero, direction is undefined. - // Default to setting x to the magnitude and y to zero. - this.m_x = magnitude; - this.m_y = 0; - } - } - - /** - * Sets the direction of the Vector while keeping its magnitude - * - * @param direction The new direction - */ - public void setDirection(Rotation2d direction) { - double magnitude = getDistance(); - this.m_x = magnitude * direction.getCos(); - this.m_y = magnitude * direction.getSin(); - } - - public Vector2D limit(Vector2D limit) { - Vector2D output = new Vector2D(m_x, m_y); - output.rotate(limit.getDirection().unaryMinus()); - output.setX(MathUtils.limitTo(limit.getDistance(), output.m_x)); - output.setDirection(this.getDirection()); - return output; - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/math/physics/Mass.java b/src/main/java/frc/excalib/control/math/physics/Mass.java deleted file mode 100644 index 5dde935..0000000 --- a/src/main/java/frc/excalib/control/math/physics/Mass.java +++ /dev/null @@ -1,51 +0,0 @@ -package frc.excalib.control.math.physics; - -import edu.wpi.first.math.geometry.Translation2d; - -import java.util.function.DoubleSupplier; - -/** - * This Class represents the mass of an object including size, center - * and utility functions. - */ -public class Mass { - private final DoubleSupplier m_xSupplier; - private final DoubleSupplier m_ySupplier; - private final double mass; - - /** - * returns a new Mass object - * - * @param xSupplier the x position of the Mass (m) - * @param ySupplier the y position of the Mass (m) - * @param mass the size value of the mass (kg) - */ - public Mass(DoubleSupplier xSupplier, DoubleSupplier ySupplier, double mass) { - this.m_xSupplier = xSupplier; - this.m_ySupplier = ySupplier; - this.mass = mass; - } - - /** - * @return the center of the mass as a Translation2d - */ - public Translation2d getCenterOfMass() { - return new Translation2d(m_xSupplier.getAsDouble(), m_ySupplier.getAsDouble()); - } - - /** - * @param other the mass to add - * @return a new Mass representing the mass of the system the includes both masses. - */ - public Mass add(Mass other) { - return new Mass( - () -> - (other.mass * other.m_xSupplier.getAsDouble() + - this.mass * this.m_xSupplier.getAsDouble()) / (other.mass + this.mass), - () -> - (other.mass * other.m_ySupplier.getAsDouble() + - this.mass * this.m_ySupplier.getAsDouble()) / (other.mass + this.mass), - other.mass + this.mass - ); - } -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/motor/Piston.java b/src/main/java/frc/excalib/control/motor/Piston.java new file mode 100644 index 0000000..6a86537 --- /dev/null +++ b/src/main/java/frc/excalib/control/motor/Piston.java @@ -0,0 +1,27 @@ +package frc.excalib.control.motor; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import frc.excalib.control.motor.motor_specs.DirectionState; + +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.*; + +public class Piston { + private final DoubleSolenoid piston; + private DirectionState direction; + + public Piston(int fwdID, int revID){ + this.piston = new DoubleSolenoid(PneumaticsModuleType.REVPH, fwdID, revID); + this.direction = DirectionState.REVERSE; + } + + public void setPiston(DirectionState direction){ + this.piston.set(direction == DirectionState.FORWARD? kForward : kReverse); + this.direction = direction; + } + + public void togglePiston(){ + if (direction == DirectionState.FORWARD) setPiston(DirectionState.REVERSE); + else setPiston(DirectionState.FORWARD); + } +} diff --git a/src/main/java/frc/excalib/mechanisms/Mechanism.java b/src/main/java/frc/excalib/mechanisms/Mechanism.java index a0f5d26..fd706a0 100644 --- a/src/main/java/frc/excalib/mechanisms/Mechanism.java +++ b/src/main/java/frc/excalib/mechanisms/Mechanism.java @@ -10,6 +10,7 @@ import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.excalib.control.encoder.Encoder; import frc.excalib.control.gains.SysidConfig; import frc.excalib.control.motor.Motor; import frc.excalib.control.motor.motorType.DifferentialMotor; @@ -27,19 +28,28 @@ */ public class Mechanism implements Logged { protected final Motor m_motor; + private final IdleState m_DEFAULT_IDLE_STATE; + + private Encoder m_externalEncoder; + protected final MutVoltage m_appliedVoltage = Volts.mutable(0); protected final MutAngle m_radians = Radians.mutable(0); protected final MutDistance m_meter = Meters.mutable(0); protected final MutAngularVelocity m_velocity = RadiansPerSecond.mutable(0); protected final MutLinearVelocity m_Linearvelocity = MetersPerSecond.mutable(0); - private final IdleState m_DEFAULT_IDLE_STATE; /** * @param motor the motor controlling the mechanism */ - public Mechanism(Motor motor) { + public Mechanism(Motor motor, Encoder externalEncoder) { m_motor = motor; m_DEFAULT_IDLE_STATE = m_motor.getIdleState(); + + m_externalEncoder = externalEncoder; + } + + public Mechanism(Motor motor){ + this(motor, null); } /** @@ -64,6 +74,10 @@ protected void setDifferentialVoltage(double voltageA, double voltageB){ ((DifferentialMotor) this.m_motor).setDifferentialVoltage(voltageA, voltageB); } + public void addExternalEncoder(Encoder externalEncoder){ + m_externalEncoder = externalEncoder; + } + /** * @param outputSupplier the dynamic setpoint for the mechanism (voltage) * @return a command which controls the mechanism manually @@ -79,7 +93,7 @@ public Command manualCommand(DoubleSupplier outputSupplier, SubsystemBase... req /** * @return an instant command to stop the motor */ - public Command stopMotorCommand(SubsystemBase... requirements) { + public Command stopMechnismCommand(SubsystemBase... requirements) { return new InstantCommand(() -> setOutput(0), requirements); } @@ -87,16 +101,18 @@ public Command stopMotorCommand(SubsystemBase... requirements) { * @return the velocity */ @Log.NT - public double logVelocity() { - return m_motor.getMotorVelocity(); + public double logMechanismVelocity() { + if (m_externalEncoder == null) return m_motor.getMotorVelocity(); + return m_externalEncoder.getVelocity(); } /** * @return the position */ @Log.NT - public double logPosition() { - return m_motor.getMotorPosition(); + public double logMechanismPosition() { + if (m_externalEncoder == null) return m_motor.getMotorPosition(); + return m_externalEncoder.getPosition(); } @Log.NT @@ -126,7 +142,7 @@ private SysIdRoutine getLinearSysIdRoutine(SubsystemBase subsystem, DoubleSuppli .voltage(m_appliedVoltage.mut_replace( m_motor.getVoltage(), Volts)) .linearPosition(m_meter.mut_replace(sensorInput.getAsDouble(), Meters)) - .linearVelocity(m_Linearvelocity.mut_replace(logVelocity(), MetersPerSecond)), + .linearVelocity(m_Linearvelocity.mut_replace(logMechanismVelocity(), MetersPerSecond)), subsystem ) ); @@ -149,7 +165,7 @@ private SysIdRoutine getAngularSysIdRoutine(SubsystemBase subsystem, DoubleSuppl .voltage(m_appliedVoltage.mut_replace( m_motor.getVoltage(), Volts)) .angularPosition(m_radians.mut_replace(sensorInput.getAsDouble(), Radians)) - .angularVelocity(m_velocity.mut_replace(logVelocity(), RadiansPerSecond)), + .angularVelocity(m_velocity.mut_replace(logMechanismVelocity(), RadiansPerSecond)), subsystem ) ); diff --git a/src/main/java/frc/excalib/mechanisms/arm/Arm.java b/src/main/java/frc/excalib/mechanisms/arm/Arm.java index bcb1006..9d809c0 100644 --- a/src/main/java/frc/excalib/mechanisms/arm/Arm.java +++ b/src/main/java/frc/excalib/mechanisms/arm/Arm.java @@ -1,73 +1,53 @@ package frc.excalib.mechanisms.arm; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.limits.SoftLimit; -import frc.excalib.control.math.physics.Mass; import frc.excalib.control.motor.Motor; import frc.excalib.mechanisms.Mechanism; -import java.util.function.Consumer; import java.util.function.DoubleSupplier; /** * This class represents an Arm Mechanism */ public class Arm extends Mechanism { - private final Mass m_mass; private final PIDController m_PIDController; - public final DoubleSupplier ANGLE_SUPPLIER; - public final double m_kv, m_ks, m_kg; - public final SoftLimit m_VELOCITY_LIMIT; + private final ArmFeedforward m_ffController; - public Arm(Motor motor, DoubleSupplier angleSupplier, - SoftLimit velocityLimit, Gains gains, Mass mass) { + private final TrapezoidProfile m_profile; + + + public Arm(Motor motor, Gains gains, TrapezoidProfile.Constraints constraints) { super(motor); - ANGLE_SUPPLIER = angleSupplier; - m_VELOCITY_LIMIT = velocityLimit; - m_kg = gains.kg; - m_kv = gains.kv; - m_ks = gains.ks; m_PIDController = gains.getPIDcontroller(); - m_mass = mass; + m_ffController = gains.applyGains(new GenericFF.ArmFF()); + + m_profile = new TrapezoidProfile(constraints); } - /** - * @param setPointSupplier the dynamic angle setpoint to go to (radians) - * @param toleranceConsumer gets updated if the measurement is at tolerance. - * @return a command that moves the arm to the specified dynamic setpoint. - */ - public Command anglePositionControlCommand( - DoubleSupplier setPointSupplier, Consumer toleranceConsumer, - double maxOffSet, SubsystemBase... requirements) { - final double dutyCycle = 0.02; - return new RunCommand( - () -> { - double error = setPointSupplier.getAsDouble() - ANGLE_SUPPLIER.getAsDouble(); - double velocitySetpoint = error / dutyCycle; - velocitySetpoint = m_VELOCITY_LIMIT.limit(velocitySetpoint); - double phyOutput = - m_ks * Math.signum(velocitySetpoint) + - m_kg * m_mass.getCenterOfMass().getX(); - double pid = m_PIDController.calculate( - ANGLE_SUPPLIER.getAsDouble(), - setPointSupplier.getAsDouble() + public Command setDynamicPositionCommand(DoubleSupplier position, SubsystemBase... requirements) { + return new RunCommand(() -> { + TrapezoidProfile.State state = m_profile.calculate( + 0.02, + new TrapezoidProfile.State( + super.logMechanismPosition(), + super.logMechanismVelocity()), + new TrapezoidProfile.State(position.getAsDouble(), 0) ); - double output = phyOutput + pid; - super.setVoltage(m_VELOCITY_LIMIT.limit(output)); - toleranceConsumer.accept(Math.abs(error) < maxOffSet); + setPosition(state.position); }, requirements); } - /** - * @param angle the angle setpoint to go to (radians) - * @param toleranceConsumer gets updated if the measurement is at tolerance. - * @return a command that moves the arm to the specified setpoint. - */ - public Command goToAngleCommand(double angle, Consumer toleranceConsumer, double maxOffSet, SubsystemBase... requirements) { - return anglePositionControlCommand(() -> angle, toleranceConsumer, maxOffSet, requirements); + public void setPosition(double position){ + double pid = m_PIDController.calculate(super.logMechanismPosition(), position); + double ff = m_ffController.calculate(super.logMechanismPosition(), super.logMechanismVelocity()); + setVoltage(pid + ff); } } diff --git a/src/main/java/frc/excalib/mechanisms/diffrential/Differential.java b/src/main/java/frc/excalib/mechanisms/diffrential/Differential.java index fc6e0c7..13cbb35 100644 --- a/src/main/java/frc/excalib/mechanisms/diffrential/Differential.java +++ b/src/main/java/frc/excalib/mechanisms/diffrential/Differential.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.GenericFF.FeedForwardGainsSetter; import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.motor.motorType.DifferentialMotor; @@ -16,14 +17,10 @@ import java.util.function.DoubleSupplier; public class Differential extends Mechanism { - private final Gains gains; private final PIDController pidController_A, pidController_B; - private final ArmFeedforward ffController; private final DifferentialMotor motor; - public final Trigger atSetpointTrigger; - private final double differentialMul; private double deltaA, deltaB; @@ -44,12 +41,8 @@ public Differential(DifferentialMotor differentialMotor, Gains gains, double dif this.motor = differentialMotor; this.motor.setIdleState(IdleState.BRAKE); - this.gains = gains; - this.atSetpointTrigger = new Trigger(()-> (Math.abs(deltaA - motorApos) + Math.abs(deltaB - motorBpos)) < tolerance); - - this.pidController_A = this.gains.getPIDcontroller(); - this.pidController_B = this.gains.getPIDcontroller(); - this.ffController = this.gains.applyGains(new GenericFF.ArmFF()); + this.pidController_A = gains.getPIDcontroller(); + this.pidController_B = gains.getPIDcontroller(); this.differentialMul = differentialMul; } @@ -59,10 +52,10 @@ public Differential(DifferentialMotor differentialMotor, Gains gains, double dif * @param differentialMotor */ public Differential(DifferentialMotor differentialMotor) { - this(differentialMotor, new Gains(), 1, 20); + this(differentialMotor, new Gains(),1, 20); } - public Command moveToState(double angle, double position, double ff, SubsystemBase... requirements){ + public Command moveToStateCommand(double angle, double position, double ff, SubsystemBase... requirements){ return Commands.startRun( ()-> { double deltaTheta = angle - getMechanismAngle(); @@ -83,7 +76,7 @@ public Command moveToState(double angle, double position, double ff, SubsystemBa requirements); } - public Command setDifferentialVoltage(DoubleSupplier voltageA, DoubleSupplier voltageB, SubsystemBase... requirements){ + public Command setDifferentialVoltageCommand(DoubleSupplier voltageA, DoubleSupplier voltageB, SubsystemBase... requirements){ return new RunCommand( ()-> setDifferentialVoltage(voltageA.getAsDouble(), voltageB.getAsDouble()), requirements); diff --git a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java b/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java index fd9f3b5..465dc77 100644 --- a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java +++ b/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -18,84 +17,35 @@ * A class the represents A FlyWheel Mechanism. */ public class FlyWheel extends Mechanism { - private double lastTime, lastVelocity; private final PIDController m_pidController; - private final double maxAcceleration; - private final double maxJerk; - private final Gains m_gains; - private final SimpleMotorFeedforward m_FF_CONTROLLER; + private final SimpleMotorFeedforward m_ffController; /** - * @param motor the FlyWheel Motor - * @param maxAcceleration the max acceleration of the FlyWheel - * @param maxJerk the max jerk of the FlyWheel - * @param gains the FF and PID gains + * @param motor the FlyWheel Motor + * @param gains the FF and PID gains */ - public FlyWheel(Motor motor, double maxAcceleration, double maxJerk, Gains gains) { + public FlyWheel(Motor motor, Gains gains) { super(motor); - m_gains = gains; - this.maxAcceleration = maxAcceleration; - this.maxJerk = maxJerk; this.m_pidController = gains.getPIDcontroller(); - this.m_FF_CONTROLLER = gains.applyGains(new GenericFF.SimpleFF()); + this.m_ffController = gains.applyGains(new GenericFF.SimpleFF()); } /** - * @param velocitySupplier a dynamic velocity setpoint. - * @return a command that controls the FlyWheels velocity with high precision - */ - public Command smartVelocityCommand(DoubleSupplier velocitySupplier, SubsystemBase... requirements) { - return new RunCommand( - () -> { - TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(maxAcceleration, maxJerk)); - TrapezoidProfile.State state = profile.calculate( - 0.02, - new TrapezoidProfile.State(super.m_motor.getMotorVelocity(), getAcceleration()), - new TrapezoidProfile.State(velocitySupplier.getAsDouble(), 0)); - double ff = m_gains.ks * Math.signum(state.position) + - m_gains.kv * state.position + - m_gains.ka * state.velocity; - double pid = m_pidController.calculate(super.m_motor.getMotorVelocity(), state.position); - setVoltage(pid + ff); - }); - } - - /** - * @return the FlyWheels current acceleration - */ - private double getAcceleration() { - //TODO: move to current based calculations - double currentTime = Timer.getFPGATimestamp(); - double currentVelocity = super.m_motor.getMotorVelocity(); - return (currentVelocity - lastVelocity) / (currentTime - lastTime); - } - - /** - * @param velocity the dynamic velocity setpoint - * @return a command which controls the FlyWheels velocity + * sets the FlyWheel to a Dynamic velocity + * @param velocity velocity supplier + * @param requirements subsystem requirements + * @return the command to set the FlyWheel velocity */ public Command setDynamicVelocityCommand(DoubleSupplier velocity, SubsystemBase... requirements) { - return new RunCommand(() -> setDynamicVelocity(velocity.getAsDouble()), requirements); + return new RunCommand(() -> setVelocity(velocity.getAsDouble()), requirements); } /** * @param velocity the velocity to set to the FlyWheel Dynamically */ - public void setDynamicVelocity(double velocity) { - double ff = m_FF_CONTROLLER.calculate(velocity); + public void setVelocity(double velocity) { double pid = m_pidController.calculate(super.m_motor.getMotorVelocity(), velocity); + double ff = m_ffController.calculate(velocity); super.setVoltage(pid + ff); } - - public void periodic() { - lastTime = Timer.getFPGATimestamp(); - lastVelocity = super.m_motor.getMotorVelocity(); - } - - /** - * @return the current velocity of the FlyWheel. - */ - public double getVelocity() { - return super.m_motor.getMotorVelocity(); - } } \ No newline at end of file diff --git a/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java b/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java index be7cb3e..149555f 100644 --- a/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java +++ b/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java @@ -1,10 +1,12 @@ package frc.excalib.mechanisms.linear_extension; +import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.motor.Motor; import frc.excalib.mechanisms.Mechanism; @@ -12,60 +14,41 @@ import java.util.function.DoubleSupplier; public class LinearExtension extends Mechanism { - private final DoubleSupplier m_positionSupplier; - private final DoubleSupplier m_angleSupplier; - private final PIDController m_PIDController; - private final double m_tolerance; - private final Gains m_gains; + private final PIDController m_pidController; + private final ElevatorFeedforward m_ffController; - private final TrapezoidProfile.Constraints m_constraints; + private final TrapezoidProfile m_profile; - public LinearExtension(Motor motor, DoubleSupplier positionSupplier, DoubleSupplier angleSupplier, - Gains gains, TrapezoidProfile.Constraints constraints, double tolerance) { + public LinearExtension(Motor motor, Gains gains, TrapezoidProfile.Constraints constraints) { super(motor); - m_positionSupplier = positionSupplier; - m_angleSupplier = angleSupplier; - m_gains = gains; - m_PIDController = gains.getPIDcontroller(); - m_constraints = constraints; - m_tolerance = tolerance; + m_pidController = gains.getPIDcontroller(); + m_ffController = gains.applyGains(new GenericFF.ElevatorFF()); + + m_profile = new TrapezoidProfile(constraints); } - public Command extendCommand(DoubleSupplier lengthSetPoint, SubsystemBase... requirements) { + /** + * moves the LinearExtension to a Dynamic position with a trapezoid profile + * @param position position supplier + * @param requirements subsystem requirements + * @return the command to move the LinearExtension + */ + public Command setDynamicPositionCommand(DoubleSupplier position, SubsystemBase... requirements){ return new RunCommand(() -> { - TrapezoidProfile profile = new TrapezoidProfile(m_constraints); - TrapezoidProfile.State state = - profile.calculate( - 0.02, - new TrapezoidProfile.State( - m_positionSupplier.getAsDouble(), - super.m_motor.getMotorVelocity()), - new TrapezoidProfile.State(lengthSetPoint.getAsDouble(), 0) - ); - double pidValue = m_PIDController.calculate(m_positionSupplier.getAsDouble(), state.position); - double ff = - (Math.abs(m_positionSupplier.getAsDouble() - lengthSetPoint.getAsDouble()) > m_tolerance) ? - - m_gains.ks * Math.signum(state.velocity) + - m_gains.kv * state.velocity + - m_gains.kg * Math.sin(m_angleSupplier.getAsDouble()) : - - m_gains.kg * Math.sin(m_angleSupplier.getAsDouble()); - double output = ff + pidValue; - setVoltage(output); + TrapezoidProfile.State state = m_profile.calculate( + 0.02, + new TrapezoidProfile.State( + super.logMechanismPosition(), + super.logMechanismVelocity()), + new TrapezoidProfile.State(position.getAsDouble(), 0) + ); + setPosition(state.position); }, requirements); } - public double logVoltage() { - return m_motor.getVoltage(); - } - - public double logVelocity() { - return m_motor.getMotorVelocity(); - } - - public double logPosition() { - return m_positionSupplier.getAsDouble(); + public void setPosition(double position){ + double pid = m_pidController.calculate(super.logMechanismPosition(), position); + double ff = m_ffController.calculate(super.logMechanismVelocity()); + setVoltage(pid + ff); } - } \ No newline at end of file diff --git a/src/main/java/frc/excalib/mechanisms/turret/Turret.java b/src/main/java/frc/excalib/mechanisms/turret/Turret.java index 9a84fb5..f3da510 100644 --- a/src/main/java/frc/excalib/mechanisms/turret/Turret.java +++ b/src/main/java/frc/excalib/mechanisms/turret/Turret.java @@ -2,9 +2,8 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.excalib.control.GenericFF.GenericFF; @@ -14,67 +13,62 @@ import frc.excalib.mechanisms.Mechanism; import java.util.function.DoubleSupplier; -import java.util.function.Supplier; /** * A class representing a Turret Mechanism */ public final class Turret extends Mechanism { + private final PIDController m_pidController; + private final SimpleMotorFeedforward m_ffController; + + private final TrapezoidProfile m_profile; private final ContinuousSoftLimit m_rotationLimit; - private final PIDController m_anglePIDcontroller; - private final SimpleMotorFeedforward m_angleFFcontroller; - private final DoubleSupplier m_POSITION_SUPPLIER; /** - * @param motor the turrets motor + * @param motor the turret's motor * @param rotationLimit the rotational boundary for the turret (radians) * @param angleGains pid gains for the turret - * @param PIDtolerance pid tolerance for the turret (radians) - * @param positionSupplier the position measurement + * @param constraints constraints for the TrapezoidProfile */ - public Turret(Motor motor, ContinuousSoftLimit rotationLimit, Gains angleGains, double PIDtolerance, DoubleSupplier positionSupplier) { + public Turret(Motor motor, ContinuousSoftLimit rotationLimit, Gains angleGains, TrapezoidProfile.Constraints constraints) { super(motor); m_rotationLimit = rotationLimit; - m_anglePIDcontroller = angleGains.getPIDcontroller(); - m_angleFFcontroller = angleGains.applyGains(new GenericFF.SimpleFF()); - - m_anglePIDcontroller.setTolerance(PIDtolerance); - m_anglePIDcontroller.enableContinuousInput(-Math.PI, Math.PI); + m_pidController = angleGains.getPIDcontroller(); + m_ffController = angleGains.applyGains(new GenericFF.SimpleFF()); - m_POSITION_SUPPLIER = positionSupplier; - } + m_pidController.enableContinuousInput(-Math.PI, Math.PI); - /** - * @param wantedPosition a Rotation2d dynamic setpoint - * @return a Command that moves the turret tho the given setpoint - */ - public Command setPositionCommand(Supplier wantedPosition, SubsystemBase... requirements){ - return new RunCommand(()-> setPosition(wantedPosition.get()), requirements); + m_profile = new TrapezoidProfile(constraints); } /** - * moves the turret to the desired position - * @param wantedPosition the wanted position of the turret. + * moves the Turret to a Dynamic position with a Trapezoid profile + * @param position position supplier + * @param requirements subsystem requirements + * @return the command to move the Turret */ - public void setPosition(Rotation2d wantedPosition) { - double smartSetPoint = m_rotationLimit.getSetPoint(getPosition().getRadians(), wantedPosition.getRadians()); - double pid = m_anglePIDcontroller.calculate(m_POSITION_SUPPLIER.getAsDouble(), smartSetPoint); -// double ff =m_angleFFcontroller.getKs() * Math.signum(pid); - super.setVoltage(pid); + public Command setDynamicPositionCommand(DoubleSupplier position, SubsystemBase... requirements){ + return new RunCommand(() -> { + TrapezoidProfile.State state = m_profile.calculate( + 0.02, + new TrapezoidProfile.State( + super.logMechanismPosition(), + super.logMechanismVelocity()), + new TrapezoidProfile.State(position.getAsDouble(), 0) + ); + setPosition(state.position); + }, requirements); } - /** - * @return get the position if the turret - */ - public Rotation2d getPosition() { - return new Rotation2d(m_POSITION_SUPPLIER.getAsDouble()); - } - - /** - * @return an Instant Command to stop the turret - */ - public Command stopTurret(SubsystemBase... requirements) { - return new InstantCommand(super.m_motor::stopMotor, requirements); + /** + * moves the turret to the desired position + * @param setpoint the wanted position of the turret. + */ + public void setPosition(double setpoint) { + double limitedSetpoint = m_rotationLimit.getSetPoint(super.logMechanismPosition(), setpoint); + double pid = m_pidController.calculate(super.logMechanismPosition(), limitedSetpoint); + double ff = m_ffController.getKs() * Math.signum(pid); + super.setVoltage(pid + ff); } } diff --git a/src/main/java/frc/excalib/slam/mapper/AuroraClient.java b/src/main/java/frc/excalib/slam/mapper/AuroraClient.java deleted file mode 100644 index d9457bc..0000000 --- a/src/main/java/frc/excalib/slam/mapper/AuroraClient.java +++ /dev/null @@ -1,110 +0,0 @@ -package frc.excalib.slam.mapper; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.wpilibj.DriverStation; - -import java.io.DataInputStream; -import java.io.IOException; -import java.net.ServerSocket; -import java.net.Socket; - -public class AuroraClient { - private ServerSocket serverSocket; - private Thread serverThread; - private volatile boolean running = true; - - // Pose Data (volatile for thread safety) - private volatile float x = 0; - private volatile float y = 0; - private volatile float z = 0; - private volatile float roll = 0; - private volatile float pitch = 0; - private volatile float yaw = 0; - - public AuroraClient(int port) { - serverThread = new Thread(() -> { - try { - serverSocket = new ServerSocket(port); - DriverStation.reportWarning("Localization server started on port " + port, false); - - while (running) { - try (Socket clientSocket = serverSocket.accept(); - DataInputStream in = new DataInputStream(clientSocket.getInputStream())) { - - DriverStation.reportWarning("Localization client connected!", false); - - while (running) { - try { - x = in.readFloat(); - y = in.readFloat(); - z = in.readFloat(); - roll = in.readFloat(); - pitch = in.readFloat(); - yaw = in.readFloat(); - } catch (IOException e) { - DriverStation.reportError("Error reading localization data: " + e.getMessage(), false); - break; - } - } - } catch (IOException e) { - DriverStation.reportError("Client connection error: " + e.getMessage(), false); - } - } - } catch (IOException e) { - DriverStation.reportError("Localization server error: " + e.getMessage(), false); - } - }); - - serverThread.setDaemon(true); - serverThread.start(); - } - - // Getter methods for retrieving pose data - public float getX() { - return x; - } - - public float getY() { - return y; - } - - public float getZ() { - return z; - } - - public float getRoll() { - return roll; - } - - public float getPitch() { - return pitch; - } - - public float getYaw() { - return yaw; - } - - public Pose3d getPose3d() { - return new Pose3d(x, y, z, new Rotation3d(roll, pitch, yaw)); - } - - public Pose2d getPose2d() { - return new Pose2d(x, y, new Rotation2d(yaw)); - } - - // Stops the server - public void stop() { - running = false; - try { - if (serverSocket != null) { - serverSocket.close(); - } - } catch (IOException e) { - DriverStation.reportError("Failed to close localization server: " + e.getMessage(), false); - } - } -} - diff --git a/src/main/java/frc/excalib/slam/mapper/Odometry.java b/src/main/java/frc/excalib/slam/mapper/Odometry.java deleted file mode 100644 index 72e0427..0000000 --- a/src/main/java/frc/excalib/slam/mapper/Odometry.java +++ /dev/null @@ -1,39 +0,0 @@ -package frc.excalib.slam.mapper; - -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; - -import java.util.function.Supplier; - -public class Odometry extends SwerveDrivePoseEstimator { - private final Supplier m_YAW_SUPPLIER; - private Pose2d m_robotPose; - - public Odometry( - SwerveDriveKinematics swerveDrive, - SwerveModulePosition[] modulesPositions, - Supplier angleSupplier, - Pose2d initialPose) { - super(swerveDrive, angleSupplier.get(), modulesPositions, initialPose); - m_YAW_SUPPLIER = angleSupplier; - m_robotPose = initialPose; - } - - public Pose2d getRobotPose() { - return m_robotPose; - } - - public void updateOdometry(SwerveModulePosition[] modulesPositions) { - m_robotPose = super.update( - m_YAW_SUPPLIER.get(), - modulesPositions - ); - } - - public void resetOdometry(SwerveModulePosition[] modulesPositions, Pose2d newInitialPose) { - super.resetPosition(m_YAW_SUPPLIER.get(), modulesPositions, newInitialPose); - } -} diff --git a/src/main/java/frc/excalib/slam/mapper/PhotonAprilTagsCamera.java b/src/main/java/frc/excalib/slam/mapper/PhotonAprilTagsCamera.java deleted file mode 100644 index ecbea3e..0000000 --- a/src/main/java/frc/excalib/slam/mapper/PhotonAprilTagsCamera.java +++ /dev/null @@ -1,91 +0,0 @@ -package frc.excalib.slam.mapper; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import monologue.Logged; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.targeting.PhotonPipelineResult; - -import java.util.List; -import java.util.Optional; -import java.util.function.BiConsumer; - -import static org.photonvision.PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY; -import static org.photonvision.PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR; - -public class PhotonAprilTagsCamera implements Logged { - private final PhotonCamera camera; - private final AprilTagFieldLayout fieldLayout; - private final PhotonPoseEstimator photonPoseEstimator; - - public PhotonAprilTagsCamera(String cameraName, AprilTagFields aprilTagField, Transform3d robotToCamera) { - camera = new PhotonCamera(cameraName); - camera.setDriverMode(false); - - fieldLayout = AprilTagFieldLayout.loadField(aprilTagField); - - photonPoseEstimator = new PhotonPoseEstimator(fieldLayout, MULTI_TAG_PNP_ON_COPROCESSOR, robotToCamera); - photonPoseEstimator.setMultiTagFallbackStrategy(LOWEST_AMBIGUITY); - } - - public PhotonCamera getCamera() { - return camera; - } - - public void setDriverMode(boolean isDriverMode) { - camera.setDriverMode(isDriverMode); - } - - public void setPipeline(int index) { - camera.setPipelineIndex(index); - } - - public PhotonPipelineResult getFirstLatestResult() { - var result = camera.getAllUnreadResults(); - - if (result != null) return result.getFirst(); - return new PhotonPipelineResult(); - } - - public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) { - List unreadResults = camera.getAllUnreadResults(); - - for (PhotonPipelineResult result : unreadResults) - if (result.hasTargets() && result.getBestTarget().getPoseAmbiguity() < 0.2) { - Translation2d targetTranslation = result.getBestTarget().getBestCameraToTarget().getTranslation().toTranslation2d(); - // m - double TOO_FAR = 3.5; - if (targetTranslation.getDistance(new Translation2d(0, 0)) < TOO_FAR) { - photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); - return photonPoseEstimator.update(result); - } - } - - return Optional.empty(); - } - - public boolean updateFromAprilTagPose(BiConsumer toUpdate) { - var unreadResults = camera.getAllUnreadResults(); - for (PhotonPipelineResult result : unreadResults) - if (!result.hasTargets()) return false; - - for (PhotonPipelineResult result : unreadResults) { - var id = result.getBestTarget().getFiducialId(); - if (id == -1) return false; - var tag = fieldLayout.getTagPose(id); - if (tag.isEmpty()) return false; - toUpdate.accept( - tag.get().plus( - result.getBestTarget().getBestCameraToTarget()).toPose2d(), - result.getTimestampSeconds() - ); - } - return true; - } -} - diff --git a/src/main/java/frc/excalib/slam/mapper/PoseEstimator.java b/src/main/java/frc/excalib/slam/mapper/PoseEstimator.java deleted file mode 100644 index 03e377d..0000000 --- a/src/main/java/frc/excalib/slam/mapper/PoseEstimator.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.excalib.slam.mapper; - -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; - -import java.util.function.Supplier; - -public class PoseEstimator extends SwerveDrivePoseEstimator { - private final Supplier m_YAW_SUPPLIER; - private Pose2d m_robotPose; - - public PoseEstimator(SwerveDriveKinematics kinematics, Supplier yawSupplier, SwerveModulePosition[] modulePositions, Pose2d initialPose) { - super(kinematics, yawSupplier.get(), modulePositions, initialPose); - m_YAW_SUPPLIER = yawSupplier; - } - - -} diff --git a/src/main/java/frc/excalib/swerve/ModulesHolder.java b/src/main/java/frc/excalib/swerve/ModulesHolder.java deleted file mode 100644 index 2cf7661..0000000 --- a/src/main/java/frc/excalib/swerve/ModulesHolder.java +++ /dev/null @@ -1,222 +0,0 @@ -package frc.excalib.swerve; - -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.excalib.control.math.Vector2D; -import monologue.Logged; - -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -import static frc.robot.SwerveConstants.MAX_VEL; -import static monologue.Annotations.Log; - -public class ModulesHolder implements Logged { - public final SwerveModule m_frontLeft; - public final SwerveModule m_frontRight; - public final SwerveModule m_backLeft; - public final SwerveModule m_backRight; - - private final SwerveDriveKinematics m_swerveDriveKinematics; - - private final SwerveModulePosition[] m_modulePositions; - - /** - * A constructor that initialize the ModulesHolder. - * - * @param frontLeft A SwerveModule represents the front-left module. - * @param frontRight A SwerveModule represents the front-right module. - * @param backLeft A SwerveModule represents the back-left module. - * @param backRight A SwerveModule represents the back-right module. - */ - public ModulesHolder( - SwerveModule frontLeft, - SwerveModule frontRight, - SwerveModule backLeft, - SwerveModule backRight) { - this.m_frontLeft = frontLeft; - this.m_frontRight = frontRight; - this.m_backLeft = backLeft; - this.m_backRight = backRight; - - // Initialize SwerveDriveKinematics once since module locations are constant - this.m_swerveDriveKinematics = new SwerveDriveKinematics( - frontLeft.m_MODULE_LOCATION, - frontRight.m_MODULE_LOCATION, - backLeft.m_MODULE_LOCATION, - backRight.m_MODULE_LOCATION - ); - - m_modulePositions = new SwerveModulePosition[]{ - m_frontLeft.getModulePosition(), - m_frontRight.getModulePosition(), - m_backLeft.getModulePosition(), - m_backRight.getModulePosition() - }; - } - - /** - * Stops all swerve modules. - */ - public void stop() { - m_frontLeft.stopModule(); - m_frontRight.stopModule(); - m_backLeft.stopModule(); - m_backRight.stopModule(); - } - - /** - * Gets the robot's average velocity based on the velocities of all modules. - * - * @return a Vector2D representing the robot's velocity. - */ - public Vector2D getVelocity() { - // Sum the velocities of all modules - double totalX = m_frontLeft.getVelocity().getX() - + m_frontRight.getVelocity().getX() - + m_backLeft.getVelocity().getX() - + m_backRight.getVelocity().getX(); - - double totalY = m_frontLeft.getVelocity().getY() - + m_frontRight.getVelocity().getY() - + m_backLeft.getVelocity().getY() - + m_backRight.getVelocity().getY(); - - // Compute the average velocity - return new Vector2D(totalX * 0.25, totalY * 0.25); - } - - @Log.NT(key = "angular vel") - public double getOmegaRadPerSec() { - return new SwerveDriveKinematics( - m_frontLeft.m_MODULE_LOCATION, - m_frontRight.m_MODULE_LOCATION, - m_backLeft.m_MODULE_LOCATION, - m_backRight.m_MODULE_LOCATION - ).toChassisSpeeds(logStates()).omegaRadiansPerSecond; - } - - @Log.NT(key = "swerve velocity") - public double getVelocityDistance() { - return getVelocity().getDistance(); - } - - /** - * Calculates the minimum velocity ratio limit among all modules. - * - * @param translationVelocity The desired translation velocity. - * @param omegaRadPerSec The desired rotation rate in radians per second. - * @return The velocity ratio limit. - */ - private double calcVelocityRatioLimit(Vector2D translationVelocity, double omegaRadPerSec) { - double flLimit = m_frontLeft.getVelocityRatioLimit(translationVelocity, omegaRadPerSec); - double frLimit = m_frontRight.getVelocityRatioLimit(translationVelocity, omegaRadPerSec); - double blLimit = m_backLeft.getVelocityRatioLimit(translationVelocity, omegaRadPerSec); - double brLimit = m_backRight.getVelocityRatioLimit(translationVelocity, omegaRadPerSec); - - double velocityRatioLimit = Math.min(Math.min(flLimit, frLimit), Math.min(blLimit, brLimit)); - return Math.min(1.0, velocityRatioLimit); // Ensure the limit does not exceed 1.0 - } - - /** - * Sets the velocities of all modules based on the desired translation and rotation velocities. - * - * @param omega The desired rotation rate supplier. - * @param translationalVel The desired translation velocity supplier. - * @return A command to set the velocities. - */ - public Command setVelocitiesCommand(Supplier translationalVel, DoubleSupplier omega) { - return new ParallelCommandGroup( - m_frontLeft.setVelocityCommand( - translationalVel, - omega, - () -> calcVelocityRatioLimit(translationalVel.get(), omega.getAsDouble()) - ), - m_frontRight.setVelocityCommand( - translationalVel, - omega, - () -> calcVelocityRatioLimit(translationalVel.get(), omega.getAsDouble()) - ), - m_backLeft.setVelocityCommand( - translationalVel, - omega, - () -> calcVelocityRatioLimit(translationalVel.get(), omega.getAsDouble()) - ), - m_backRight.setVelocityCommand( - translationalVel, - omega, - () -> calcVelocityRatioLimit(translationalVel.get(), omega.getAsDouble()) - ) - ); - } - - public Command coastCommand() { - return new ParallelCommandGroup( - m_frontLeft.coastCommand(), - m_frontRight.coastCommand(), - m_backLeft.coastCommand(), - m_backRight.coastCommand() - ); - } - - public void setModulesStates(SwerveModuleState[] states) { - SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_VEL); - m_frontLeft.setDesiredState(states[0]); - m_frontRight.setDesiredState(states[1]); - m_backLeft.setDesiredState(states[2]); - m_backRight.setDesiredState(states[3]); - } - - /** - * Logs the states of all modules. - * - * @return An array of SwerveModuleState representing the states of the modules. - */ - @Log.NT(key = "Swerve States") - public SwerveModuleState[] logStates() { - return new SwerveModuleState[]{ - m_frontLeft.logState(), - m_frontRight.logState(), - m_backLeft.logState(), - m_backRight.logState() - }; - } - - @Log.NT(key = "SetPoints") - public SwerveModuleState[] logSetPointStates() { - return new SwerveModuleState[]{ - m_frontLeft.logSetpointState(), - m_frontRight.logSetpointState(), - m_backLeft.logSetpointState(), - m_backRight.logSetpointState() - }; - } - - /** - * Gets the swerve drive kinematics. - * - * @return The SwerveDriveKinematics instance. - */ - public SwerveDriveKinematics getSwerveDriveKinematics() { - return m_swerveDriveKinematics; - } - - /** - * Gets the positions of all modules. - * - * @return An array of SwerveModulePosition representing the positions of the modules. - */ - public SwerveModulePosition[] getModulesPositions() { - return m_modulePositions; - } - - public void periodic() { - m_frontLeft.periodic(); - m_frontRight.periodic(); - m_backLeft.periodic(); - m_backRight.periodic(); - } -} diff --git a/src/main/java/frc/excalib/swerve/Swerve.java b/src/main/java/frc/excalib/swerve/Swerve.java deleted file mode 100644 index 3779bf5..0000000 --- a/src/main/java/frc/excalib/swerve/Swerve.java +++ /dev/null @@ -1,501 +0,0 @@ -package frc.excalib.swerve; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.path.PathPlannerPath; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj2.command.*; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import frc.excalib.additional_utilities.AllianceUtils; -import frc.excalib.additional_utilities.Elastic; -import frc.excalib.control.gains.SysidConfig; -import frc.excalib.control.imu.IMU; -import frc.excalib.control.math.Vector2D; -import frc.excalib.control.motor.controllers.SparkMaxMotor; -import frc.excalib.control.motor.controllers.TalonFXMotor; -import frc.excalib.slam.mapper.Odometry; -import frc.excalib.slam.mapper.PhotonAprilTagsCamera; -import monologue.Logged; -import org.json.simple.parser.ParseException; -import org.photonvision.EstimatedRobotPose; - -import java.io.IOException; -import java.util.Optional; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -import static com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless; -import static edu.wpi.first.apriltag.AprilTagFields.*; -import static frc.excalib.additional_utilities.Elastic.Notification.NotificationLevel.WARNING; -import static frc.robot.SwerveConstants.*; -import static monologue.Annotations.Log; - -/** - * A class representing a swerve subsystem. - */ -public class Swerve extends SubsystemBase implements Logged { - private final ModulesHolder modules; - private final IMU imu; - private final Odometry odometry; - PhotonAprilTagsCamera exampleCamera; - private ChassisSpeeds desiredChassisSpeeds = new ChassisSpeeds(); - private final Trigger finishTrigger; - private final Rotation2d PI = new Rotation2d(Math.PI); - private final InterpolatingDoubleTreeMap velocityLimit = new InterpolatingDoubleTreeMap(); - - private final SwerveDriveKinematics swerveDriveKinematics; - - private final PIDController angleController = new PIDController(ANGLE_GAINS.kp, ANGLE_GAINS.ki, ANGLE_GAINS.kd); - private final PIDController xController = new PIDController(TRANSLATION_GAINS.kp, TRANSLATION_GAINS.ki, TRANSLATION_GAINS.kd); - private final PIDController yController = new PIDController(TRANSLATION_GAINS.kp, TRANSLATION_GAINS.ki, TRANSLATION_GAINS.kd); - public final Field2d field = new Field2d(); - private Supplier angleSetpoint = Rotation2d::new; - private Supplier translationSetpoint = Translation2d::new; - - /** - * A constructor that initialize the Swerve Subsystem - * - * @param modules The ModulesHolder containing all swerve modules. - * @param imu IMU sensor. - * @param initialPosition The initial position of the robot. - */ - public Swerve(ModulesHolder modules, IMU imu, Pose2d initialPosition) { - this.modules = modules; - this.imu = imu; - this.imu.resetIMU(); - - angleController.enableContinuousInput(-Math.PI, Math.PI); - angleController.setTolerance(ANGLE_CONTROLLER_TOLORANCE); - xController.setTolerance(X_CONTROLLER_TOLORANCE); - yController.setTolerance(Y_CONTROLLER_TOLORANCE); - - finishTrigger = new Trigger(xController::atSetpoint).and(yController::atSetpoint).and(angleController::atSetpoint).debounce(0.1); - this.odometry = new Odometry(modules.getSwerveDriveKinematics(), modules.getModulesPositions(), this.imu::getZRotation, initialPosition); - PhotonAprilTagsCamera m_exampleCamera = new PhotonAprilTagsCamera("example", k2025ReefscapeWelded, new Transform3d(0, 0, 0, new Rotation3d())); - - swerveDriveKinematics = this.modules.getSwerveDriveKinematics(); - velocityLimit.put(0.1, 0.4); - velocityLimit.put(0.7, 2.0); - velocityLimit.put(1.5, MAX_VEL); - - initAutoBuilder(); - } - - /** - * Creates a drive command for the swerve drive. - * - * @param velocityMPS Supplier for the desired linear velocity in meters per second. - * @param omegaRadPerSec Supplier for the desired rotational velocity in radians per second. - * @param fieldOriented Supplier indicating whether the control is field-oriented. - * @return A command that drives the robot. - */ - public Command driveCommand( - Supplier velocityMPS, - DoubleSupplier omegaRadPerSec, - BooleanSupplier fieldOriented) { - - // Precompute values to avoid redundant calculations - Supplier adjustedVelocitySupplier = () -> { - Vector2D velocity = velocityMPS.get(); -// Vector2D velocity = getSmartTranslationalVelocitySetPoint(getVelocity(), velocityMPS.get()); - if (fieldOriented.getAsBoolean()) { - Rotation2d yaw = getRotation2D().unaryMinus(); - if (!AllianceUtils.isBlueAlliance()) yaw = yaw.plus(PI); - return velocity.rotate(yaw); - } - return velocity; - }; - - Command driveCommand = new ParallelCommandGroup(modules.setVelocitiesCommand( - adjustedVelocitySupplier, - omegaRadPerSec - ), - new RunCommand( - () -> desiredChassisSpeeds = new ChassisSpeeds( - adjustedVelocitySupplier.get().getX(), - adjustedVelocitySupplier.get().getY(), - omegaRadPerSec.getAsDouble()) - ) - ); - driveCommand.setName("Drive Command"); - driveCommand.addRequirements(this); - return driveCommand; - } - - /** - * A non-command function that drives the robot (for pathplanner). - * - * @param speeds A ChassisSpeeds object represents ROBOT RELATIVE speeds desired speeds. - */ - public void driveRobotRelativeChassisSpeeds(ChassisSpeeds speeds) { - modules.setModulesStates(swerveDriveKinematics.toSwerveModuleStates(speeds)); - } - - /** - * A method that turns the robot to a desired angle. - * - * @param angleSetpoint The desired angle in radians. - * @return A command that turns the robot to the wanted angle. - */ - public Command turnToAngleCommand(Supplier velocityMPS, Supplier angleSetpoint) { - return new SequentialCommandGroup( - new InstantCommand(() -> this.angleSetpoint = angleSetpoint), - driveCommand( - velocityMPS, - () -> angleController.calculate(getRotation2D().getRadians(), angleSetpoint.get().getRadians()), - () -> true - ) - ).withName("Turn To Angle"); - } - - public Command pidToPoseCommand(Supplier poseSetpoint) { - return new SequentialCommandGroup( - new InstantCommand( - () -> { - xController.calculate(getPose2D().getX(), poseSetpoint.get().getX()); - yController.calculate(getPose2D().getY(), poseSetpoint.get().getY()); - angleController.calculate(getRotation2D().getRadians(), poseSetpoint.get().getRotation().getRadians()); - translationSetpoint = () -> poseSetpoint.get().getTranslation(); - angleSetpoint = () -> poseSetpoint.get().getRotation(); - } - ), - driveCommand( - () -> { - Vector2D vel = new Vector2D( - xController.calculate(getPose2D().getX(), poseSetpoint.get().getX()), - yController.calculate(getPose2D().getY(), poseSetpoint.get().getY()) - ); - double distance = getPose2D().getTranslation().getDistance(poseSetpoint.get().getTranslation()); - vel.setMagnitude(Math.min(vel.getDistance(), velocityLimit.get(distance))); -// vel = vel.rotate(poseSetpoint.get().getRotation()); -// vel.setX(Math.signum(vel.getX()) * Math.min(Math.abs(vel.getX()), 1.2)); -// vel.setY(Math.signum(vel.getY()) * Math.min(Math.abs(vel.getY()), 0.4)); -// vel = vel.rotate(poseSetpoint.get().getRotation().unaryMinus()); - if (!AllianceUtils.isBlueAlliance()) return vel.rotate(PI); - return vel; - }, - () -> angleController.calculate(getRotation2D().getRadians(), poseSetpoint.get().getRotation().getRadians()), - () -> true - ) - ).until(finishTrigger).withName("PID To Pose"); - } - - /** - * A method that drives the robot to a desired pose. - * - * @param setPoint The desired pose. - * @return A command that drives the robot to the wanted pose. - */ - public Command driveToPoseCommand(Pose2d setPoint) { - return AutoBuilder.pathfindToPose( - setPoint, - MAX_PATH_CONSTRAINTS - ).withName("Pathfinding Command"); - } - - public Command driveToPoseWithOverrideCommand( - Pose2d setPoint, - BooleanSupplier override, - Supplier velocityMPS, - DoubleSupplier omegaRadPerSec) { - Command driveToPoseCommand = driveToPoseCommand(setPoint); - return new SequentialCommandGroup( - driveToPoseCommand.until(() -> velocityMPS.get().getDistance() != 0 && override.getAsBoolean()), - driveCommand( - velocityMPS, - omegaRadPerSec, - () -> true - ).until(() -> velocityMPS.get().getDistance() == 0) - ).repeatedly().until(driveToPoseCommand::isFinished).withName("Pathfinding With Override Command"); - } - - /** - * A method that drives the robot to the starting pose of a path, then follows the path. - * - * @param pathName The path which the robot needs to follow. - * @return A command that turns the robot to the wanted angle. - */ - public Command pathfindThenFollowPathCommand(String pathName) { - PathPlannerPath path; - try { - path = PathPlannerPath.fromPathFile(pathName); - } catch (IOException | ParseException e) { - Elastic.sendNotification(new Elastic.Notification( - WARNING, - "Path Creating Error", - "the path file " + pathName + " doesn't exist") - ); - return new PrintCommand("this path file doesn't exist"); - } - - return AutoBuilder.pathfindThenFollowPath( - path, - MAX_PATH_CONSTRAINTS - ); - } - - public Command resetAngleCommand() { - return new InstantCommand(imu::resetIMU).ignoringDisable(true); - } - - public Command coastCommand() { - Command coastCommand = modules.coastCommand().ignoringDisable(true).withName("Coast Command"); - coastCommand.addRequirements(this); - return coastCommand; - } - - /** - * Updates the robot's odometry. - */ - public void updateOdometry() { - odometry.updateOdometry(modules.getModulesPositions()); - Optional backPose = exampleCamera.getEstimatedGlobalPose(odometry.getEstimatedPosition()); - backPose.ifPresent( - estimatedRobotPose -> odometry.addVisionMeasurement( - estimatedRobotPose.estimatedPose.toPose2d(), - estimatedRobotPose.timestampSeconds - ) - ); - } - - /** - * A method that restarts the odometry. - * - * @param newPose the wanted new Pose2d of the robot. - */ - public void resetOdometry(Pose2d newPose) { - odometry.resetOdometry(modules.getModulesPositions(), newPose); - } - - /** - * Gets the robot's rotation. - * - * @return The current rotation of the robot. - */ - @Log.NT(key = "Robot Rotation") - public Rotation2d getRotation2D() { - return getPose2D().getRotation(); - } - - @Log.NT(key = "Angle Setpoint") - public Rotation2d getAngleSetpoint() { - return angleSetpoint.get(); - } - - @Log.NT(key = "Translation Setpoint") - public Translation2d getTranslationSetpoint() { - return translationSetpoint.get(); - } - - /** - * Gets the robot's pose. - * - * @return The current pose of the robot. - */ - @Log.NT(key = "Robot Pose") - public Pose2d getPose2D() { - return odometry.getRobotPose(); - } - - /** - * Gets the current velocity of the robot. - * - * @return The robot's velocity as a Vector2D. - */ - public Vector2D getVelocity() { - return modules.getVelocity(); - } - - /** - * Gets the current acceleration of the robot. - * - * @return The robot's acceleration as a Vector2D. - */ - @Log.NT(key = "Acceleration") - public double getAccelerationDistance() { - return new Vector2D(imu.getAccX(), imu.getAccY()).getDistance(); - } - - /** - * Gets the current robot relative speed. - * - * @return The robot's speed as a ChassisSpeeds. - */ - @Log.NT(key = "Measured Chassis Speeds") - public ChassisSpeeds getRobotRelativeSpeeds() { - return swerveDriveKinematics.toChassisSpeeds(modules.logStates()); - } - - @Log.NT - public ChassisSpeeds getDesiredChassisSpeeds() { - return desiredChassisSpeeds; - } - - public Command stopCommand() { - return driveCommand(() -> new Vector2D(0, 0), () -> 0, () -> true); - } - - /** - * A function that initialize the AutoBuilder for pathplanner. - */ - private void initAutoBuilder() { - // Load the RobotConfig from the GUI settings. You should probably - // store this in your Constants file - RobotConfig config = null; - try { - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - } - - // Configure AutoBuilder last - assert config != null; - AutoBuilder.configure( - this::getPose2D, // Robot pose supplier - this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose) - this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> driveRobotRelativeChassisSpeeds(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains - TRANSLATION_PID_CONSTANTS, // Translation PID constants - ANGLE_PID_CONSTANTS // Rotation PID constants - ), - config, // The robot configuration - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - return alliance.filter(value -> value == DriverStation.Alliance.Red).isPresent(); - }, - this // Reference to this subsystem to set requirements - ); - } - - - /** - * Runs a system identification routine on a specific module's angle. - * - * @param module The module index (0-3). - * @param dir The direction of the sysid routine. - * @param dynamic Whether to perform a dynamic or quasistatic test. - * @return The command to perform the sysid routine. - */ - public Command driveSysId(int module, Direction dir, SysidConfig sysidConfig, boolean dynamic) { - SwerveModule selectedModule; - - switch (module) { - case 0 -> selectedModule = modules.m_frontLeft; - case 1 -> selectedModule = modules.m_frontRight; - case 2 -> selectedModule = modules.m_backLeft; - case 3 -> selectedModule = modules.m_backRight; - default -> { - throw new IllegalArgumentException("Invalid module index: " + module); - } - } - return dynamic ? - selectedModule.driveSysIdDynamic(dir, this, sysidConfig) - : selectedModule.driveSysIdQuas(dir, this, sysidConfig); - } - - /** - * Runs a system identification routine on a specific module's angle. - * - * @param module The module index (0-3). - * @param dir The direction of the sysid routine. - * @param dynamic Whether to perform a dynamic or quasistatic test. - * @return The command to perform the sysid routine. - */ - public Command angleSysId(int module, Direction dir, SysidConfig sysidConfig, boolean dynamic) { - SwerveModule selectedModule; - - switch (module) { - case 0 -> selectedModule = modules.m_frontLeft; - case 1 -> selectedModule = modules.m_frontRight; - case 2 -> selectedModule = modules.m_backLeft; - case 3 -> selectedModule = modules.m_backRight; - default -> { - throw new IllegalArgumentException("Invalid module index: " + module); - } - } - return dynamic ? - selectedModule.angleSysIdDynamic(dir, this, sysidConfig) - : selectedModule.angleSysIdQuas(dir, this, sysidConfig); - } - - public static Swerve configureSwerve(Pose2d initialPose) { - return new Swerve( - new ModulesHolder( - new SwerveModule( - new TalonFXMotor(FRONT_LEFT_DRIVE_ID, SWERVE_CANBUS), - new SparkMaxMotor(FRONT_LEFT_ROTATION_ID, kBrushless), - SWERVE_DRIVE_MODULE_GAINS, - SWERVE_ROTATION_MODULE_GAINS, - PID_TOLERANCE, - FRONT_LEFT_TRANSLATION, - () -> FRONT_LEFT_ABS_ENCODER.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI, - MAX_MODULE_VEL, - VELOCITY_CONVERSION_FACTOR, - POSITION_CONVERSION_FACTOR, - ROTATION_VELOCITY_CONVERSION_FACTOR - ), - new SwerveModule( - new TalonFXMotor(FRONT_RIGHT_DRIVE_ID, SWERVE_CANBUS), - new SparkMaxMotor(FRONT_RIGHT_ROTATION_ID, kBrushless), - SWERVE_DRIVE_MODULE_GAINS, - SWERVE_ROTATION_MODULE_GAINS, - PID_TOLERANCE, - FRONT_RIGHT_TRANSLATION, - () -> FRONT_RIGHT_ABS_ENCODER.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI, - MAX_MODULE_VEL, - VELOCITY_CONVERSION_FACTOR, - POSITION_CONVERSION_FACTOR, - ROTATION_VELOCITY_CONVERSION_FACTOR - ), - new SwerveModule( - new TalonFXMotor(BACK_LEFT_DRIVE_ID, SWERVE_CANBUS), - new SparkMaxMotor(BACK_LEFT_ROTATION_ID, kBrushless), - SWERVE_DRIVE_MODULE_GAINS, - SWERVE_ROTATION_MODULE_GAINS, - PID_TOLERANCE, - BACK_LEFT_TRANSLATION, - () -> BACK_LEFT_ABS_ENCODER.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI, - MAX_MODULE_VEL, - VELOCITY_CONVERSION_FACTOR, - POSITION_CONVERSION_FACTOR, - ROTATION_VELOCITY_CONVERSION_FACTOR - ), - new SwerveModule( - new TalonFXMotor(BACK_RIGHT_DRIVE_ID, SWERVE_CANBUS), - new SparkMaxMotor(BACK_RIGHT_ROTATION_ID, kBrushless), - SWERVE_DRIVE_MODULE_GAINS, - SWERVE_ROTATION_MODULE_GAINS, - PID_TOLERANCE, - BACK_RIGHT_TRANSLATION, - () -> BACK_RIGHT_ABS_ENCODER.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI, - MAX_MODULE_VEL, - VELOCITY_CONVERSION_FACTOR, - POSITION_CONVERSION_FACTOR, - ROTATION_VELOCITY_CONVERSION_FACTOR - )), - GYRO, - initialPose - ); - } - - @Override - public void periodic() { - modules.periodic(); - field.setRobotPose(getPose2D()); - } -} diff --git a/src/main/java/frc/excalib/swerve/SwerveAccUtils.java b/src/main/java/frc/excalib/swerve/SwerveAccUtils.java deleted file mode 100644 index a3c2e5a..0000000 --- a/src/main/java/frc/excalib/swerve/SwerveAccUtils.java +++ /dev/null @@ -1,79 +0,0 @@ -package frc.excalib.swerve; - -import frc.excalib.control.math.Vector2D; - -import static frc.excalib.control.math.MathUtils.minSize; -import static frc.robot.SwerveConstants.*; - -public class SwerveAccUtils { - private static final double CYCLE_TIME = 0.02; - - /** - * A function to get the translational velocity setpoint - * - * @param velocitySetPoint wanted velocity setpoint - * @return translational velocity setpoint - */ - public static Vector2D getSmartTranslationalVelocitySetPoint(Vector2D currentVel, Vector2D velocitySetPoint) { - Vector2D deltaVelocity = velocitySetPoint.plus( - currentVel.mul(-1)); - Vector2D actualDeltaVelocity = applyAccelerationLimits(currentVel, deltaVelocity); - return currentVel.plus(actualDeltaVelocity); - } - - /** - * A function to apply the acceleration limits - * - * @param velocityError wanted velocity - * @return velocity limited by acceleration limits - */ - private static Vector2D applyAccelerationLimits(Vector2D currentVel, Vector2D velocityError) { - Vector2D wantedAcceleration = velocityError.mul(1 / CYCLE_TIME); - -// wantedAcceleration = applyForwardLimit(currentVel, wantedAcceleration); -// wantedAcceleration = applyTiltLimit(wantedAcceleration); - wantedAcceleration = applySkidLimit(wantedAcceleration); - - return wantedAcceleration.mul(CYCLE_TIME); - } - - /** - * A function to apply the forward acceleration limit - * - * @param wantedAcceleration the wanted acceleration - * @return wanted acceleration limited by forward acceleration limit - */ - private static Vector2D applyForwardLimit(Vector2D currentVel, Vector2D wantedAcceleration) { - double maxAcceleration = - MAX_FORWARD_ACC * - (1 - (currentVel.getDistance() / MAX_VEL)); - - wantedAcceleration = wantedAcceleration.limit(new Vector2D(maxAcceleration, currentVel.getDirection())); - return wantedAcceleration; - } - - /** - * A function to apply the tilt acceleration limit - * - * @param wantedAcceleration the wanted acceleration - * @return wanted acceleration limited by tilt acceleration limit - */ - private static Vector2D applyTiltLimit(Vector2D wantedAcceleration) { - double frontAcceleration = minSize(wantedAcceleration.getX(), MAX_FRONT_ACC); - double sideAcceleration = minSize(wantedAcceleration.getY(), MAX_SIDE_ACC); - return new Vector2D(frontAcceleration, sideAcceleration); - } - - /** - * A function to apply the skid acceleration limit - * - * @param wantedAcceleration the wanted acceleration - * @return wanted acceleration limited by skid acceleration limit - */ - private static Vector2D applySkidLimit(Vector2D wantedAcceleration) { - return new Vector2D( - Math.min(wantedAcceleration.getDistance(), MAX_SKID_ACC), - wantedAcceleration.getDirection() - ); - } -} diff --git a/src/main/java/frc/excalib/swerve/SwerveModule.java b/src/main/java/frc/excalib/swerve/SwerveModule.java deleted file mode 100644 index cb1efd4..0000000 --- a/src/main/java/frc/excalib/swerve/SwerveModule.java +++ /dev/null @@ -1,231 +0,0 @@ -package frc.excalib.swerve; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.excalib.control.gains.Gains; -import frc.excalib.control.gains.SysidConfig; -import frc.excalib.control.limits.ContinuousSoftLimit; -import frc.excalib.control.math.Vector2D; -import frc.excalib.control.motor.Motor; -import frc.excalib.mechanisms.fly_wheel.FlyWheel; -import frc.excalib.mechanisms.turret.Turret; -import monologue.Logged; - -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -import static frc.excalib.control.motor.motor_specs.DirectionState.FORWARD; -import static frc.excalib.control.motor.motor_specs.DirectionState.REVERSE; -import static frc.excalib.control.motor.motor_specs.IdleState.BRAKE; - -/** - * A class representing a swerve module - * - * @author Yoav Cohen & Itay Keller - */ -public class SwerveModule implements Logged { - private final FlyWheel m_driveWheel; - private final Turret m_turret; - public final Translation2d m_MODULE_LOCATION; - private final double m_MAX_VEL; - private final Rotation2d m_moduleAnglePlus90; - private final Vector2D m_setPoint = new Vector2D(0, 0); - private final SwerveModulePosition m_swerveModulePosition; - - /** - * A constructor for the SwerveModule - */ - public SwerveModule(Motor driveMotor, Motor rotationMotor, Gains angleGains, Gains velocityGains, - double PIDTolerance, Translation2d moduleLocation, DoubleSupplier angleSupplier, - double maxVel, double velocityConversionFactor, double positionConversionFactor, - double rotationVelocityConversionFactor) { - driveMotor.setInverted(FORWARD); - driveMotor.setVelocityConversionFactor(velocityConversionFactor); - driveMotor.setIdleState(BRAKE); - driveMotor.setPositionConversionFactor(positionConversionFactor); - driveMotor.setCurrentLimit(0, 60); - - rotationMotor.setIdleState(BRAKE); - rotationMotor.setVelocityConversionFactor(rotationVelocityConversionFactor); - rotationMotor.setInverted(REVERSE); - - m_driveWheel = new FlyWheel(driveMotor, 10, 10, velocityGains); - - m_turret = new Turret(rotationMotor, new ContinuousSoftLimit(() -> Double.NEGATIVE_INFINITY, () -> Double.POSITIVE_INFINITY), - angleGains, PIDTolerance, angleSupplier); - - m_MODULE_LOCATION = moduleLocation; - m_MAX_VEL = maxVel; - - // Precompute the rotated module angle (module angle + 90 degrees) - m_moduleAnglePlus90 = m_MODULE_LOCATION.getAngle().plus(new Rotation2d(Math.PI / 2)); - - m_swerveModulePosition = new SwerveModulePosition(m_driveWheel.logPosition(), m_turret.getPosition()); - } - - double getVelocityRatioLimit(Vector2D translationVelocity, double omegaRadPerSec) { - Vector2D rotationVector = new Vector2D( - omegaRadPerSec, - m_moduleAnglePlus90 - ); - Vector2D sigmaVel = translationVelocity.plus(rotationVector); - double sigmaVelDistance = sigmaVel.getDistance(); - - // Avoid division by zero - if (sigmaVelDistance == 0) { - return 0; - } - return m_MAX_VEL / sigmaVelDistance; - } - - Vector2D getSigmaVelocity(Vector2D translationVelocity, double omegaRadPerSec, double velocityRatioLimit) { - Vector2D rotationVector = new Vector2D( - omegaRadPerSec, - m_moduleAnglePlus90 - ); - Vector2D sigmaVel = translationVelocity.plus(rotationVector); - sigmaVel = sigmaVel.mul(velocityRatioLimit); - return sigmaVel; - } - - public boolean isOptimizable(Vector2D moduleVelocitySetPoint) { - Rotation2d setPointDirection = moduleVelocitySetPoint.getDirection(); - Rotation2d currentDirection = m_turret.getPosition(); - double deltaDirection = Math.cos(setPointDirection.minus(currentDirection).getRadians()); - - // If the dot product is negative, reversing the wheel direction may be beneficial - return deltaDirection < 0; - } - - public Command setVelocityCommand(Supplier moduleVelocity) { - return new ParallelCommandGroup( - m_driveWheel.setDynamicVelocityCommand(() -> { - Vector2D velocity = moduleVelocity.get(); - double speed = velocity.getDistance(); - - if (speed < 0.1) { - speed = 0; - } - - boolean optimize = isOptimizable(velocity); - return optimize ? -speed : speed; - }), - m_turret.setPositionCommand(() -> { - Vector2D velocity = moduleVelocity.get(); - double speed = velocity.getDistance(); - - if (speed < 0.1) { - return m_turret.getPosition(); - } - - boolean optimize = isOptimizable(velocity); - Rotation2d direction = velocity.getDirection(); - return optimize ? direction.rotateBy(Rotation2d.fromRadians(Math.PI)) : direction; - }), - new RunCommand(() -> { - m_setPoint.setY(moduleVelocity.get().getY()); - m_setPoint.setX(moduleVelocity.get().getX()); - }) - ); - } - - public Command setVelocityCommand( - Supplier translationVelocity, - DoubleSupplier omegaRadPerSec, - DoubleSupplier velocityRatioLimit) { - - return setVelocityCommand(() -> getSigmaVelocity( - translationVelocity.get(), - omegaRadPerSec.getAsDouble(), - velocityRatioLimit.getAsDouble())); - } - - public Command coastCommand() { - return new ParallelCommandGroup( - m_driveWheel.coastCommand(), - m_turret.coastCommand() - ); - } - - public void setDesiredState(SwerveModuleState wantedState) { - Vector2D velocity = new Vector2D(wantedState.speedMetersPerSecond, wantedState.angle); - double speed = velocity.getDistance(); - Rotation2d direction = velocity.getDirection(); - - if (speed < 0.1) { - speed = 0.0; - direction = m_turret.getPosition(); - } - - boolean optimize = isOptimizable(velocity); - - m_driveWheel.setDynamicVelocity(optimize ? -speed : speed); - m_turret.setPosition(optimize ? direction.rotateBy(Rotation2d.fromRadians(Math.PI)) : direction); - } - - /** - * Stops the module by setting the drive wheel output to zero. - */ - void stopModule() { - m_driveWheel.setOutput(0); - } - - /** - * Gets the module's velocity. - * - * @return a Vector2D representing the velocity. - */ - Vector2D getVelocity() { - return new Vector2D(m_driveWheel.getVelocity(), getPosition()); - } - - /** - * Gets the turret's position. - * - * @return the current position of the turret. - */ - Rotation2d getPosition() { - return m_turret.getPosition(); - } - - public SwerveModulePosition getModulePosition() { - return m_swerveModulePosition; - } - - public SwerveModuleState logState() { - Vector2D velocity = getVelocity(); - return new SwerveModuleState(velocity.getDistance(), velocity.getDirection()); - } - - public SwerveModuleState logSetpointState() { - return new SwerveModuleState(m_setPoint.getDistance(), m_setPoint.getDirection()); - } - - - public Command driveSysIdDynamic(SysIdRoutine.Direction direction, Swerve swerve, SysidConfig sysidConfig) { - return m_driveWheel.sysIdDynamic(direction, swerve, m_driveWheel::logPosition, sysidConfig, false); - } - - public Command driveSysIdQuas(SysIdRoutine.Direction direction, Swerve swerve, SysidConfig sysidConfig) { - return m_driveWheel.sysIdQuasistatic(direction, swerve, m_driveWheel::logPosition, sysidConfig, false); - } - - public Command angleSysIdDynamic(SysIdRoutine.Direction direction, Swerve swerve, SysidConfig sysidConfig) { - return m_turret.sysIdDynamic(direction, swerve, m_turret::logPosition, sysidConfig, false); - } - - public Command angleSysIdQuas(SysIdRoutine.Direction direction, Swerve swerve, SysidConfig sysidConfig) { - return m_turret.sysIdQuasistatic(direction, swerve, m_turret::logPosition, sysidConfig, false); - } - - public void periodic() { - m_swerveModulePosition.distanceMeters = m_driveWheel.logPosition(); - m_swerveModulePosition.angle = m_turret.getPosition(); - } -} From 33d129e11911e1042f13f7351e00b8de44baa646 Mon Sep 17 00:00:00 2001 From: TapChap Date: Tue, 20 May 2025 15:36:58 +0300 Subject: [PATCH 08/17] minor changes --- src/main/java/frc/excalib/control/gains/Gains.java | 10 ++++++---- .../excalib/mechanisms/diffrential/Differential.java | 5 ++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/excalib/control/gains/Gains.java b/src/main/java/frc/excalib/control/gains/Gains.java index a706818..2c54088 100644 --- a/src/main/java/frc/excalib/control/gains/Gains.java +++ b/src/main/java/frc/excalib/control/gains/Gains.java @@ -126,9 +126,11 @@ public PIDController getPIDcontroller(){ return new PIDController(this.kp, this.ki, this.kd); } - - public GenericFeedFoward applyGains(GenericFeedFoward feedFoward){ - feedFoward.setValues(this.ks, this.kv, this.ka, this.kg); - return feedFoward; + public GenericFeedFoward applyGains(GenericFeedFoward feedForward){ + feedForward.setKv(this.kv); + feedForward.setKs(this.ks); + feedForward.setKa(this.ka); + feedForward.setKg(this.kg); + return feedForward; } } diff --git a/src/main/java/frc/excalib/mechanisms/diffrential/Differential.java b/src/main/java/frc/excalib/mechanisms/diffrential/Differential.java index 13cbb35..39bf61b 100644 --- a/src/main/java/frc/excalib/mechanisms/diffrential/Differential.java +++ b/src/main/java/frc/excalib/mechanisms/diffrential/Differential.java @@ -33,9 +33,8 @@ public class Differential extends Mechanism { * @param differentialMotor a differential motor object of both mechanism motors * @param gains gains for pid & ff for the two motors * @param differentialMul an optional multiplier to correct acc error in certain types of differential mechanism - * @param tolerance tolerance for the accumulated error from BOTH motors combined */ - public Differential(DifferentialMotor differentialMotor, Gains gains, double differentialMul, double tolerance) { + public Differential(DifferentialMotor differentialMotor, Gains gains, double differentialMul) { super(differentialMotor); this.motor = differentialMotor; @@ -52,7 +51,7 @@ public Differential(DifferentialMotor differentialMotor, Gains gains, double dif * @param differentialMotor */ public Differential(DifferentialMotor differentialMotor) { - this(differentialMotor, new Gains(),1, 20); + this(differentialMotor, new Gains(),1); } public Command moveToStateCommand(double angle, double position, double ff, SubsystemBase... requirements){ From 9e0984248461835205cbf2b33e20e633716fa208 Mon Sep 17 00:00:00 2001 From: TapChap Date: Tue, 20 May 2025 17:50:39 +0300 Subject: [PATCH 09/17] ADD: example subsystem representing a Tank's turret and cannon --- .../control/encoder/DutyCycleEncoder.java | 4 +- .../frc/excalib/control/encoder/Encoder.java | 4 +- .../frc/excalib/control/motor/Piston.java | 4 +- .../{FlexMotor.java => SparkFlexMotor.java} | 4 +- .../frc/excalib/mechanisms/Mechanism.java | 8 +- .../java/frc/excalib/mechanisms/arm/Arm.java | 10 +- .../frc/excalib/mechanisms/turret/Turret.java | 13 ++- src/main/java/frc/robot/Constants.java | 32 ++++++ src/main/java/frc/robot/RobotContainer.java | 19 +++- src/main/java/frc/robot/SwerveConstants.java | 77 ------------- .../java/frc/robot/subsystems/Cannon.java | 106 ++++++++++++++++++ .../java/frc/robot/subsystems/Swerve.java | 11 ++ 12 files changed, 195 insertions(+), 97 deletions(-) rename src/main/java/frc/excalib/control/motor/controllers/{FlexMotor.java => SparkFlexMotor.java} (96%) create mode 100644 src/main/java/frc/robot/Constants.java delete mode 100644 src/main/java/frc/robot/SwerveConstants.java create mode 100644 src/main/java/frc/robot/subsystems/Cannon.java create mode 100644 src/main/java/frc/robot/subsystems/Swerve.java diff --git a/src/main/java/frc/excalib/control/encoder/DutyCycleEncoder.java b/src/main/java/frc/excalib/control/encoder/DutyCycleEncoder.java index f3f03d4..3ad5dfa 100644 --- a/src/main/java/frc/excalib/control/encoder/DutyCycleEncoder.java +++ b/src/main/java/frc/excalib/control/encoder/DutyCycleEncoder.java @@ -29,7 +29,7 @@ public double getVelocity() { } @Override - public void setPosition(double position) { - this.position = position; + public void setPosition(double position, boolean offset) { + this.position = offset? -position : position; } } diff --git a/src/main/java/frc/excalib/control/encoder/Encoder.java b/src/main/java/frc/excalib/control/encoder/Encoder.java index ddeb6d4..b529322 100644 --- a/src/main/java/frc/excalib/control/encoder/Encoder.java +++ b/src/main/java/frc/excalib/control/encoder/Encoder.java @@ -10,7 +10,5 @@ public interface Encoder { double getVelocity(); - void setPosition(double position); - - void setInverted(boolean inverted); + void setPosition(double position, boolean offset); } diff --git a/src/main/java/frc/excalib/control/motor/Piston.java b/src/main/java/frc/excalib/control/motor/Piston.java index 6a86537..a7ea87a 100644 --- a/src/main/java/frc/excalib/control/motor/Piston.java +++ b/src/main/java/frc/excalib/control/motor/Piston.java @@ -10,8 +10,8 @@ public class Piston { private final DoubleSolenoid piston; private DirectionState direction; - public Piston(int fwdID, int revID){ - this.piston = new DoubleSolenoid(PneumaticsModuleType.REVPH, fwdID, revID); + public Piston(int fwdID, int revID, PneumaticsModuleType moduleType){ + this.piston = new DoubleSolenoid(moduleType, fwdID, revID); this.direction = DirectionState.REVERSE; } diff --git a/src/main/java/frc/excalib/control/motor/controllers/FlexMotor.java b/src/main/java/frc/excalib/control/motor/controllers/SparkFlexMotor.java similarity index 96% rename from src/main/java/frc/excalib/control/motor/controllers/FlexMotor.java rename to src/main/java/frc/excalib/control/motor/controllers/SparkFlexMotor.java index c6e44cf..91885b7 100644 --- a/src/main/java/frc/excalib/control/motor/controllers/FlexMotor.java +++ b/src/main/java/frc/excalib/control/motor/controllers/SparkFlexMotor.java @@ -18,10 +18,10 @@ * This class represents a implemantation of a motor controller * using the Vortex Motor. */ -public class FlexMotor extends SparkFlex implements Motor { +public class SparkFlexMotor extends SparkFlex implements Motor { private final SparkFlexConfig m_config = new SparkFlexConfig(); - public FlexMotor(int deviceId, MotorType type) { + public SparkFlexMotor(int deviceId, MotorType type) { super(deviceId, type); } diff --git a/src/main/java/frc/excalib/mechanisms/Mechanism.java b/src/main/java/frc/excalib/mechanisms/Mechanism.java index fd706a0..163973e 100644 --- a/src/main/java/frc/excalib/mechanisms/Mechanism.java +++ b/src/main/java/frc/excalib/mechanisms/Mechanism.java @@ -41,15 +41,9 @@ public class Mechanism implements Logged { /** * @param motor the motor controlling the mechanism */ - public Mechanism(Motor motor, Encoder externalEncoder) { + public Mechanism(Motor motor) { m_motor = motor; m_DEFAULT_IDLE_STATE = m_motor.getIdleState(); - - m_externalEncoder = externalEncoder; - } - - public Mechanism(Motor motor){ - this(motor, null); } /** diff --git a/src/main/java/frc/excalib/mechanisms/arm/Arm.java b/src/main/java/frc/excalib/mechanisms/arm/Arm.java index 9d809c0..a0883fc 100644 --- a/src/main/java/frc/excalib/mechanisms/arm/Arm.java +++ b/src/main/java/frc/excalib/mechanisms/arm/Arm.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.limits.SoftLimit; @@ -23,13 +24,18 @@ public class Arm extends Mechanism { private final TrapezoidProfile m_profile; + private double m_setpoint, m_tolerance; + public final Trigger atSetpointTrigger = new Trigger(()-> Math.abs(this.m_setpoint - super.logMechanismPosition()) < m_tolerance); - public Arm(Motor motor, Gains gains, TrapezoidProfile.Constraints constraints) { + public Arm(Motor motor, Gains gains, TrapezoidProfile.Constraints constraints, double tolerance) { super(motor); m_PIDController = gains.getPIDcontroller(); m_ffController = gains.applyGains(new GenericFF.ArmFF()); m_profile = new TrapezoidProfile(constraints); + + m_tolerance = tolerance; + m_setpoint = 0; } public Command setDynamicPositionCommand(DoubleSupplier position, SubsystemBase... requirements) { @@ -46,6 +52,8 @@ public Command setDynamicPositionCommand(DoubleSupplier position, SubsystemBase. } public void setPosition(double position){ + this.m_setpoint = position; + double pid = m_PIDController.calculate(super.logMechanismPosition(), position); double ff = m_ffController.calculate(super.logMechanismPosition(), super.logMechanismVelocity()); setVoltage(pid + ff); diff --git a/src/main/java/frc/excalib/mechanisms/turret/Turret.java b/src/main/java/frc/excalib/mechanisms/turret/Turret.java index f3da510..ef79511 100644 --- a/src/main/java/frc/excalib/mechanisms/turret/Turret.java +++ b/src/main/java/frc/excalib/mechanisms/turret/Turret.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.limits.ContinuousSoftLimit; @@ -24,22 +25,28 @@ public final class Turret extends Mechanism { private final TrapezoidProfile m_profile; private final ContinuousSoftLimit m_rotationLimit; + private double m_setpoint, m_tolerance; + public final Trigger atSetpointTrigger = new Trigger(()-> Math.abs(this.m_setpoint - super.logMechanismPosition()) < m_tolerance); + /** * @param motor the turret's motor * @param rotationLimit the rotational boundary for the turret (radians) * @param angleGains pid gains for the turret * @param constraints constraints for the TrapezoidProfile */ - public Turret(Motor motor, ContinuousSoftLimit rotationLimit, Gains angleGains, TrapezoidProfile.Constraints constraints) { + public Turret(Motor motor, ContinuousSoftLimit rotationLimit, Gains angleGains, TrapezoidProfile.Constraints constraints, double tolerance) { super(motor); m_rotationLimit = rotationLimit; m_pidController = angleGains.getPIDcontroller(); m_ffController = angleGains.applyGains(new GenericFF.SimpleFF()); - m_pidController.enableContinuousInput(-Math.PI, Math.PI); + m_pidController.enableContinuousInput(-180, 180); m_profile = new TrapezoidProfile(constraints); + + m_tolerance = tolerance; + m_setpoint = 0; } /** @@ -66,6 +73,8 @@ public Command setDynamicPositionCommand(DoubleSupplier position, SubsystemBase. * @param setpoint the wanted position of the turret. */ public void setPosition(double setpoint) { + this.m_setpoint = setpoint; + double limitedSetpoint = m_rotationLimit.getSetPoint(super.logMechanismPosition(), setpoint); double pid = m_pidController.calculate(super.logMechanismPosition(), limitedSetpoint); double ff = m_ffController.getKs() * Math.signum(pid); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..578088a --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,32 @@ +package frc.robot; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import frc.excalib.control.gains.Gains; +import frc.excalib.control.limits.ContinuousSoftLimit; + +public class Constants { + public static final class CannonConstants{ + + // turret constants + public static final double UPPER_LIMIT = 450; + public static final double LOWER_LIMIT = 0; + + public static final double TURRET_POSITION_CONVERSION_FACTOR = 1.0 / 150.0 * 360; + public static final double TURRET_VELOCITY_CONVERSION_FACTOR = 1.0 / 150.0 * 360; + + public static final ContinuousSoftLimit TURRET_SOFT_LIMIT = new ContinuousSoftLimit(()-> UPPER_LIMIT, ()-> LOWER_LIMIT); + public static final Gains TURRET_GAINS = new Gains(0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7); + public static final TrapezoidProfile.Constraints TURRET_CONSTRAINTS = new TrapezoidProfile.Constraints(6.28, 3.14); + + public static final double TURRET_TOLERANCE = 3; // deg + + // arm constants + public static final Gains ARM_GAINS = new Gains(0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7); + public static final TrapezoidProfile.Constraints ARM_CONSTRAINTS = new TrapezoidProfile.Constraints(6.28, 3.14); + + public static final double ARM_POSITION_CONVERSION_FACTOR = 1.0 / 452.0 * 360; + public static final double ENCODER_OFFSET = 0.1234; + + public static final double ARM_TOLERANCE = 3; // deg + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 31f6d16..d10798f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,13 +7,30 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; +import frc.robot.subsystems.Cannon; +import frc.robot.subsystems.Swerve; public class RobotContainer { + Cannon cannon = new Cannon(); + Swerve swerve = new Swerve(); + + CommandPS5Controller controller = new CommandPS5Controller(0); + public RobotContainer() { configureBindings(); } - private void configureBindings() {} + private void configureBindings() { + + // move and shoot to target when cross is pressed + controller.cross().onTrue(cannon.shootToPositionCommand(swerve::getRotationToTarget, swerve::getPitchToTarget)); + + // toggle manual control when pressing square + controller.square().toggleOnTrue(cannon.manualControlCommand( + controller::getRightX, controller::getLeftY, controller.L1() //move the turret with joysticks, shoot with L1 + )); + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/SwerveConstants.java b/src/main/java/frc/robot/SwerveConstants.java deleted file mode 100644 index 999bc70..0000000 --- a/src/main/java/frc/robot/SwerveConstants.java +++ /dev/null @@ -1,77 +0,0 @@ -package frc.robot; - -import com.ctre.phoenix6.hardware.CANcoder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.path.PathConstraints; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import frc.excalib.control.gains.Gains; -import frc.excalib.control.imu.IMU; -import frc.excalib.control.imu.Pigeon; - -public class SwerveConstants { - public static final int FRONT_LEFT_DRIVE_ID = 0; - public static final int FRONT_RIGHT_DRIVE_ID = 0; - public static final int BACK_RIGHT_DRIVE_ID = 0; - public static final int BACK_LEFT_DRIVE_ID = 0; - - public static final int FRONT_LEFT_ROTATION_ID = 0; - public static final int FRONT_RIGHT_ROTATION_ID = 0; - public static final int BACK_RIGHT_ROTATION_ID = 0; - public static final int BACK_LEFT_ROTATION_ID = 0; - - public static final int GYRO_ID = 0; - public static final String SWERVE_CANBUS = "Swerve"; - - public static final double PID_TOLERANCE = 0; - - public static final double TRACK_WIDTH = 0; // Meters - public static final Translation2d FRONT_LEFT_TRANSLATION = new Translation2d(TRACK_WIDTH / 2, TRACK_WIDTH / 2); - public static final Translation2d FRONT_RIGHT_TRANSLATION = new Translation2d(TRACK_WIDTH / 2, -TRACK_WIDTH / 2); - public static final Translation2d BACK_LEFT_TRANSLATION = new Translation2d(-TRACK_WIDTH / 2, TRACK_WIDTH / 2); - public static final Translation2d BACK_RIGHT_TRANSLATION = new Translation2d(-TRACK_WIDTH / 2, -TRACK_WIDTH / 2); - - public static final double MAX_MODULE_VEL = 0; - public static final double MAX_FRONT_ACC = 0; - public static final double MAX_SIDE_ACC = 0; - public static final double MAX_SKID_ACC = 0; - public static final double MAX_FORWARD_ACC = 0; - public static final double MAX_VEL = 0; - public static final double MAX_OMEGA_RAD_PER_SEC = 0; - public static final double MAX_OMEGA_RAD_PER_SEC_SQUARE = 0; - - public static final double ANGLE_CONTROLLER_TOLORANCE = 0; - public static final double X_CONTROLLER_TOLORANCE = 0; - public static final double Y_CONTROLLER_TOLORANCE = 0; - - public static final PathConstraints MAX_PATH_CONSTRAINTS = new PathConstraints( - MAX_VEL, - MAX_SKID_ACC, - MAX_OMEGA_RAD_PER_SEC, - MAX_OMEGA_RAD_PER_SEC_SQUARE, - 12.0, - false - ); - - public static final CANcoder FRONT_LEFT_ABS_ENCODER = new CANcoder(0, SWERVE_CANBUS); - public static final CANcoder FRONT_RIGHT_ABS_ENCODER = new CANcoder(0, SWERVE_CANBUS); - public static final CANcoder BACK_RIGHT_ABS_ENCODER = new CANcoder(0, SWERVE_CANBUS); - public static final CANcoder BACK_LEFT_ABS_ENCODER = new CANcoder(0, SWERVE_CANBUS); - - public static final double VELOCITY_CONVERSION_FACTOR = 0; - public static final double POSITION_CONVERSION_FACTOR = 0; - public static final double ROTATION_VELOCITY_CONVERSION_FACTOR = 0; - - public static final PIDConstants TRANSLATION_PID_CONSTANTS = new PIDConstants(0, 0, 0); - public static final PIDConstants ANGLE_PID_CONSTANTS = new PIDConstants(0, 0, 0); - public static final Gains ANGLE_GAINS = new Gains(0, 0, 0); - public static final Gains TRANSLATION_GAINS = new Gains(0, 0, 0); - - public static final Gains SWERVE_DRIVE_MODULE_GAINS = new Gains(0, 0, 0, 0, 0, 0,0); - public static final Gains SWERVE_ROTATION_MODULE_GAINS = new Gains(0, 0, 0, 0, 0, 0,0); - - public static final IMU GYRO = new Pigeon(GYRO_ID, SWERVE_CANBUS, new Rotation3d()); - - -} - diff --git a/src/main/java/frc/robot/subsystems/Cannon.java b/src/main/java/frc/robot/subsystems/Cannon.java new file mode 100644 index 0000000..6285f61 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Cannon.java @@ -0,0 +1,106 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.encoder.DutyCycleEncoder; +import frc.excalib.control.motor.Piston; +import frc.excalib.control.motor.controllers.SparkFlexMotor; +import frc.excalib.control.motor.controllers.TalonFXMotor; +import frc.excalib.control.motor.motorType.MotorGroup; +import frc.excalib.control.motor.motor_specs.DirectionState; +import frc.excalib.mechanisms.arm.Arm; +import frc.excalib.mechanisms.turret.Turret; + +import java.util.function.DoubleSupplier; + +import static com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless; +import static frc.robot.Constants.CannonConstants.*; + +public class Cannon extends SubsystemBase { + + // Subsystem Hardware components + private final SparkFlexMotor rotationMotorA, rotationMotorB; + private final TalonFXMotor pitchMotor; + private final Piston primer; + + private final DutyCycleEncoder angleEncoder; + + // Subsystem mechanisms + private final Turret turret; + private final Arm arm; + + public Cannon() { + // initialize hardware + this.rotationMotorA = new SparkFlexMotor(0, kBrushless); + this.rotationMotorB = new SparkFlexMotor(1, kBrushless); + + this.pitchMotor = new TalonFXMotor(3); + this.primer = new Piston(1, 2, PneumaticsModuleType.REVPH); + + this.angleEncoder = new DutyCycleEncoder(3); + + // reverse one of the motors in the gearbox + this.rotationMotorB.setInverted(DirectionState.REVERSE); + + // set the encoder's offset and conversion factor + this.angleEncoder.setPositionConversionFactor(ARM_POSITION_CONVERSION_FACTOR); + this.angleEncoder.setPosition(ENCODER_OFFSET, true); + + // setup rotationMotors conversion factors + MotorGroup rotationMotors = new MotorGroup(rotationMotorA, rotationMotorB); + rotationMotors.setPositionConversionFactor(TURRET_POSITION_CONVERSION_FACTOR); + rotationMotors.setVelocityConversionFactor(TURRET_VELOCITY_CONVERSION_FACTOR); + + // create turret and arm mechanisms + this.turret = new Turret(rotationMotors, TURRET_SOFT_LIMIT, TURRET_GAINS, TURRET_CONSTRAINTS, TURRET_TOLERANCE); + this.arm = new Arm(this.pitchMotor, ARM_GAINS, ARM_CONSTRAINTS, ARM_TOLERANCE); + + // set the arm mechanism to use an external encoder rather than the motor's built in + this.arm.addExternalEncoder(this.angleEncoder); + + setDefaultCommand(moveToPositionCommand(() -> 0, () -> 0)); + } + + private Command moveToPositionCommand(DoubleSupplier rotation, DoubleSupplier pitch) { + return new ParallelCommandGroup( + new RunCommand(() -> { + }, this), // add requirements to the command + this.turret.setDynamicPositionCommand(rotation), + this.arm.setDynamicPositionCommand(pitch) + ); + } + + private Command shootCommand() { + return new StartEndCommand( + () -> this.primer.setPiston(DirectionState.FORWARD), + () -> this.primer.setPiston(DirectionState.REVERSE) + ); + } + + public Command shootToPositionCommand(DoubleSupplier rotation, DoubleSupplier pitch) { + return new ParallelCommandGroup( + moveToPositionCommand(rotation, pitch), + new WaitUntilCommand(turret.atSetpointTrigger.and(arm.atSetpointTrigger)) + .andThen(shootCommand()) + ); + } + + public Command manualControlCommand(DoubleSupplier rotation, DoubleSupplier pitch, Trigger shoot) { + return new ParallelCommandGroup( + moveToPositionCommand( + () -> getCannonRotation() + rotation.getAsDouble(), // rotate up to 5 deg a sec + () -> getCannonPitch() + pitch.getAsDouble()), // pitch up to 5 deg a sec + new WaitUntilCommand(shoot).andThen(shootCommand()) + ); + } + + + public double getCannonRotation() { + return this.turret.logMechanismPosition(); + } + + public double getCannonPitch() { + return this.arm.logMechanismPosition(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java new file mode 100644 index 0000000..6ba1a04 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems; + +public class Swerve { + public double getRotationToTarget(){ + return 0; + } + + public double getPitchToTarget(){ + return 0; + } +} From 15e743b0c8fd58906354d809e9bf6d71c970a5f6 Mon Sep 17 00:00:00 2001 From: TapChap Date: Tue, 20 May 2025 18:05:42 +0300 Subject: [PATCH 10/17] ADD: atSetpointTrigger to all mechanisms --- .../frc/excalib/mechanisms/fly_wheel/FlyWheel.java | 12 +++++++++++- .../mechanisms/linear_extension/LinearExtension.java | 11 ++++++++++- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/subsystems/Cannon.java | 7 +++---- 4 files changed, 25 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java b/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java index 465dc77..7baa49c 100644 --- a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java +++ b/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.motor.Motor; @@ -20,14 +21,21 @@ public class FlyWheel extends Mechanism { private final PIDController m_pidController; private final SimpleMotorFeedforward m_ffController; + + private double m_setpoint, m_tolerance; + public final Trigger atSetpointTrigger = new Trigger(()-> Math.abs(this.m_setpoint - super.logMechanismPosition()) < m_tolerance); + /** * @param motor the FlyWheel Motor * @param gains the FF and PID gains */ - public FlyWheel(Motor motor, Gains gains) { + public FlyWheel(Motor motor, Gains gains, double tolerance) { super(motor); this.m_pidController = gains.getPIDcontroller(); this.m_ffController = gains.applyGains(new GenericFF.SimpleFF()); + + m_tolerance = tolerance; + m_setpoint = 0; } /** @@ -44,6 +52,8 @@ public Command setDynamicVelocityCommand(DoubleSupplier velocity, SubsystemBase. * @param velocity the velocity to set to the FlyWheel Dynamically */ public void setVelocity(double velocity) { + this.m_setpoint = velocity; + double pid = m_pidController.calculate(super.m_motor.getMotorVelocity(), velocity); double ff = m_ffController.calculate(velocity); super.setVoltage(pid + ff); diff --git a/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java b/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java index 149555f..de1562f 100644 --- a/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java +++ b/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.motor.Motor; @@ -19,12 +20,18 @@ public class LinearExtension extends Mechanism { private final TrapezoidProfile m_profile; - public LinearExtension(Motor motor, Gains gains, TrapezoidProfile.Constraints constraints) { + private double m_setpoint, m_tolerance; + public final Trigger atSetpointTrigger = new Trigger(()-> Math.abs(this.m_setpoint - super.logMechanismPosition()) < m_tolerance); + + public LinearExtension(Motor motor, Gains gains, TrapezoidProfile.Constraints constraints, double tolerance) { super(motor); m_pidController = gains.getPIDcontroller(); m_ffController = gains.applyGains(new GenericFF.ElevatorFF()); m_profile = new TrapezoidProfile(constraints); + + m_tolerance = tolerance; + m_setpoint = 0; } /** @@ -47,6 +54,8 @@ public Command setDynamicPositionCommand(DoubleSupplier position, SubsystemBase. } public void setPosition(double position){ + this.m_setpoint = position; + double pid = m_pidController.calculate(super.logMechanismPosition(), position); double ff = m_ffController.calculate(super.logMechanismVelocity()); setVoltage(pid + ff); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 578088a..b4abe56 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,7 +25,7 @@ public static final class CannonConstants{ public static final TrapezoidProfile.Constraints ARM_CONSTRAINTS = new TrapezoidProfile.Constraints(6.28, 3.14); public static final double ARM_POSITION_CONVERSION_FACTOR = 1.0 / 452.0 * 360; - public static final double ENCODER_OFFSET = 0.1234; + public static final double ENCODER_OFFSET = 18.75; // deg public static final double ARM_TOLERANCE = 3; // deg } diff --git a/src/main/java/frc/robot/subsystems/Cannon.java b/src/main/java/frc/robot/subsystems/Cannon.java index 6285f61..a066cec 100644 --- a/src/main/java/frc/robot/subsystems/Cannon.java +++ b/src/main/java/frc/robot/subsystems/Cannon.java @@ -32,8 +32,8 @@ public class Cannon extends SubsystemBase { public Cannon() { // initialize hardware - this.rotationMotorA = new SparkFlexMotor(0, kBrushless); - this.rotationMotorB = new SparkFlexMotor(1, kBrushless); + this.rotationMotorA = new SparkFlexMotor(1, kBrushless); + this.rotationMotorB = new SparkFlexMotor(2, kBrushless); this.pitchMotor = new TalonFXMotor(3); this.primer = new Piston(1, 2, PneumaticsModuleType.REVPH); @@ -81,8 +81,7 @@ private Command shootCommand() { public Command shootToPositionCommand(DoubleSupplier rotation, DoubleSupplier pitch) { return new ParallelCommandGroup( moveToPositionCommand(rotation, pitch), - new WaitUntilCommand(turret.atSetpointTrigger.and(arm.atSetpointTrigger)) - .andThen(shootCommand()) + new WaitUntilCommand(turret.atSetpointTrigger.and(arm.atSetpointTrigger)).andThen(shootCommand()) ); } From 0ed91e35997f297b73fa7e787aa276b1267636e2 Mon Sep 17 00:00:00 2001 From: TapChap Date: Tue, 20 May 2025 18:09:38 +0300 Subject: [PATCH 11/17] REFACTOR: move machanisms to a seperate package --- .../mechanisms/{arm => mechanical_mechanisms}/Arm.java | 3 +-- .../Differential.java | 8 +------- .../{fly_wheel => mechanical_mechanisms}/FlyWheel.java | 3 +-- .../LinearExtension.java | 2 +- .../{turret => mechanical_mechanisms}/Turret.java | 2 +- src/main/java/frc/robot/subsystems/Cannon.java | 4 ++-- 6 files changed, 7 insertions(+), 15 deletions(-) rename src/main/java/frc/excalib/mechanisms/{arm => mechanical_mechanisms}/Arm.java (96%) rename src/main/java/frc/excalib/mechanisms/{diffrential => mechanical_mechanisms}/Differential.java (91%) rename src/main/java/frc/excalib/mechanisms/{fly_wheel => mechanical_mechanisms}/FlyWheel.java (96%) rename src/main/java/frc/excalib/mechanisms/{linear_extension => mechanical_mechanisms}/LinearExtension.java (97%) rename src/main/java/frc/excalib/mechanisms/{turret => mechanical_mechanisms}/Turret.java (98%) diff --git a/src/main/java/frc/excalib/mechanisms/arm/Arm.java b/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Arm.java similarity index 96% rename from src/main/java/frc/excalib/mechanisms/arm/Arm.java rename to src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Arm.java index a0883fc..e4652d4 100644 --- a/src/main/java/frc/excalib/mechanisms/arm/Arm.java +++ b/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Arm.java @@ -1,4 +1,4 @@ -package frc.excalib.mechanisms.arm; +package frc.excalib.mechanisms.mechanical_mechanisms; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; -import frc.excalib.control.limits.SoftLimit; import frc.excalib.control.motor.Motor; import frc.excalib.mechanisms.Mechanism; diff --git a/src/main/java/frc/excalib/mechanisms/diffrential/Differential.java b/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Differential.java similarity index 91% rename from src/main/java/frc/excalib/mechanisms/diffrential/Differential.java rename to src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Differential.java index 39bf61b..eca52dd 100644 --- a/src/main/java/frc/excalib/mechanisms/diffrential/Differential.java +++ b/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Differential.java @@ -1,17 +1,11 @@ -package frc.excalib.mechanisms.diffrential; +package frc.excalib.mechanisms.mechanical_mechanisms; -import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.*; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.excalib.control.GenericFF.FeedForwardGainsSetter; -import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.motor.motorType.DifferentialMotor; import frc.excalib.control.motor.motor_specs.IdleState; import frc.excalib.mechanisms.Mechanism; -import monologue.Annotations; import monologue.Annotations.Log; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java b/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/FlyWheel.java similarity index 96% rename from src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java rename to src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/FlyWheel.java index 7baa49c..6469064 100644 --- a/src/main/java/frc/excalib/mechanisms/fly_wheel/FlyWheel.java +++ b/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/FlyWheel.java @@ -1,8 +1,7 @@ -package frc.excalib.mechanisms.fly_wheel; +package frc.excalib.mechanisms.mechanical_mechanisms; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java b/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/LinearExtension.java similarity index 97% rename from src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java rename to src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/LinearExtension.java index de1562f..d6d230d 100644 --- a/src/main/java/frc/excalib/mechanisms/linear_extension/LinearExtension.java +++ b/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/LinearExtension.java @@ -1,4 +1,4 @@ -package frc.excalib.mechanisms.linear_extension; +package frc.excalib.mechanisms.mechanical_mechanisms; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; diff --git a/src/main/java/frc/excalib/mechanisms/turret/Turret.java b/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Turret.java similarity index 98% rename from src/main/java/frc/excalib/mechanisms/turret/Turret.java rename to src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Turret.java index ef79511..73ac221 100644 --- a/src/main/java/frc/excalib/mechanisms/turret/Turret.java +++ b/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Turret.java @@ -1,4 +1,4 @@ -package frc.excalib.mechanisms.turret; +package frc.excalib.mechanisms.mechanical_mechanisms; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; diff --git a/src/main/java/frc/robot/subsystems/Cannon.java b/src/main/java/frc/robot/subsystems/Cannon.java index a066cec..7f16db5 100644 --- a/src/main/java/frc/robot/subsystems/Cannon.java +++ b/src/main/java/frc/robot/subsystems/Cannon.java @@ -9,8 +9,8 @@ import frc.excalib.control.motor.controllers.TalonFXMotor; import frc.excalib.control.motor.motorType.MotorGroup; import frc.excalib.control.motor.motor_specs.DirectionState; -import frc.excalib.mechanisms.arm.Arm; -import frc.excalib.mechanisms.turret.Turret; +import frc.excalib.mechanisms.mechanical_mechanisms.Arm; +import frc.excalib.mechanisms.mechanical_mechanisms.Turret; import java.util.function.DoubleSupplier; From 10a6dddf92815fa1044aeed0e11fa1287ea41a90 Mon Sep 17 00:00:00 2001 From: TapChap Date: Tue, 20 May 2025 18:17:31 +0300 Subject: [PATCH 12/17] REFACTOR: move machanisms to a seperate package --- .../java/frc/excalib/{mechanisms => mechanism}/Mechanism.java | 2 +- .../mechanical_mechanisms => mechanism/mechanisms}/Arm.java | 4 ++-- .../mechanisms}/Differential.java | 4 ++-- .../mechanisms}/FlyWheel.java | 4 ++-- .../mechanisms}/LinearExtension.java | 4 ++-- .../mechanisms}/Turret.java | 4 ++-- src/main/java/frc/robot/subsystems/Cannon.java | 4 ++-- 7 files changed, 13 insertions(+), 13 deletions(-) rename src/main/java/frc/excalib/{mechanisms => mechanism}/Mechanism.java (99%) rename src/main/java/frc/excalib/{mechanisms/mechanical_mechanisms => mechanism/mechanisms}/Arm.java (95%) rename src/main/java/frc/excalib/{mechanisms/mechanical_mechanisms => mechanism/mechanisms}/Differential.java (97%) rename src/main/java/frc/excalib/{mechanisms/mechanical_mechanisms => mechanism/mechanisms}/FlyWheel.java (95%) rename src/main/java/frc/excalib/{mechanisms/mechanical_mechanisms => mechanism/mechanisms}/LinearExtension.java (96%) rename src/main/java/frc/excalib/{mechanisms/mechanical_mechanisms => mechanism/mechanisms}/Turret.java (97%) diff --git a/src/main/java/frc/excalib/mechanisms/Mechanism.java b/src/main/java/frc/excalib/mechanism/Mechanism.java similarity index 99% rename from src/main/java/frc/excalib/mechanisms/Mechanism.java rename to src/main/java/frc/excalib/mechanism/Mechanism.java index 163973e..7959512 100644 --- a/src/main/java/frc/excalib/mechanisms/Mechanism.java +++ b/src/main/java/frc/excalib/mechanism/Mechanism.java @@ -5,7 +5,7 @@ * learn more at - https://github.com/ExaliburFRC/ExcaLIb */ -package frc.excalib.mechanisms; +package frc.excalib.mechanism; import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj2.command.*; diff --git a/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Arm.java b/src/main/java/frc/excalib/mechanism/mechanisms/Arm.java similarity index 95% rename from src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Arm.java rename to src/main/java/frc/excalib/mechanism/mechanisms/Arm.java index e4652d4..1070329 100644 --- a/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Arm.java +++ b/src/main/java/frc/excalib/mechanism/mechanisms/Arm.java @@ -1,4 +1,4 @@ -package frc.excalib.mechanisms.mechanical_mechanisms; +package frc.excalib.mechanism.mechanisms; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; @@ -10,7 +10,7 @@ import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.motor.Motor; -import frc.excalib.mechanisms.Mechanism; +import frc.excalib.mechanism.Mechanism; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Differential.java b/src/main/java/frc/excalib/mechanism/mechanisms/Differential.java similarity index 97% rename from src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Differential.java rename to src/main/java/frc/excalib/mechanism/mechanisms/Differential.java index eca52dd..5ee45df 100644 --- a/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Differential.java +++ b/src/main/java/frc/excalib/mechanism/mechanisms/Differential.java @@ -1,11 +1,11 @@ -package frc.excalib.mechanisms.mechanical_mechanisms; +package frc.excalib.mechanism.mechanisms; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.*; import frc.excalib.control.gains.Gains; import frc.excalib.control.motor.motorType.DifferentialMotor; import frc.excalib.control.motor.motor_specs.IdleState; -import frc.excalib.mechanisms.Mechanism; +import frc.excalib.mechanism.Mechanism; import monologue.Annotations.Log; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/FlyWheel.java b/src/main/java/frc/excalib/mechanism/mechanisms/FlyWheel.java similarity index 95% rename from src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/FlyWheel.java rename to src/main/java/frc/excalib/mechanism/mechanisms/FlyWheel.java index 6469064..e0d4546 100644 --- a/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/FlyWheel.java +++ b/src/main/java/frc/excalib/mechanism/mechanisms/FlyWheel.java @@ -1,4 +1,4 @@ -package frc.excalib.mechanisms.mechanical_mechanisms; +package frc.excalib.mechanism.mechanisms; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -9,7 +9,7 @@ import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.motor.Motor; -import frc.excalib.mechanisms.Mechanism; +import frc.excalib.mechanism.Mechanism; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/LinearExtension.java b/src/main/java/frc/excalib/mechanism/mechanisms/LinearExtension.java similarity index 96% rename from src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/LinearExtension.java rename to src/main/java/frc/excalib/mechanism/mechanisms/LinearExtension.java index d6d230d..88b0cab 100644 --- a/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/LinearExtension.java +++ b/src/main/java/frc/excalib/mechanism/mechanisms/LinearExtension.java @@ -1,4 +1,4 @@ -package frc.excalib.mechanisms.mechanical_mechanisms; +package frc.excalib.mechanism.mechanisms; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; @@ -10,7 +10,7 @@ import frc.excalib.control.GenericFF.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.motor.Motor; -import frc.excalib.mechanisms.Mechanism; +import frc.excalib.mechanism.Mechanism; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Turret.java b/src/main/java/frc/excalib/mechanism/mechanisms/Turret.java similarity index 97% rename from src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Turret.java rename to src/main/java/frc/excalib/mechanism/mechanisms/Turret.java index 73ac221..225ff32 100644 --- a/src/main/java/frc/excalib/mechanisms/mechanical_mechanisms/Turret.java +++ b/src/main/java/frc/excalib/mechanism/mechanisms/Turret.java @@ -1,4 +1,4 @@ -package frc.excalib.mechanisms.mechanical_mechanisms; +package frc.excalib.mechanism.mechanisms; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -11,7 +11,7 @@ import frc.excalib.control.gains.Gains; import frc.excalib.control.limits.ContinuousSoftLimit; import frc.excalib.control.motor.Motor; -import frc.excalib.mechanisms.Mechanism; +import frc.excalib.mechanism.Mechanism; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/robot/subsystems/Cannon.java b/src/main/java/frc/robot/subsystems/Cannon.java index 7f16db5..01bddb5 100644 --- a/src/main/java/frc/robot/subsystems/Cannon.java +++ b/src/main/java/frc/robot/subsystems/Cannon.java @@ -9,8 +9,8 @@ import frc.excalib.control.motor.controllers.TalonFXMotor; import frc.excalib.control.motor.motorType.MotorGroup; import frc.excalib.control.motor.motor_specs.DirectionState; -import frc.excalib.mechanisms.mechanical_mechanisms.Arm; -import frc.excalib.mechanisms.mechanical_mechanisms.Turret; +import frc.excalib.mechanism.mechanisms.Arm; +import frc.excalib.mechanism.mechanisms.Turret; import java.util.function.DoubleSupplier; From 1dd427286a7fb0886872a050e5bc20d3c152afdf Mon Sep 17 00:00:00 2001 From: TapChap Date: Tue, 20 May 2025 19:13:06 +0300 Subject: [PATCH 13/17] ADD: support for non floor-tangent LInear extensions --- .../GenericFF/FeedForwardGainsSetter.java | 11 --- .../java/frc/excalib/control/gains/Gains.java | 3 +- .../{GenericFF => gains}/GenericFF.java | 15 ++-- .../control/limits/ContinuousSoftLimit.java | 72 +++++++++++-------- .../frc/excalib/control/limits/SoftLimit.java | 9 +-- .../frc/excalib/mechanism/mechanisms/Arm.java | 5 +- .../mechanism/mechanisms/FlyWheel.java | 6 +- .../mechanism/mechanisms/LinearExtension.java | 23 ++++-- .../excalib/mechanism/mechanisms/Turret.java | 7 +- .../java/frc/robot/subsystems/Cannon.java | 6 +- 10 files changed, 87 insertions(+), 70 deletions(-) delete mode 100644 src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java rename src/main/java/frc/excalib/control/{GenericFF => gains}/GenericFF.java (86%) diff --git a/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java b/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java deleted file mode 100644 index f9c208f..0000000 --- a/src/main/java/frc/excalib/control/GenericFF/FeedForwardGainsSetter.java +++ /dev/null @@ -1,11 +0,0 @@ -package frc.excalib.control.GenericFF; - -/** - * Interface for feedforward controllers that have kS, kV, and kA gains. - */ -public interface FeedForwardGainsSetter { - void setKs(double Ks); - void setKv(double Kv); - void setKa(double Ka); - void setKg(double Kg); -} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/gains/Gains.java b/src/main/java/frc/excalib/control/gains/Gains.java index 2c54088..66b9296 100644 --- a/src/main/java/frc/excalib/control/gains/Gains.java +++ b/src/main/java/frc/excalib/control/gains/Gains.java @@ -1,7 +1,6 @@ package frc.excalib.control.gains; import edu.wpi.first.math.controller.PIDController; -import frc.excalib.control.GenericFF.FeedForwardGainsSetter; /** @@ -126,7 +125,7 @@ public PIDController getPIDcontroller(){ return new PIDController(this.kp, this.ki, this.kd); } - public GenericFeedFoward applyGains(GenericFeedFoward feedForward){ + public GenericFeedFoward applyGains(GenericFeedFoward feedForward){ feedForward.setKv(this.kv); feedForward.setKs(this.ks); feedForward.setKa(this.ka); diff --git a/src/main/java/frc/excalib/control/GenericFF/GenericFF.java b/src/main/java/frc/excalib/control/gains/GenericFF.java similarity index 86% rename from src/main/java/frc/excalib/control/GenericFF/GenericFF.java rename to src/main/java/frc/excalib/control/gains/GenericFF.java index be87a8b..fbfe810 100644 --- a/src/main/java/frc/excalib/control/GenericFF/GenericFF.java +++ b/src/main/java/frc/excalib/control/gains/GenericFF.java @@ -1,11 +1,18 @@ -package frc.excalib.control.GenericFF; +package frc.excalib.control.gains; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.SimpleMotorFeedforward; public class GenericFF { - public static class ArmFF extends ArmFeedforward implements FeedForwardGainsSetter { + public interface GenericFeedForward { + void setKs(double Ks); + void setKv(double Kv); + void setKa(double Ka); + void setKg(double Kg); + } + + public static class ArmFF extends ArmFeedforward implements GenericFeedForward { public ArmFF() { super(0, 0, 0, 0); } @@ -34,7 +41,7 @@ public void setKg(double Kg) { } } - public static class SimpleFF extends SimpleMotorFeedforward implements FeedForwardGainsSetter { + public static class SimpleFF extends SimpleMotorFeedforward implements GenericFeedForward { public SimpleFF() { super(0, 0, 0); } @@ -66,7 +73,7 @@ public void setKg(double Kg) { } } - public static class ElevatorFF extends ElevatorFeedforward implements FeedForwardGainsSetter { + public static class ElevatorFF extends ElevatorFeedforward implements GenericFeedForward { public ElevatorFF() { super(0, 0, 0, 0); } diff --git a/src/main/java/frc/excalib/control/limits/ContinuousSoftLimit.java b/src/main/java/frc/excalib/control/limits/ContinuousSoftLimit.java index dce51ed..93cdaca 100644 --- a/src/main/java/frc/excalib/control/limits/ContinuousSoftLimit.java +++ b/src/main/java/frc/excalib/control/limits/ContinuousSoftLimit.java @@ -1,42 +1,52 @@ package frc.excalib.control.limits; import java.util.function.DoubleSupplier; +import edu.wpi.first.math.MathUtil; // Import the MathUtil class public class ContinuousSoftLimit extends SoftLimit { - /** - * A constructor that takes two DoubleSuppliers representing the dynamic limits: - * - * @param minLimit the minimal limit of the represented range - * @param maxLimit the maximal limit of the represented range - */ + private static final double TWO_PI = 2 * Math.PI; + public ContinuousSoftLimit(DoubleSupplier minLimit, DoubleSupplier maxLimit) { super(minLimit, maxLimit); } - public double getSetPoint(double measurement, double wantedSetPoint) { - double upperSetPoint, lowerSetPoint; - if (wantedSetPoint > measurement) { - upperSetPoint = wantedSetPoint; - while ((upperSetPoint - 2 * Math.PI) > measurement) { - upperSetPoint -= 2 * Math.PI; - } - lowerSetPoint = upperSetPoint - 2 * Math.PI; - } else if (wantedSetPoint < measurement) { - lowerSetPoint = wantedSetPoint; - while ((lowerSetPoint + 2 * Math.PI) < measurement) { - lowerSetPoint += 2 * Math.PI; - } - upperSetPoint = lowerSetPoint + 2 * Math.PI; - } else { - return wantedSetPoint; - } - if (upperSetPoint > super.getMaxLimit()) { - return lowerSetPoint; - } else if (lowerSetPoint < super.getMinLimit()) { - return upperSetPoint; + public double getSetpoint(double measurement, double wantedSetPoint) { + double minLimit = super.getMinLimit(); + double maxLimit = super.getMaxLimit(); + + // Calculate the difference and normalize it to (-PI, PI] + // This ensures 'candidate1' is the shortest angular path from 'measurement' + double angleDifference = MathUtil.angleModulus(wantedSetPoint - measurement); + + // Candidate 1: The target reached by the shortest angular path + double candidate1 = measurement + angleDifference; + + // Candidate 2: The target reached by taking the "other way around" + // If angleDifference was positive (clockwise), candidate2 is counter-clockwise. + // If angleDifference was negative (counter-clockwise), candidate2 is clockwise. + double candidate2 = (angleDifference > 0) ? (candidate1 - TWO_PI) : (candidate1 + TWO_PI); + + // Check if candidates are within the soft limits + boolean candidate1Valid = candidate1 >= minLimit && candidate1 <= maxLimit; + boolean candidate2Valid = candidate2 >= minLimit && candidate2 <= maxLimit; + + if (candidate1Valid && candidate2Valid) + // Both paths lead to a valid angle, choose the one truly closer to the measurement + return Math.abs(measurement - candidate1) < Math.abs(measurement - candidate2) ? candidate1 : candidate2; + if (candidate1Valid) + // Only candidate1 is valid + return candidate1; + if (candidate2Valid) + // Only candidate2 is valid + return candidate2; + + // Neither of the primary cyclic candidates are valid. + // This means the 'wantedSetPoint' is unreachable given the current `measurement` + // and the `minLimit`/`maxLimit` constraints. + // In this case, the most robust action is to clamp the desired setpoint + // to the absolute bounds of the soft limit. This ensures the returned value + // is always within the safe range. + return super.limit(wantedSetPoint); } - return - Math.abs(measurement - upperSetPoint) < Math.abs(measurement - lowerSetPoint) ? - upperSetPoint : lowerSetPoint; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/excalib/control/limits/SoftLimit.java b/src/main/java/frc/excalib/control/limits/SoftLimit.java index e2c3bd6..3bd080e 100644 --- a/src/main/java/frc/excalib/control/limits/SoftLimit.java +++ b/src/main/java/frc/excalib/control/limits/SoftLimit.java @@ -1,5 +1,6 @@ package frc.excalib.control.limits; +import edu.wpi.first.math.MathUtil; import monologue.Annotations.Log; import monologue.Logged; @@ -44,13 +45,7 @@ public boolean within(double val) { */ @Log.NT public double limit(double val) { - if (within(val)) { - return val; - } - if (val > m_maxLimit.getAsDouble()) { - return m_maxLimit.getAsDouble(); - } - return m_minLimit.getAsDouble(); + return MathUtil.clamp(val, m_minLimit.getAsDouble(), m_maxLimit.getAsDouble()); } /** diff --git a/src/main/java/frc/excalib/mechanism/mechanisms/Arm.java b/src/main/java/frc/excalib/mechanism/mechanisms/Arm.java index 1070329..1761ef4 100644 --- a/src/main/java/frc/excalib/mechanism/mechanisms/Arm.java +++ b/src/main/java/frc/excalib/mechanism/mechanisms/Arm.java @@ -1,5 +1,6 @@ package frc.excalib.mechanism.mechanisms; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -7,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.excalib.control.GenericFF.GenericFF; +import frc.excalib.control.gains.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.motor.Motor; import frc.excalib.mechanism.Mechanism; @@ -24,7 +25,7 @@ public class Arm extends Mechanism { private final TrapezoidProfile m_profile; private double m_setpoint, m_tolerance; - public final Trigger atSetpointTrigger = new Trigger(()-> Math.abs(this.m_setpoint - super.logMechanismPosition()) < m_tolerance); + public final Trigger atSetpointTrigger = new Trigger(()-> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), m_tolerance)); public Arm(Motor motor, Gains gains, TrapezoidProfile.Constraints constraints, double tolerance) { super(motor); diff --git a/src/main/java/frc/excalib/mechanism/mechanisms/FlyWheel.java b/src/main/java/frc/excalib/mechanism/mechanisms/FlyWheel.java index e0d4546..c0be1f5 100644 --- a/src/main/java/frc/excalib/mechanism/mechanisms/FlyWheel.java +++ b/src/main/java/frc/excalib/mechanism/mechanisms/FlyWheel.java @@ -1,12 +1,13 @@ package frc.excalib.mechanism.mechanisms; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.excalib.control.GenericFF.GenericFF; +import frc.excalib.control.gains.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.motor.Motor; import frc.excalib.mechanism.Mechanism; @@ -20,9 +21,8 @@ public class FlyWheel extends Mechanism { private final PIDController m_pidController; private final SimpleMotorFeedforward m_ffController; - private double m_setpoint, m_tolerance; - public final Trigger atSetpointTrigger = new Trigger(()-> Math.abs(this.m_setpoint - super.logMechanismPosition()) < m_tolerance); + public final Trigger atSetpointTrigger = new Trigger(()-> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), m_tolerance)); /** * @param motor the FlyWheel Motor diff --git a/src/main/java/frc/excalib/mechanism/mechanisms/LinearExtension.java b/src/main/java/frc/excalib/mechanism/mechanisms/LinearExtension.java index 88b0cab..8e90281 100644 --- a/src/main/java/frc/excalib/mechanism/mechanisms/LinearExtension.java +++ b/src/main/java/frc/excalib/mechanism/mechanisms/LinearExtension.java @@ -1,5 +1,6 @@ package frc.excalib.mechanism.mechanisms; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -7,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.excalib.control.GenericFF.GenericFF; +import frc.excalib.control.gains.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.motor.Motor; import frc.excalib.mechanism.Mechanism; @@ -21,10 +22,15 @@ public class LinearExtension extends Mechanism { private final TrapezoidProfile m_profile; private double m_setpoint, m_tolerance; - public final Trigger atSetpointTrigger = new Trigger(()-> Math.abs(this.m_setpoint - super.logMechanismPosition()) < m_tolerance); + public final Trigger atSetpointTrigger = new Trigger(()-> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), m_tolerance)); + //--- - public LinearExtension(Motor motor, Gains gains, TrapezoidProfile.Constraints constraints, double tolerance) { + private DoubleSupplier m_angleSupplier = ()-> 0; + private final Gains m_angleGains; + + public LinearExtension(Motor motor, Gains gains, TrapezoidProfile.Constraints constraints, DoubleSupplier angleSupplier, double tolerance) { super(motor); + m_angleGains = gains; m_pidController = gains.getPIDcontroller(); m_ffController = gains.applyGains(new GenericFF.ElevatorFF()); @@ -53,11 +59,20 @@ public Command setDynamicPositionCommand(DoubleSupplier position, SubsystemBase. }, requirements); } + /** + * allows for non floor-tangent linear extensions to add the angle for the FF calculations + * @param angleSupplier supplier for the angle between the mechanism and the ground + */ + public void setMechanismAngle(DoubleSupplier angleSupplier){ + this.m_angleSupplier = angleSupplier; + } + public void setPosition(double position){ this.m_setpoint = position; double pid = m_pidController.calculate(super.logMechanismPosition(), position); - double ff = m_ffController.calculate(super.logMechanismVelocity()); + double ff = (m_ffController.calculate(super.logMechanismVelocity()) - m_angleGains.kg) + + m_angleGains.kg * Math.sin(m_angleSupplier.getAsDouble()); setVoltage(pid + ff); } } \ No newline at end of file diff --git a/src/main/java/frc/excalib/mechanism/mechanisms/Turret.java b/src/main/java/frc/excalib/mechanism/mechanisms/Turret.java index 225ff32..76be490 100644 --- a/src/main/java/frc/excalib/mechanism/mechanisms/Turret.java +++ b/src/main/java/frc/excalib/mechanism/mechanisms/Turret.java @@ -1,5 +1,6 @@ package frc.excalib.mechanism.mechanisms; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -7,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.excalib.control.GenericFF.GenericFF; +import frc.excalib.control.gains.GenericFF; import frc.excalib.control.gains.Gains; import frc.excalib.control.limits.ContinuousSoftLimit; import frc.excalib.control.motor.Motor; @@ -26,7 +27,7 @@ public final class Turret extends Mechanism { private final ContinuousSoftLimit m_rotationLimit; private double m_setpoint, m_tolerance; - public final Trigger atSetpointTrigger = new Trigger(()-> Math.abs(this.m_setpoint - super.logMechanismPosition()) < m_tolerance); + public final Trigger atSetpointTrigger = new Trigger(()-> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), m_tolerance)); /** * @param motor the turret's motor @@ -75,7 +76,7 @@ public Command setDynamicPositionCommand(DoubleSupplier position, SubsystemBase. public void setPosition(double setpoint) { this.m_setpoint = setpoint; - double limitedSetpoint = m_rotationLimit.getSetPoint(super.logMechanismPosition(), setpoint); + double limitedSetpoint = m_rotationLimit.getSetpoint(super.logMechanismPosition(), setpoint); double pid = m_pidController.calculate(super.logMechanismPosition(), limitedSetpoint); double ff = m_ffController.getKs() * Math.signum(pid); super.setVoltage(pid + ff); diff --git a/src/main/java/frc/robot/subsystems/Cannon.java b/src/main/java/frc/robot/subsystems/Cannon.java index 01bddb5..cb80768 100644 --- a/src/main/java/frc/robot/subsystems/Cannon.java +++ b/src/main/java/frc/robot/subsystems/Cannon.java @@ -79,9 +79,9 @@ private Command shootCommand() { } public Command shootToPositionCommand(DoubleSupplier rotation, DoubleSupplier pitch) { - return new ParallelCommandGroup( - moveToPositionCommand(rotation, pitch), - new WaitUntilCommand(turret.atSetpointTrigger.and(arm.atSetpointTrigger)).andThen(shootCommand()) + return moveToPositionCommand(rotation, pitch) + .until(turret.atSetpointTrigger.and(arm.atSetpointTrigger)) + .andThen(shootCommand() ); } From b7fb433f13c5c7ab6d95f5760264a4879b3770e2 Mon Sep 17 00:00:00 2001 From: TapChap Date: Tue, 20 May 2025 23:11:35 +0300 Subject: [PATCH 14/17] ADD: ControlPositionMechanism & ControlVelocityMechanism to replace mechanisms --- .../control/limits/ContinuousSoftLimit.java | 13 ++- .../mechanism/ControlPositionMechanism.java | 79 +++++++++++++++++++ .../mechanism/ControlVelocityMechanism.java | 61 ++++++++++++++ src/main/java/frc/robot/Constants.java | 3 + src/main/java/frc/robot/RobotContainer.java | 45 +++++++---- .../robot/subsystems/AdjustableShooter.java | 77 ++++++++++++++++++ .../java/frc/robot/subsystems/Cannon.java | 2 + .../java/frc/robot/subsystems/Swerve.java | 4 + 8 files changed, 260 insertions(+), 24 deletions(-) create mode 100644 src/main/java/frc/excalib/mechanism/ControlPositionMechanism.java create mode 100644 src/main/java/frc/excalib/mechanism/ControlVelocityMechanism.java create mode 100644 src/main/java/frc/robot/subsystems/AdjustableShooter.java diff --git a/src/main/java/frc/excalib/control/limits/ContinuousSoftLimit.java b/src/main/java/frc/excalib/control/limits/ContinuousSoftLimit.java index 93cdaca..ae36a95 100644 --- a/src/main/java/frc/excalib/control/limits/ContinuousSoftLimit.java +++ b/src/main/java/frc/excalib/control/limits/ContinuousSoftLimit.java @@ -40,13 +40,12 @@ public double getSetpoint(double measurement, double wantedSetPoint) { // Only candidate2 is valid return candidate2; - // Neither of the primary cyclic candidates are valid. - // This means the 'wantedSetPoint' is unreachable given the current `measurement` - // and the `minLimit`/`maxLimit` constraints. - // In this case, the most robust action is to clamp the desired setpoint - // to the absolute bounds of the soft limit. This ensures the returned value - // is always within the safe range. + // Neither of the primary cyclic candidates are valid. + // This means the 'wantedSetPoint' is unreachable given the current `measurement` + // and the `minLimit`/`maxLimit` constraints. + // In this case, the most robust action is to clamp the desired setpoint + // to the absolute bounds of the soft limit. This ensures the returned value + // is always within the safe range. return super.limit(wantedSetPoint); - } } } \ No newline at end of file diff --git a/src/main/java/frc/excalib/mechanism/ControlPositionMechanism.java b/src/main/java/frc/excalib/mechanism/ControlPositionMechanism.java new file mode 100644 index 0000000..eab0a3f --- /dev/null +++ b/src/main/java/frc/excalib/mechanism/ControlPositionMechanism.java @@ -0,0 +1,79 @@ +package frc.excalib.mechanism; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.gains.Gains; +import frc.excalib.control.limits.ContinuousSoftLimit; +import frc.excalib.control.limits.SoftLimit; +import frc.excalib.control.motor.Motor; + +import java.util.function.BiFunction; +import java.util.function.DoubleSupplier; + +public class ControlPositionMechanism extends Mechanism{ + private final PIDController m_pidController; + private BiFunction m_feedforwardCalculator; + + private SoftLimit limit; + + private double m_setpoint, m_tolerance; + public final Trigger atSetpointTrigger = new Trigger(()-> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), m_tolerance)); + + /** + * @param motor the FlyWheel Motor + * @param gains the FF and PID gains + */ + public ControlPositionMechanism(Motor motor, Gains gains, double tolerance, + BiFunction feedforwardCalculator) { + super(motor); + this.m_pidController = gains.getPIDcontroller(); + m_feedforwardCalculator = feedforwardCalculator; + + m_tolerance = tolerance; + m_setpoint = 0; + } + + /** + * moves the Turret to a Dynamic position with a Trapezoid profile + * @param position position supplier + * @param requirements subsystem requirements + * @return the command to move the Turret + */ + public Command setPositionCommand(DoubleSupplier position, TrapezoidProfile profile, SubsystemBase... requirements){ + return new RunCommand(() -> { + TrapezoidProfile.State state = profile.calculate( + 0.02, + new TrapezoidProfile.State( + super.logMechanismPosition(), + super.logMechanismVelocity()), + new TrapezoidProfile.State(position.getAsDouble(), 0) + ); + setPosition(state.position); + }, requirements); + } + + public void withLimits(SoftLimit limit){ + this.limit = limit; + } + + /** + * @param position the position to set to the FlyWheel Dynamically + */ + public void setPosition(double position) { + this.m_setpoint = position; + + if (limit != null){ + if (limit instanceof ContinuousSoftLimit) m_setpoint = ((ContinuousSoftLimit) limit).getSetpoint(logMechanismPosition(), position); + else m_setpoint = limit.limit(position); + } + + double pid = m_pidController.calculate(super.m_motor.getMotorVelocity(), m_setpoint); + double ff = m_feedforwardCalculator.apply(super.logMechanismPosition(), super.logMechanismVelocity()); + super.setVoltage(pid + ff); + } +} diff --git a/src/main/java/frc/excalib/mechanism/ControlVelocityMechanism.java b/src/main/java/frc/excalib/mechanism/ControlVelocityMechanism.java new file mode 100644 index 0000000..f82ab37 --- /dev/null +++ b/src/main/java/frc/excalib/mechanism/ControlVelocityMechanism.java @@ -0,0 +1,61 @@ +package frc.excalib.mechanism; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.gains.Gains; +import frc.excalib.control.limits.ContinuousSoftLimit; +import frc.excalib.control.limits.SoftLimit; +import frc.excalib.control.motor.Motor; + +import java.util.function.BiFunction; +import java.util.function.DoubleSupplier; + +public class ControlVelocityMechanism extends Mechanism{ + private final PIDController m_pidController; + private BiFunction m_feedforwardCalculator; + + private SoftLimit limit; + + private double m_setpoint, m_tolerance; + public final Trigger atSetpointTrigger = new Trigger(()-> MathUtil.isNear(this.m_setpoint, super.logMechanismVelocity(), m_tolerance)); + + /** + * @param motor the FlyWheel Motor + * @param gains the FF and PID gains + */ + public ControlVelocityMechanism(Motor motor, Gains gains, double tolerance, + BiFunction feedforwardCalculator) { + super(motor); + this.m_pidController = gains.getPIDcontroller(); + m_feedforwardCalculator = feedforwardCalculator; + + m_tolerance = tolerance; + m_setpoint = 0; + } + + /** + * sets the Mechanism to a Dynamic velocity + * @param velocity velocity supplier + * @param requirements subsystem requirements + * @return the command to set the FlyWheel velocity + */ + public Command setVelocityCommand(DoubleSupplier velocity, SubsystemBase... requirements) { + return new RunCommand(() -> setVelocity(velocity.getAsDouble()), requirements); + } + + /** + * @param velocity the velocity to set to the FlyWheel Dynamically + */ + public void setVelocity(double velocity) { + this.m_setpoint = velocity; + + double pid = m_pidController.calculate(super.m_motor.getMotorVelocity(), m_setpoint); + double ff = m_feedforwardCalculator.apply(super.logMechanismPosition(), super.logMechanismVelocity()); + super.setVoltage(pid + ff); + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b4abe56..e105301 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import frc.excalib.control.gains.Gains; import frc.excalib.control.limits.ContinuousSoftLimit; +import frc.excalib.control.limits.SoftLimit; public class Constants { public static final class CannonConstants{ @@ -24,6 +25,8 @@ public static final class CannonConstants{ public static final Gains ARM_GAINS = new Gains(0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7); public static final TrapezoidProfile.Constraints ARM_CONSTRAINTS = new TrapezoidProfile.Constraints(6.28, 3.14); + public static final SoftLimit ARM_SOFT_LIMITS = new SoftLimit(()-> 0, ()-> 90); + public static final double ARM_POSITION_CONVERSION_FACTOR = 1.0 / 452.0 * 360; public static final double ENCODER_OFFSET = 18.75; // deg diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d10798f..e8817f8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,34 +5,45 @@ package frc.robot; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; +import frc.robot.subsystems.AdjustableShooter; import frc.robot.subsystems.Cannon; import frc.robot.subsystems.Swerve; public class RobotContainer { - Cannon cannon = new Cannon(); - Swerve swerve = new Swerve(); + Cannon cannon = new Cannon(); + AdjustableShooter adjShooter = new AdjustableShooter(); + Swerve swerve = new Swerve(); - CommandPS5Controller controller = new CommandPS5Controller(0); + InterpolatingDoubleTreeMap velInterpolate = new InterpolatingDoubleTreeMap(); + InterpolatingDoubleTreeMap angleInterpolate = new InterpolatingDoubleTreeMap(); - public RobotContainer() { - configureBindings(); - } + CommandPS5Controller controller = new CommandPS5Controller(0); - private void configureBindings() { + public RobotContainer() { + configureBindings(); + } - // move and shoot to target when cross is pressed - controller.cross().onTrue(cannon.shootToPositionCommand(swerve::getRotationToTarget, swerve::getPitchToTarget)); + private void configureBindings() { - // toggle manual control when pressing square - controller.square().toggleOnTrue(cannon.manualControlCommand( - controller::getRightX, controller::getLeftY, controller.L1() //move the turret with joysticks, shoot with L1 - )); - } + // move and shoot to target when cross is pressed + controller.cross().onTrue(cannon.shootToPositionCommand(swerve::getRotationToTarget, swerve::getPitchToTarget)); - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } + // toggle manual control when pressing square + controller.square().toggleOnTrue(cannon.manualControlCommand( + controller::getRightX, controller::getLeftY, controller.L1() //move the turret with joysticks, shoot with L1 + )); + + controller.triangle().onTrue(adjShooter.setShooterStateCommand( + () -> velInterpolate.get(swerve.getDistanceFromTarget()), + () -> angleInterpolate.get(swerve.getDistanceFromTarget()) + )); + } + + public Command getAutonomousCommand() { + return Commands.print("No autonomous command configured"); + } } diff --git a/src/main/java/frc/robot/subsystems/AdjustableShooter.java b/src/main/java/frc/robot/subsystems/AdjustableShooter.java new file mode 100644 index 0000000..ea24945 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AdjustableShooter.java @@ -0,0 +1,77 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.gains.GenericFF; +import frc.excalib.control.motor.controllers.SparkFlexMotor; +import frc.excalib.control.motor.controllers.TalonFXMotor; +import frc.excalib.control.motor.motorType.MotorGroup; +import frc.excalib.control.motor.motor_specs.DirectionState; +import frc.excalib.mechanism.ControlPositionMechanism; +import frc.excalib.mechanism.ControlVelocityMechanism; + +import java.util.function.DoubleSupplier; + +import static com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless; +import static frc.robot.Constants.CannonConstants.*; + +public class AdjustableShooter extends SubsystemBase{ + // Subsystem Hardware components + private final SparkFlexMotor flywheelMotorA, flywhellMotorB; + private final TalonFXMotor hoodMotor; + + // Subsystem mechanisms + private final ControlPositionMechanism hood; + private final ControlVelocityMechanism shooter; + + public AdjustableShooter() { + // initialize hardware + this.flywheelMotorA = new SparkFlexMotor(1, kBrushless); + this.flywhellMotorB = new SparkFlexMotor(2, kBrushless); + + this.hoodMotor = new TalonFXMotor(3); + + // reverse one of the motors in the gearbox + this.flywhellMotorB.setInverted(DirectionState.REVERSE); + + // setup shooterMotors conversion factors + MotorGroup shooterMotors = new MotorGroup(flywheelMotorA, flywhellMotorB); + shooterMotors.setPositionConversionFactor(TURRET_POSITION_CONVERSION_FACTOR); + shooterMotors.setVelocityConversionFactor(TURRET_VELOCITY_CONVERSION_FACTOR); + + // create turret and arm mechanisms + this.hood = new ControlPositionMechanism(hoodMotor, TURRET_GAINS, TURRET_TOLERANCE, + (position, velocity)-> TURRET_GAINS.ks * Math.signum(velocity)); + this.shooter = new ControlVelocityMechanism(shooterMotors, ARM_GAINS, ARM_TOLERANCE, + (position, velocity)-> ARM_GAINS.applyGains(new GenericFF.SimpleFF()).calculate(velocity)); + + this.hood.withLimits(TURRET_SOFT_LIMIT); + + setDefaultCommand(moveHoodToPositionCommand(() -> 0)); + } + + private Command moveHoodToPositionCommand(DoubleSupplier position) { + return this.hood.setPositionCommand(position, new TrapezoidProfile(ARM_CONSTRAINTS)); + } + + private Command setShooterVelocityCommand(DoubleSupplier velocity) { + return this.shooter.setVelocityCommand(velocity); + } + + public Command setShooterStateCommand(DoubleSupplier hoodPosition, DoubleSupplier velocity) { + return new ParallelCommandGroup( + new RunCommand(()-> {}, this), + moveHoodToPositionCommand(hoodPosition), + setShooterVelocityCommand(velocity) + ); + } + + public double getHoodPosition() { + return this.hood.logMechanismPosition(); + } + + public double getShooterVelocity() { + return this.shooter.logMechanismVelocity(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Cannon.java b/src/main/java/frc/robot/subsystems/Cannon.java index cb80768..b4527e6 100644 --- a/src/main/java/frc/robot/subsystems/Cannon.java +++ b/src/main/java/frc/robot/subsystems/Cannon.java @@ -4,12 +4,14 @@ import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.excalib.control.encoder.DutyCycleEncoder; +import frc.excalib.control.gains.GenericFF; import frc.excalib.control.motor.Piston; import frc.excalib.control.motor.controllers.SparkFlexMotor; import frc.excalib.control.motor.controllers.TalonFXMotor; import frc.excalib.control.motor.motorType.MotorGroup; import frc.excalib.control.motor.motor_specs.DirectionState; import frc.excalib.mechanism.mechanisms.Arm; +import frc.excalib.mechanism.mechanisms.LinearExtension; import frc.excalib.mechanism.mechanisms.Turret; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 6ba1a04..dc0cbf8 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -8,4 +8,8 @@ public double getRotationToTarget(){ public double getPitchToTarget(){ return 0; } + + public double getDistanceFromTarget(){ + return 0; + } } From 4a179c012cea76563f0f66cd1c0bd114baf35263 Mon Sep 17 00:00:00 2001 From: TapChap Date: Tue, 20 May 2025 23:35:59 +0300 Subject: [PATCH 15/17] REFACTOR: change addLimit function name --- .../excalib/mechanism/ControlPositionMechanism.java | 6 +----- .../frc/robot/subsystems/AdjustableShooter.java | 13 ++++++------- 2 files changed, 7 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/excalib/mechanism/ControlPositionMechanism.java b/src/main/java/frc/excalib/mechanism/ControlPositionMechanism.java index eab0a3f..b61e898 100644 --- a/src/main/java/frc/excalib/mechanism/ControlPositionMechanism.java +++ b/src/main/java/frc/excalib/mechanism/ControlPositionMechanism.java @@ -24,10 +24,6 @@ public class ControlPositionMechanism extends Mechanism{ private double m_setpoint, m_tolerance; public final Trigger atSetpointTrigger = new Trigger(()-> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), m_tolerance)); - /** - * @param motor the FlyWheel Motor - * @param gains the FF and PID gains - */ public ControlPositionMechanism(Motor motor, Gains gains, double tolerance, BiFunction feedforwardCalculator) { super(motor); @@ -57,7 +53,7 @@ public Command setPositionCommand(DoubleSupplier position, TrapezoidProfile prof }, requirements); } - public void withLimits(SoftLimit limit){ + public void addLimit(SoftLimit limit){ this.limit = limit; } diff --git a/src/main/java/frc/robot/subsystems/AdjustableShooter.java b/src/main/java/frc/robot/subsystems/AdjustableShooter.java index ea24945..147e546 100644 --- a/src/main/java/frc/robot/subsystems/AdjustableShooter.java +++ b/src/main/java/frc/robot/subsystems/AdjustableShooter.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.*; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.excalib.control.gains.GenericFF; import frc.excalib.control.motor.controllers.SparkFlexMotor; import frc.excalib.control.motor.controllers.TalonFXMotor; @@ -18,7 +17,7 @@ public class AdjustableShooter extends SubsystemBase{ // Subsystem Hardware components - private final SparkFlexMotor flywheelMotorA, flywhellMotorB; + private final SparkFlexMotor flywheelMotorA, flywheelMotorB; private final TalonFXMotor hoodMotor; // Subsystem mechanisms @@ -28,15 +27,15 @@ public class AdjustableShooter extends SubsystemBase{ public AdjustableShooter() { // initialize hardware this.flywheelMotorA = new SparkFlexMotor(1, kBrushless); - this.flywhellMotorB = new SparkFlexMotor(2, kBrushless); + this.flywheelMotorB = new SparkFlexMotor(2, kBrushless); this.hoodMotor = new TalonFXMotor(3); // reverse one of the motors in the gearbox - this.flywhellMotorB.setInverted(DirectionState.REVERSE); + this.flywheelMotorB.setInverted(DirectionState.REVERSE); // setup shooterMotors conversion factors - MotorGroup shooterMotors = new MotorGroup(flywheelMotorA, flywhellMotorB); + MotorGroup shooterMotors = new MotorGroup(flywheelMotorA, flywheelMotorB); shooterMotors.setPositionConversionFactor(TURRET_POSITION_CONVERSION_FACTOR); shooterMotors.setVelocityConversionFactor(TURRET_VELOCITY_CONVERSION_FACTOR); @@ -46,9 +45,9 @@ public AdjustableShooter() { this.shooter = new ControlVelocityMechanism(shooterMotors, ARM_GAINS, ARM_TOLERANCE, (position, velocity)-> ARM_GAINS.applyGains(new GenericFF.SimpleFF()).calculate(velocity)); - this.hood.withLimits(TURRET_SOFT_LIMIT); + this.hood.addLimit(TURRET_SOFT_LIMIT); - setDefaultCommand(moveHoodToPositionCommand(() -> 0)); + setDefaultCommand(setShooterStateCommand(() -> 0, ()-> 0)); } private Command moveHoodToPositionCommand(DoubleSupplier position) { From 64913259239ecbf092a44ccb49bf054bb1d85dd8 Mon Sep 17 00:00:00 2001 From: TapChap Date: Wed, 21 May 2025 11:39:27 +0300 Subject: [PATCH 16/17] REWRITE: PositionMechanism to utilize a Builder deign pattern --- .../mechanism/ControlPositionMechanism.java | 75 ------------- .../excalib/mechanism/PositionMechanism.java | 105 ++++++++++++++++++ ...yMechanism.java => VelocityMechanism.java} | 32 +++--- .../robot/subsystems/AdjustableShooter.java | 19 ++-- 4 files changed, 130 insertions(+), 101 deletions(-) delete mode 100644 src/main/java/frc/excalib/mechanism/ControlPositionMechanism.java create mode 100644 src/main/java/frc/excalib/mechanism/PositionMechanism.java rename src/main/java/frc/excalib/mechanism/{ControlVelocityMechanism.java => VelocityMechanism.java} (63%) diff --git a/src/main/java/frc/excalib/mechanism/ControlPositionMechanism.java b/src/main/java/frc/excalib/mechanism/ControlPositionMechanism.java deleted file mode 100644 index b61e898..0000000 --- a/src/main/java/frc/excalib/mechanism/ControlPositionMechanism.java +++ /dev/null @@ -1,75 +0,0 @@ -package frc.excalib.mechanism; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.excalib.control.gains.Gains; -import frc.excalib.control.limits.ContinuousSoftLimit; -import frc.excalib.control.limits.SoftLimit; -import frc.excalib.control.motor.Motor; - -import java.util.function.BiFunction; -import java.util.function.DoubleSupplier; - -public class ControlPositionMechanism extends Mechanism{ - private final PIDController m_pidController; - private BiFunction m_feedforwardCalculator; - - private SoftLimit limit; - - private double m_setpoint, m_tolerance; - public final Trigger atSetpointTrigger = new Trigger(()-> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), m_tolerance)); - - public ControlPositionMechanism(Motor motor, Gains gains, double tolerance, - BiFunction feedforwardCalculator) { - super(motor); - this.m_pidController = gains.getPIDcontroller(); - m_feedforwardCalculator = feedforwardCalculator; - - m_tolerance = tolerance; - m_setpoint = 0; - } - - /** - * moves the Turret to a Dynamic position with a Trapezoid profile - * @param position position supplier - * @param requirements subsystem requirements - * @return the command to move the Turret - */ - public Command setPositionCommand(DoubleSupplier position, TrapezoidProfile profile, SubsystemBase... requirements){ - return new RunCommand(() -> { - TrapezoidProfile.State state = profile.calculate( - 0.02, - new TrapezoidProfile.State( - super.logMechanismPosition(), - super.logMechanismVelocity()), - new TrapezoidProfile.State(position.getAsDouble(), 0) - ); - setPosition(state.position); - }, requirements); - } - - public void addLimit(SoftLimit limit){ - this.limit = limit; - } - - /** - * @param position the position to set to the FlyWheel Dynamically - */ - public void setPosition(double position) { - this.m_setpoint = position; - - if (limit != null){ - if (limit instanceof ContinuousSoftLimit) m_setpoint = ((ContinuousSoftLimit) limit).getSetpoint(logMechanismPosition(), position); - else m_setpoint = limit.limit(position); - } - - double pid = m_pidController.calculate(super.m_motor.getMotorVelocity(), m_setpoint); - double ff = m_feedforwardCalculator.apply(super.logMechanismPosition(), super.logMechanismVelocity()); - super.setVoltage(pid + ff); - } -} diff --git a/src/main/java/frc/excalib/mechanism/PositionMechanism.java b/src/main/java/frc/excalib/mechanism/PositionMechanism.java new file mode 100644 index 0000000..d2981f7 --- /dev/null +++ b/src/main/java/frc/excalib/mechanism/PositionMechanism.java @@ -0,0 +1,105 @@ +package frc.excalib.mechanism; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.encoder.Encoder; +import frc.excalib.control.gains.Gains; +import frc.excalib.control.limits.ContinuousSoftLimit; +import frc.excalib.control.limits.SoftLimit; +import frc.excalib.control.motor.Motor; + +import java.util.function.BiFunction; +import java.util.function.DoubleSupplier; + +public class PositionMechanism extends Mechanism { + private double m_setpoint, m_tolerance; + private PIDController m_pidController; + private BiFunction m_feedforwardCalculator; + private SoftLimit m_limit; + + public Trigger m_atSetpointTrigger; + + private PositionMechanism(Builder builder) { + super(builder.motor); + super.addExternalEncoder(builder.externalEncoder); + + this.m_pidController = builder.gains.getPIDcontroller(); + this.m_feedforwardCalculator = builder.feedforwardCalculator; + this.m_tolerance = builder.tolerance; + this.m_limit = builder.limit; + this.m_setpoint = 0; + + this.m_atSetpointTrigger = new Trigger(() -> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), m_tolerance)); + } + + public static final class Builder { + private Motor motor; + private Gains gains; + private double tolerance = 1; + private SoftLimit limit = null; + private Encoder externalEncoder = null; + private BiFunction feedforwardCalculator = (pos, vel) -> 0.0; + + public Builder(Motor motor, Gains gains){ + this.motor = motor; + this.gains = gains; + } + + public Builder withTolerance(double tolerance) { + this.tolerance = tolerance; + return this; + } + + public Builder withFeedforwardCalculator(BiFunction feedforward) { + this.feedforwardCalculator = feedforward; + return this; + } + + public Builder withLimit(SoftLimit limit) { + this.limit = limit; + return this; + } + + public Builder withExternalEncoder(Encoder encoder) { + this.externalEncoder = encoder; + return this; + } + + public PositionMechanism build() { + if (motor == null || gains == null) { + throw new IllegalStateException("Motor and Gains are required for PositionMechanism"); + } + return new PositionMechanism(this); + } + } + + public Command setPositionCommand(DoubleSupplier positionSupplier, TrapezoidProfile profile, SubsystemBase... requirements) { + return new RunCommand(() -> { + State state = profile.calculate( + 0.02, + new State(super.logMechanismPosition(), super.logMechanismVelocity()), + new State(positionSupplier.getAsDouble(), 0) + ); + setPosition(state.position); + }, requirements); + } + + public void setPosition(double position) { + m_setpoint = position; + + if (m_limit != null) { + if (m_limit instanceof ContinuousSoftLimit) m_setpoint = ((ContinuousSoftLimit) m_limit).getSetpoint(super.logMechanismPosition(), position); + else m_setpoint = m_limit.limit(position); + } + + double pid = m_pidController.calculate(super.logMechanismPosition(), m_setpoint); + double ff = m_feedforwardCalculator.apply(super.logMechanismPosition(), super.logMechanismVelocity()); + super.setVoltage(pid + ff); + } +} diff --git a/src/main/java/frc/excalib/mechanism/ControlVelocityMechanism.java b/src/main/java/frc/excalib/mechanism/VelocityMechanism.java similarity index 63% rename from src/main/java/frc/excalib/mechanism/ControlVelocityMechanism.java rename to src/main/java/frc/excalib/mechanism/VelocityMechanism.java index f82ab37..e2ba21e 100644 --- a/src/main/java/frc/excalib/mechanism/ControlVelocityMechanism.java +++ b/src/main/java/frc/excalib/mechanism/VelocityMechanism.java @@ -2,45 +2,43 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.excalib.control.gains.Gains; -import frc.excalib.control.limits.ContinuousSoftLimit; -import frc.excalib.control.limits.SoftLimit; import frc.excalib.control.motor.Motor; import java.util.function.BiFunction; import java.util.function.DoubleSupplier; -public class ControlVelocityMechanism extends Mechanism{ +public class VelocityMechanism extends Mechanism { + private double m_setpoint, m_tolerance; private final PIDController m_pidController; - private BiFunction m_feedforwardCalculator; - - private SoftLimit limit; + private final BiFunction m_feedforwardCalculator; - private double m_setpoint, m_tolerance; - public final Trigger atSetpointTrigger = new Trigger(()-> MathUtil.isNear(this.m_setpoint, super.logMechanismVelocity(), m_tolerance)); + public final Trigger m_atSetpointTrigger; /** - * @param motor the FlyWheel Motor - * @param gains the FF and PID gains + * @param motor the Mechanism motor + * @param gains the FF and PID gains + * @param tolerance setpoint tolerance + * @param feedforwardCalculator FF calculation */ - public ControlVelocityMechanism(Motor motor, Gains gains, double tolerance, - BiFunction feedforwardCalculator) { + public VelocityMechanism(Motor motor, Gains gains, double tolerance, + BiFunction feedforwardCalculator) { super(motor); this.m_pidController = gains.getPIDcontroller(); m_feedforwardCalculator = feedforwardCalculator; - m_tolerance = tolerance; - m_setpoint = 0; + + m_atSetpointTrigger = new Trigger(() -> MathUtil.isNear(this.m_setpoint, super.logMechanismVelocity(), m_tolerance)); } /** * sets the Mechanism to a Dynamic velocity - * @param velocity velocity supplier + * + * @param velocity velocity supplier * @param requirements subsystem requirements * @return the command to set the FlyWheel velocity */ @@ -49,7 +47,7 @@ public Command setVelocityCommand(DoubleSupplier velocity, SubsystemBase... requ } /** - * @param velocity the velocity to set to the FlyWheel Dynamically + * @param velocity the velocity to set to the Mechanism */ public void setVelocity(double velocity) { this.m_setpoint = velocity; diff --git a/src/main/java/frc/robot/subsystems/AdjustableShooter.java b/src/main/java/frc/robot/subsystems/AdjustableShooter.java index 147e546..74a11f0 100644 --- a/src/main/java/frc/robot/subsystems/AdjustableShooter.java +++ b/src/main/java/frc/robot/subsystems/AdjustableShooter.java @@ -7,8 +7,8 @@ import frc.excalib.control.motor.controllers.TalonFXMotor; import frc.excalib.control.motor.motorType.MotorGroup; import frc.excalib.control.motor.motor_specs.DirectionState; -import frc.excalib.mechanism.ControlPositionMechanism; -import frc.excalib.mechanism.ControlVelocityMechanism; +import frc.excalib.mechanism.PositionMechanism; +import frc.excalib.mechanism.VelocityMechanism; import java.util.function.DoubleSupplier; @@ -21,8 +21,8 @@ public class AdjustableShooter extends SubsystemBase{ private final TalonFXMotor hoodMotor; // Subsystem mechanisms - private final ControlPositionMechanism hood; - private final ControlVelocityMechanism shooter; + private final PositionMechanism hood; + private final VelocityMechanism shooter; public AdjustableShooter() { // initialize hardware @@ -40,13 +40,14 @@ public AdjustableShooter() { shooterMotors.setVelocityConversionFactor(TURRET_VELOCITY_CONVERSION_FACTOR); // create turret and arm mechanisms - this.hood = new ControlPositionMechanism(hoodMotor, TURRET_GAINS, TURRET_TOLERANCE, - (position, velocity)-> TURRET_GAINS.ks * Math.signum(velocity)); - this.shooter = new ControlVelocityMechanism(shooterMotors, ARM_GAINS, ARM_TOLERANCE, + this.hood = new PositionMechanism.Builder(hoodMotor, TURRET_GAINS) + .withTolerance(TURRET_TOLERANCE) + .withLimit(TURRET_SOFT_LIMIT) + .withFeedforwardCalculator((position, velocity)-> TURRET_GAINS.ks * Math.signum(velocity)) + .build(); + this.shooter = new VelocityMechanism(shooterMotors, ARM_GAINS, ARM_TOLERANCE, (position, velocity)-> ARM_GAINS.applyGains(new GenericFF.SimpleFF()).calculate(velocity)); - this.hood.addLimit(TURRET_SOFT_LIMIT); - setDefaultCommand(setShooterStateCommand(() -> 0, ()-> 0)); } From 2358fa82510a4f842ef7a6cac4dfaff862d9358a Mon Sep 17 00:00:00 2001 From: TapChap Date: Wed, 21 May 2025 12:10:36 +0300 Subject: [PATCH 17/17] REWRITE: PositionMechanism to utilize a Fluent Initialization deign pattern --- .../additional_utilities/AllianceUtils.java | 6 +- .../excalib/mechanism/PositionMechanism.java | 68 ++++++------------- .../excalib/mechanism/VelocityMechanism.java | 35 +++++++--- .../robot/subsystems/AdjustableShooter.java | 14 ++-- 4 files changed, 56 insertions(+), 67 deletions(-) diff --git a/src/main/java/frc/excalib/additional_utilities/AllianceUtils.java b/src/main/java/frc/excalib/additional_utilities/AllianceUtils.java index 85e92a6..7458227 100644 --- a/src/main/java/frc/excalib/additional_utilities/AllianceUtils.java +++ b/src/main/java/frc/excalib/additional_utilities/AllianceUtils.java @@ -17,6 +17,7 @@ /** * the purpose of this class is too supply different utility functions for functionality * that depends on the robot alliance. + * * @author Shai Grossman */ public class AllianceUtils { @@ -43,6 +44,7 @@ public static boolean isRedAlliance() { /** * Converts a pose to the pose relative to the current driver station alliance. + * * @param bluePose the current blue alliance pose * @return the converted Pose2d pose */ @@ -69,7 +71,7 @@ public static Pose2d mirrorAlliance(Pose2d pose) { public static class AlliancePose { private Pose2d pose; - public AlliancePose(double x, double y, double degrees){ + public AlliancePose(double x, double y, double degrees) { this.pose = new Pose2d(x, y, Rotation2d.fromDegrees(degrees)); } @@ -77,7 +79,7 @@ public AlliancePose(Translation2d translation, Rotation2d rotation) { this.pose = new Pose2d(translation, rotation); } - public AlliancePose(double degrees){ + public AlliancePose(double degrees) { this.pose = new Pose2d(0, 0, Rotation2d.fromDegrees(degrees)); } diff --git a/src/main/java/frc/excalib/mechanism/PositionMechanism.java b/src/main/java/frc/excalib/mechanism/PositionMechanism.java index d2981f7..d8814a3 100644 --- a/src/main/java/frc/excalib/mechanism/PositionMechanism.java +++ b/src/main/java/frc/excalib/mechanism/PositionMechanism.java @@ -18,65 +18,39 @@ import java.util.function.DoubleSupplier; public class PositionMechanism extends Mechanism { - private double m_setpoint, m_tolerance; + private double m_setpoint; private PIDController m_pidController; private BiFunction m_feedforwardCalculator; private SoftLimit m_limit; public Trigger m_atSetpointTrigger; - private PositionMechanism(Builder builder) { - super(builder.motor); - super.addExternalEncoder(builder.externalEncoder); + public PositionMechanism(Motor motor, Gains gains) { + super(motor); + m_pidController = gains.getPIDcontroller(); - this.m_pidController = builder.gains.getPIDcontroller(); - this.m_feedforwardCalculator = builder.feedforwardCalculator; - this.m_tolerance = builder.tolerance; - this.m_limit = builder.limit; - this.m_setpoint = 0; - - this.m_atSetpointTrigger = new Trigger(() -> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), m_tolerance)); + m_atSetpointTrigger = new Trigger(()-> false); + m_feedforwardCalculator = ((a, b)-> 0.0); } - public static final class Builder { - private Motor motor; - private Gains gains; - private double tolerance = 1; - private SoftLimit limit = null; - private Encoder externalEncoder = null; - private BiFunction feedforwardCalculator = (pos, vel) -> 0.0; - - public Builder(Motor motor, Gains gains){ - this.motor = motor; - this.gains = gains; - } - - public Builder withTolerance(double tolerance) { - this.tolerance = tolerance; - return this; - } - - public Builder withFeedforwardCalculator(BiFunction feedforward) { - this.feedforwardCalculator = feedforward; - return this; - } + public PositionMechanism withTolerance(double tolerance) { + m_atSetpointTrigger = new Trigger(() -> MathUtil.isNear(this.m_setpoint, super.logMechanismPosition(), tolerance)); + return this; + } - public Builder withLimit(SoftLimit limit) { - this.limit = limit; - return this; - } + public PositionMechanism withFeedforwardCalculator(BiFunction calculator) { + m_feedforwardCalculator = calculator; + return this; + } - public Builder withExternalEncoder(Encoder encoder) { - this.externalEncoder = encoder; - return this; - } + public PositionMechanism withLimit(SoftLimit limit) { + m_limit = limit; + return this; + } - public PositionMechanism build() { - if (motor == null || gains == null) { - throw new IllegalStateException("Motor and Gains are required for PositionMechanism"); - } - return new PositionMechanism(this); - } + public PositionMechanism withExternalEncoder(Encoder encoder) { + super.addExternalEncoder(encoder); + return this; } public Command setPositionCommand(DoubleSupplier positionSupplier, TrapezoidProfile profile, SubsystemBase... requirements) { diff --git a/src/main/java/frc/excalib/mechanism/VelocityMechanism.java b/src/main/java/frc/excalib/mechanism/VelocityMechanism.java index e2ba21e..9461607 100644 --- a/src/main/java/frc/excalib/mechanism/VelocityMechanism.java +++ b/src/main/java/frc/excalib/mechanism/VelocityMechanism.java @@ -6,33 +6,46 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.excalib.control.encoder.Encoder; import frc.excalib.control.gains.Gains; +import frc.excalib.control.limits.SoftLimit; import frc.excalib.control.motor.Motor; import java.util.function.BiFunction; import java.util.function.DoubleSupplier; public class VelocityMechanism extends Mechanism { - private double m_setpoint, m_tolerance; + private double m_setpoint; private final PIDController m_pidController; - private final BiFunction m_feedforwardCalculator; + private BiFunction m_feedforwardCalculator; - public final Trigger m_atSetpointTrigger; + public Trigger m_atSetpointTrigger; /** * @param motor the Mechanism motor - * @param gains the FF and PID gains - * @param tolerance setpoint tolerance - * @param feedforwardCalculator FF calculation + * @param gains PID gains */ - public VelocityMechanism(Motor motor, Gains gains, double tolerance, - BiFunction feedforwardCalculator) { + public VelocityMechanism(Motor motor, Gains gains) { super(motor); this.m_pidController = gains.getPIDcontroller(); - m_feedforwardCalculator = feedforwardCalculator; - m_tolerance = tolerance; - m_atSetpointTrigger = new Trigger(() -> MathUtil.isNear(this.m_setpoint, super.logMechanismVelocity(), m_tolerance)); + m_atSetpointTrigger = new Trigger(()-> false); + m_feedforwardCalculator = ((a, b)-> 0.0); + } + + public VelocityMechanism withTolerance(double tolerance) { + m_atSetpointTrigger = new Trigger(() -> MathUtil.isNear(this.m_setpoint, super.logMechanismVelocity(), tolerance)); + return this; + } + + public VelocityMechanism withFeedforwardCalculator(BiFunction calculator) { + m_feedforwardCalculator = calculator; + return this; + } + + public VelocityMechanism withExternalEncoder(Encoder encoder) { + super.addExternalEncoder(encoder); + return this; } /** diff --git a/src/main/java/frc/robot/subsystems/AdjustableShooter.java b/src/main/java/frc/robot/subsystems/AdjustableShooter.java index 74a11f0..7aaf258 100644 --- a/src/main/java/frc/robot/subsystems/AdjustableShooter.java +++ b/src/main/java/frc/robot/subsystems/AdjustableShooter.java @@ -40,13 +40,13 @@ public AdjustableShooter() { shooterMotors.setVelocityConversionFactor(TURRET_VELOCITY_CONVERSION_FACTOR); // create turret and arm mechanisms - this.hood = new PositionMechanism.Builder(hoodMotor, TURRET_GAINS) - .withTolerance(TURRET_TOLERANCE) - .withLimit(TURRET_SOFT_LIMIT) - .withFeedforwardCalculator((position, velocity)-> TURRET_GAINS.ks * Math.signum(velocity)) - .build(); - this.shooter = new VelocityMechanism(shooterMotors, ARM_GAINS, ARM_TOLERANCE, - (position, velocity)-> ARM_GAINS.applyGains(new GenericFF.SimpleFF()).calculate(velocity)); + this.hood = new PositionMechanism(hoodMotor, TURRET_GAINS) + .withTolerance(TURRET_TOLERANCE).withLimit(TURRET_SOFT_LIMIT) + .withFeedforwardCalculator((position, velocity)-> TURRET_GAINS.ks * Math.signum(velocity)); + + this.shooter = new VelocityMechanism(shooterMotors, ARM_GAINS) + .withTolerance(ARM_TOLERANCE) + .withFeedforwardCalculator((position, velocity)-> ARM_GAINS.applyGains(new GenericFF.SimpleFF()).calculate(velocity)); setDefaultCommand(setShooterStateCommand(() -> 0, ()-> 0)); }