From 67acf3fbc8681c040eab7dc20747195708a26f85 Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 18 Jan 2024 19:32:49 -0500 Subject: [PATCH 01/57] fix(CI): Fixed workflow file typo --- .github/workflows/main.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yaml b/.github/workflows/main.yaml index edc469f..025a0ad 100644 --- a/.github/workflows/main.yaml +++ b/.github/workflows/main.yaml @@ -44,7 +44,7 @@ jobs: build: runs-on: ubuntu-latest - container: wpilib/roborio-cross-ubuntu:2024-22.04 + container: wpilib/roborio-cross-ubuntu:2024-22.04 steps: - uses: actions/checkout@v3 - name: Add repository to git safe directories From c506f870fd615661374a5f758e57f66b57f15fbe Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 18 Jan 2024 19:46:00 -0500 Subject: [PATCH 02/57] fix(CI): Added BuildConstants --- .../org/robolancers321/BuildConstants.java | 12 +-- vendordeps/REVLib.json | 74 +++++++++++++++++++ 2 files changed, 80 insertions(+), 6 deletions(-) create mode 100644 vendordeps/REVLib.json diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index c52be06..443621a 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 4; - public static final String GIT_SHA = "890b1f13f053a4a882dc48c8de3887bfe2f9294f"; - public static final String GIT_DATE = "2024-01-18 19:40:03 EST"; - public static final String GIT_BRANCH = "dev"; - public static final String BUILD_DATE = "2024-01-18 19:43:26 EST"; - public static final long BUILD_UNIX_TIME = 1705625006821L; + public static final int GIT_REVISION = 3; + public static final String GIT_SHA = "d13c62ef0ceab7cdac174d739f9809756c7f7ca4"; + public static final String GIT_DATE = "2024-01-08 20:26:15 EST"; + public static final String GIT_BRANCH = "dev-launcher-4"; + public static final String BUILD_DATE = "2024-01-18 19:16:36 EST"; + public static final long BUILD_UNIX_TIME = 1705623396997L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..0f3520e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.0", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.0", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From edadb267237b63f212bc655cc176f9bf88377d51 Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 18 Jan 2024 19:20:17 -0500 Subject: [PATCH 03/57] feat(indexer): Added first iteration of indexer subsystem --- .../subsystems/launcher/Indexer.java | 167 ++++++++++++++++++ 1 file changed, 167 insertions(+) create mode 100644 src/main/java/org/robolancers321/subsystems/launcher/Indexer.java diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java new file mode 100644 index 0000000..0715615 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -0,0 +1,167 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.launcher; + +import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; +import static org.robolancers321.util.TunableSet.Tunable.tune; + +import com.revrobotics.*; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.robolancers321.util.TunableSet; + +public class Indexer extends SubsystemBase { + private static Indexer INSTANCE; + + public static Indexer getInstance() { + if (INSTANCE == null) { + INSTANCE = new Indexer(); + } + return INSTANCE; + } + + /* Constants */ + public static final String tunablePrefix = "Tune Indexer"; + public boolean tuning = false; + + private final TunableSet tuner = new TunableSet("Indexer"); + private static int kDeviceID = 0; + + private static int kCurrentLimit = 0; + + private static double kWheelCircumference = 0; + + private static boolean kInverted = false; + private static double kP = 0; + + private static double kI = 0; + + private static double kD = 0; + + private static double kFF = 0; + + private static int kBBC = 0; + + private static double kMinOutput = -1; + + private static double kMaxOutput = 1; + + private static double kMaxRPM = 5700; + private double desiredVelocity; + /*Indexer Motor*/ + private static CANSparkMax m_indexerMotor; + + private static SparkPIDController m_pidController; + + private static AbsoluteEncoder m_absoluteEncoder; + + private static DigitalInput bb = new DigitalInput(kBBC); + + private Indexer() { + m_indexerMotor = new CANSparkMax(kDeviceID, kBrushless); + + configureMotor(); + + m_pidController = m_indexerMotor.getPIDController(); + + m_absoluteEncoder = m_indexerMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); + + m_absoluteEncoder.setVelocityConversionFactor(kWheelCircumference); + } + + private void configureMotor() { + m_indexerMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + m_indexerMotor.setSmartCurrentLimit(kCurrentLimit); + m_pidController.setOutputRange(kMinOutput, kMaxOutput); + + configureMotorValues(); + } + + private void configureMotorValues() { + m_indexerMotor.setInverted(kInverted); + m_absoluteEncoder.setInverted(kInverted); + + m_pidController.setP(kP); + m_pidController.setI(kI); + m_pidController.setD(kD); + m_pidController.setFF(kFF); + } + + private void setTunables() { + // using static tune to avoid double prefix + tuning = tune(tunablePrefix, tuning); + + if (tuning) { + // Changing constant values, this does not matter as the constructor is run once with the + // initial values + kP = tuner.tune("kP", kP); + kI = tuner.tune("kI", kI); + kD = tuner.tune("kD", kD); + kFF = tuner.tune("kFF", kFF); + + kInverted = tuner.tune("motor inverted", kInverted); + + // reset all the tunables + + configureMotorValues(); + } + } + + @Override + public void periodic() { + setTunables(); + doSendables(); + } + + public void setDesiredVelocity(double desiredVelocity) { + this.desiredVelocity = desiredVelocity; + } + + public double getDesiredVelocity() { + return this.desiredVelocity; + } + + public void setSpeedRPM(double rpm) { + m_indexerMotor.set(Math.signum(rpm) * (Math.abs(rpm) / kMaxRPM)); + } + + public void setSpeed(double speed) { + + m_indexerMotor.set(MathUtil.clamp(speed, -1, 1)); + } + + public void intakeJawn() { + m_pidController.setReference(this.desiredVelocity, CANSparkBase.ControlType.kVelocity); + } + // Probably not needed but nice to have + public void outtakeJawn() { + m_pidController.setReference(-this.desiredVelocity, CANSparkBase.ControlType.kVelocity); + } + + public void stopSpinningJawn() { + m_pidController.setReference(0.0, CANSparkBase.ControlType.kVelocity); + } + + public boolean noteDetected() { + return bb.get(); + } + + public double getIntakeVelocity() { + return m_absoluteEncoder.getVelocity(); + } + + public Command manualIndex(double appliedSpeed) { + return run(() -> setSpeed(appliedSpeed)); + } + + private void doSendables() { + SmartDashboard.putNumber("Indexer velocity", getIntakeVelocity()); + SmartDashboard.putNumber("Indexer set speed", m_indexerMotor.get()); + SmartDashboard.putNumber("Indexer voltage", m_indexerMotor.getBusVoltage()); + SmartDashboard.putNumber("Indexer amperage", m_indexerMotor.getOutputCurrent()); + SmartDashboard.putNumber("Indexer motor temp", m_indexerMotor.getMotorTemperature()); + SmartDashboard.putBoolean("Note detected", noteDetected()); + } +} From 215b48430dc68fe3c5298af70fbad3a562b9963c Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 18 Jan 2024 19:21:52 -0500 Subject: [PATCH 04/57] feat(indexer): Added IndexNote command --- .../commands/launcher/IndexNote.java | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100644 src/main/java/org/robolancers321/commands/launcher/IndexNote.java diff --git a/src/main/java/org/robolancers321/commands/launcher/IndexNote.java b/src/main/java/org/robolancers321/commands/launcher/IndexNote.java new file mode 100644 index 0000000..6345944 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/launcher/IndexNote.java @@ -0,0 +1,36 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.launcher; + +import static org.robolancers321.util.TunableSet.Tunable.tune; + +import edu.wpi.first.wpilibj2.command.Command; +import org.robolancers321.subsystems.launcher.Indexer; + +public class IndexNote extends Command { + private final Indexer indexer = Indexer.getInstance(); + + public IndexNote() { + addRequirements(this.indexer); + } + + @Override + public void initialize() { + var velocity = indexer.tuning ? tune("Desired Velocity", 0) : 0; + indexer.setDesiredVelocity(velocity); + } + + @Override + public void execute() { + indexer.intakeJawn(); + } + + @Override + public boolean isFinished() { + return indexer.noteDetected(); + } + + @Override + public void end(boolean interrupted) { + indexer.stopSpinningJawn(); + } +} From b55bff69754332f8afc182901f0f3133e5047f2c Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 18 Jan 2024 19:22:22 -0500 Subject: [PATCH 05/57] feat(pivot): Added first iteration of pivot subsystem --- .../subsystems/launcher/Pivot.java | 184 ++++++++++++++++++ 1 file changed, 184 insertions(+) create mode 100644 src/main/java/org/robolancers321/subsystems/launcher/Pivot.java diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java new file mode 100644 index 0000000..81929ca --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -0,0 +1,184 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.launcher; + +import static org.robolancers321.util.TunableSet.Tunable.tune; + +import com.revrobotics.*; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; +import org.robolancers321.util.TunableSet; + +public class Pivot extends ProfiledPIDSubsystem { + + private static class PivotFeedForward { + private double kG; + + private PivotFeedForward(double kG) { + this.kG = kG; + } + + public void setkG(double kG) { + this.kG = kG; + } + + // setpoint is in degrees + public double calculate(double setpoint) { + return kG * Math.cos(setpoint); + } + } + + private static Pivot INSTANCE; + + public static Pivot getInstance() { + if (INSTANCE == null) { + INSTANCE = new Pivot(); + } + return INSTANCE; + } + + /* Constants */ + + private static String tunablePrefix = "Tune Pivot"; + + private static boolean tuning = false; + + private static TunableSet tuner = new TunableSet("Pivot"); + private static double kP = 0.0; + + private static double kI = 0.0; + + private static double kD = 0.0; + + private static double kG = 0.0; + + private static double kMaxAngle = 270; + + private static double kMinAngle = 0.0; + private static final int kCurrentLimit = 0; + + private static final int kNominalVoltage = 12; + + private static final double kDegreesPerRot = 360; + + private static final double kTolerance = 0.0; + + private static int kDeviceID = 0; + + public enum PivotSetpoint { + SPEAKER(0.0), + AMP(0.0); + + private double angle; + + PivotSetpoint(double angle) { + this.angle = angle; + } + + public double getAngle() { + return this.angle; + } + } + + private static double kMaxVelocity = 0.0; + + private static double kMaxAcceleration = 0.0; + + private static PivotFeedForward pivotFF; + + private static CANSparkMax m_pivotMotor; + + private static AbsoluteEncoder m_absoluteEncoder; + + private Pivot() { + // TODO: Determine real PID values needed and configure them here + // as well as the TrapezoidProfile 'maxVelocity' & 'maxAcceleration' constraints + super( + new ProfiledPIDController( + kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration))); + + pivotFF = new PivotFeedForward(kG); + + m_pivotMotor = new CANSparkMax(kDeviceID, CANSparkLowLevel.MotorType.kBrushless); + + m_absoluteEncoder = m_pivotMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); + + m_absoluteEncoder.setPositionConversionFactor(kDegreesPerRot); + + configureMotor(); + } + + private void configureMotor() { + m_pivotMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + m_pivotMotor.setSmartCurrentLimit(kCurrentLimit); + m_pivotMotor.enableVoltageCompensation(kNominalVoltage); + m_pivotMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, (float) kMaxAngle); + m_pivotMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, (float) kMinAngle); + m_pivotMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); + m_pivotMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); + + super.m_controller.setTolerance(kTolerance); + super.m_controller.enableContinuousInput(kMinAngle, kMinAngle); + } + + // used for tuning + private void configureMotorValues() { + super.m_controller.setPID(kP, kI, kD); + pivotFF.setkG(kG); + } + + private void setTunables() { + tuning = tune(tunablePrefix, tuning); + + if (tuning) { + kP = tuner.tune("kP", kP); + kI = tuner.tune("kI", kI); + kD = tuner.tune("kD", kD); + kG = tuner.tune("kG", kG); + + kMaxAcceleration = tuner.tune("kMaxAcceleration", kMaxAcceleration); + kMaxVelocity = tuner.tune("kMaxVelocity", kMaxVelocity); + + configureMotorValues(); + } + } + + public void setAngleGoal(PivotSetpoint goal) { + super.setGoal(new TrapezoidProfile.State(goal.getAngle(), 0)); + } + + public void setAngleGoal(double goal) { + super.setGoal(new TrapezoidProfile.State(goal, 0)); + } + + @Override + public void periodic() { + setTunables(); + doSendables(); + } + + @Override + public double getMeasurement() { + + return m_absoluteEncoder.getPosition(); + } + + @Override + protected void useOutput(double output, TrapezoidProfile.State setpoint) { + + m_pivotMotor.setVoltage(output + pivotFF.calculate(setpoint.position)); + } + + public boolean atSetpoint() { + return super.m_controller.atSetpoint(); + } + + public void doSendables() { + SmartDashboard.putBoolean("Pivot at setpoint", atSetpoint()); + SmartDashboard.putNumber("Pivot position", getMeasurement()); + SmartDashboard.putNumber("Pivot voltage", m_pivotMotor.getBusVoltage()); + SmartDashboard.putNumber("Pivot amperage", m_pivotMotor.getOutputCurrent()); + SmartDashboard.putNumber("Pivot motor temp", m_pivotMotor.getMotorTemperature()); + } +} From 68deeee61df186d5319a72b35056908bc584d8d9 Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 18 Jan 2024 19:23:05 -0500 Subject: [PATCH 06/57] feat(shooter): Added shooter subsystem skeleton --- .../subsystems/launcher/Shooter.java | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 src/main/java/org/robolancers321/subsystems/launcher/Shooter.java diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java b/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java new file mode 100644 index 0000000..7ce2b32 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java @@ -0,0 +1,24 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.launcher; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Shooter extends SubsystemBase { + private static Shooter INSTANCE; + + public static Shooter getInstance() { + if (INSTANCE == null) { + INSTANCE = new Shooter(); + } + return INSTANCE; + } + + private Shooter() { + // TODO: Set the default command, if any, for this subsystem by calling + // setDefaultCommand(command) + // in the constructor or in the robot coordination class, such as RobotContainer. + // Also, you can call addChild(name, sendableChild) to associate sendables with the + // subsystem + // such as SpeedControllers, Encoders, DigitalInputs, etc. + } +} From 0cb29dda2aab95ee515ff83c0129092f7e571f78 Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 18 Jan 2024 19:23:22 -0500 Subject: [PATCH 07/57] feat(misc): Added tuning utility --- .../org/robolancers321/util/TunableSet.java | 63 +++++++++++++++++++ 1 file changed, 63 insertions(+) create mode 100644 src/main/java/org/robolancers321/util/TunableSet.java diff --git a/src/main/java/org/robolancers321/util/TunableSet.java b/src/main/java/org/robolancers321/util/TunableSet.java new file mode 100644 index 0000000..2181211 --- /dev/null +++ b/src/main/java/org/robolancers321/util/TunableSet.java @@ -0,0 +1,63 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.util; + +import static edu.wpi.first.wpilibj.smartdashboard.SmartDashboard.*; + +public class TunableSet { + public static class Tunable { + public static int tune(String key, int defaultValue) { + var entry = getEntry(key); + if (!entry.exists()) putNumber(key, defaultValue); + return (int) entry.getInteger(defaultValue); + } + + public static double tune(String key, double defaultValue) { + var entry = getEntry(key); + if (!entry.exists()) putNumber(key, defaultValue); + return entry.getDouble(defaultValue); + } + + public static String tune(String key, String defaultValue) { + var entry = getEntry(key); + if (!entry.exists()) putString(key, defaultValue); + return entry.getString(defaultValue); + } + + public static boolean tune(String key, boolean defaultValue) { + var entry = getEntry(key); + if (!entry.exists()) putBoolean(key, defaultValue); + return entry.getBoolean(defaultValue); + } + } + + private String prefix; + + public TunableSet(String prefix) { + this.prefix = prefix; + } + + // !TODO Add prefixes + public int tune(String key, int defaultValue) { + var entry = getEntry(prefix + " " + key); + if (!entry.exists()) putNumber(key, defaultValue); + return (int) entry.getInteger(defaultValue); + } + + public double tune(String key, double defaultValue) { + var entry = getEntry(prefix + " " + key); + if (!entry.exists()) putNumber(key, defaultValue); + return entry.getDouble(defaultValue); + } + + public String tune(String key, String defaultValue) { + var entry = getEntry(prefix + " " + key); + if (!entry.exists()) putString(key, defaultValue); + return entry.getString(defaultValue); + } + + public boolean tune(String key, boolean defaultValue) { + var entry = getEntry(prefix + " " + key); + if (!entry.exists()) putBoolean(key, defaultValue); + return entry.getBoolean(defaultValue); + } +} From 35ba87f32cc46dec1176e7a5e4f02273ae3d18df Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Thu, 18 Jan 2024 22:44:52 -0500 Subject: [PATCH 08/57] major indexer refactoring: renamed members, changed access modifiers, new method abstractions, changed tuning infrastructure. next steps: commands that use the controller and beam break --- .../subsystems/launcher/Indexer.java | 204 ++++++++---------- 1 file changed, 94 insertions(+), 110 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index 0715615..ca8643b 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -2,166 +2,150 @@ package org.robolancers321.subsystems.launcher; import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; -import static org.robolancers321.util.TunableSet.Tunable.tune; + +import java.util.function.DoubleSupplier; import com.revrobotics.*; -import edu.wpi.first.math.MathUtil; +import com.revrobotics.CANSparkBase.ControlType; + import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.robolancers321.util.TunableSet; public class Indexer extends SubsystemBase { - private static Indexer INSTANCE; + /* + * Singleton + */ + private static Indexer instance; public static Indexer getInstance() { - if (INSTANCE == null) { - INSTANCE = new Indexer(); - } - return INSTANCE; - } - - /* Constants */ - public static final String tunablePrefix = "Tune Indexer"; - public boolean tuning = false; - - private final TunableSet tuner = new TunableSet("Indexer"); - private static int kDeviceID = 0; - - private static int kCurrentLimit = 0; - - private static double kWheelCircumference = 0; - - private static boolean kInverted = false; - private static double kP = 0; - - private static double kI = 0; + if (instance == null) instance = new Indexer(); - private static double kD = 0; - - private static double kFF = 0; + return instance; + } - private static int kBBC = 0; + /* + * Constants + */ - private static double kMinOutput = -1; + private static final int kMotorPort = 0; + private static final int kBeamBreakPort = 0; - private static double kMaxOutput = 1; + private static final boolean kInvertMotor = false; + private static final int kCurrentLimit = 20; - private static double kMaxRPM = 5700; - private double desiredVelocity; - /*Indexer Motor*/ - private static CANSparkMax m_indexerMotor; + private static final double kP = 0; + private static final double kI = 0; + private static final double kD = 0; + private static final double kFF = 0; - private static SparkPIDController m_pidController; + private static final double kMaxRPM = 5700; + private static final double kIntakeSpeedRPM = 500; + private static final double kOuttakeSpeedRPM = -500; + + /* + * Implementation + */ - private static AbsoluteEncoder m_absoluteEncoder; + private CANSparkMax motor; + private SparkPIDController controller; + private RelativeEncoder encoder; - private static DigitalInput bb = new DigitalInput(kBBC); + private DigitalInput beamBreak; private Indexer() { - m_indexerMotor = new CANSparkMax(kDeviceID, kBrushless); - - configureMotor(); + this.motor = new CANSparkMax(kMotorPort, kBrushless); + this.encoder = this.motor.getEncoder(); + this.controller = this.motor.getPIDController(); - m_pidController = m_indexerMotor.getPIDController(); + this.beamBreak = new DigitalInput(kBeamBreakPort); - m_absoluteEncoder = m_indexerMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); - - m_absoluteEncoder.setVelocityConversionFactor(kWheelCircumference); + this.configureMotor(); + this.configureEncoder(); + this.configureController(); } private void configureMotor() { - m_indexerMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); - m_indexerMotor.setSmartCurrentLimit(kCurrentLimit); - m_pidController.setOutputRange(kMinOutput, kMaxOutput); - - configureMotorValues(); + this.motor.setInverted(kInvertMotor); + this.motor.setIdleMode(CANSparkBase.IdleMode.kBrake); + this.motor.setSmartCurrentLimit(kCurrentLimit); + this.motor.enableVoltageCompensation(12); } - private void configureMotorValues() { - m_indexerMotor.setInverted(kInverted); - m_absoluteEncoder.setInverted(kInverted); - - m_pidController.setP(kP); - m_pidController.setI(kI); - m_pidController.setD(kD); - m_pidController.setFF(kFF); - } - - private void setTunables() { - // using static tune to avoid double prefix - tuning = tune(tunablePrefix, tuning); - - if (tuning) { - // Changing constant values, this does not matter as the constructor is run once with the - // initial values - kP = tuner.tune("kP", kP); - kI = tuner.tune("kI", kI); - kD = tuner.tune("kD", kD); - kFF = tuner.tune("kFF", kFF); - - kInverted = tuner.tune("motor inverted", kInverted); - - // reset all the tunables - - configureMotorValues(); - } + private void configureEncoder(){ + this.encoder.setInverted(kInvertMotor); } - @Override - public void periodic() { - setTunables(); - doSendables(); + private void configureController() { + this.controller.setP(kP); + this.controller.setI(kI); + this.controller.setD(kD); + this.controller.setFF(kFF); } - public void setDesiredVelocity(double desiredVelocity) { - this.desiredVelocity = desiredVelocity; + public double getIntakeVelocityRPM() { + return this.encoder.getVelocity(); } - public double getDesiredVelocity() { - return this.desiredVelocity; + public boolean jawnDetected() { + return this.beamBreak.get(); } - public void setSpeedRPM(double rpm) { - m_indexerMotor.set(Math.signum(rpm) * (Math.abs(rpm) / kMaxRPM)); + public void dangerouslySetRPM(double rpm) { + this.motor.set(rpm / kMaxRPM); } - public void setSpeed(double speed) { - - m_indexerMotor.set(MathUtil.clamp(speed, -1, 1)); + public void dangerouslySetSpeed(double speed) { + this.motor.set(speed); } public void intakeJawn() { - m_pidController.setReference(this.desiredVelocity, CANSparkBase.ControlType.kVelocity); + this.controller.setReference(kIntakeSpeedRPM, ControlType.kVelocity); } - // Probably not needed but nice to have + public void outtakeJawn() { - m_pidController.setReference(-this.desiredVelocity, CANSparkBase.ControlType.kVelocity); + this.controller.setReference(kOuttakeSpeedRPM, ControlType.kVelocity); } public void stopSpinningJawn() { - m_pidController.setReference(0.0, CANSparkBase.ControlType.kVelocity); - } - - public boolean noteDetected() { - return bb.get(); + this.controller.setReference(0.0, ControlType.kVelocity); } - public double getIntakeVelocity() { - return m_absoluteEncoder.getVelocity(); + public Command manualIndex(DoubleSupplier appliedSpeedSupplier) { + return run(() -> dangerouslySetSpeed(appliedSpeedSupplier.getAsDouble())); } public Command manualIndex(double appliedSpeed) { - return run(() -> setSpeed(appliedSpeed)); + return this.manualIndex(() -> appliedSpeed); } private void doSendables() { - SmartDashboard.putNumber("Indexer velocity", getIntakeVelocity()); - SmartDashboard.putNumber("Indexer set speed", m_indexerMotor.get()); - SmartDashboard.putNumber("Indexer voltage", m_indexerMotor.getBusVoltage()); - SmartDashboard.putNumber("Indexer amperage", m_indexerMotor.getOutputCurrent()); - SmartDashboard.putNumber("Indexer motor temp", m_indexerMotor.getMotorTemperature()); - SmartDashboard.putBoolean("Note detected", noteDetected()); - } -} + SmartDashboard.putNumber("Indexer Velocity (rpm)", this.getIntakeVelocityRPM()); + SmartDashboard.putBoolean("Note Detected", this.jawnDetected()); + } + + @Override + public void periodic() { + this.doSendables(); + } + + public void initTuning(){ + SmartDashboard.putNumber("indexer kp", SmartDashboard.getNumber("indexer kp", kP)); + SmartDashboard.putNumber("indexer kp", SmartDashboard.getNumber("indexer ki", kI)); + SmartDashboard.putNumber("indexer kp", SmartDashboard.getNumber("indexer kd", kD)); + SmartDashboard.putNumber("indexer kff", SmartDashboard.getNumber("indexer kff", kFF)); + } + + public void tune(){ + double tunedP = SmartDashboard.getNumber("indexer kp", kP); + double tunedI = SmartDashboard.getNumber("indexer ki", kI); + double tunedD = SmartDashboard.getNumber("indexer kd", kD); + double tunedFF = SmartDashboard.getNumber("indexer kff", kFF); + + this.controller.setP(tunedP); + this.controller.setI(tunedI); + this.controller.setD(tunedD); + this.controller.setFF(tunedFF); + } +} \ No newline at end of file From e3bdc4b7e8dc83b003b673ab83dbcf9ffe554aaa Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Thu, 18 Jan 2024 22:47:35 -0500 Subject: [PATCH 09/57] major pivot refactoring: renamed members, changed access modifiers, new method abstractions, changed tuning infrastructure, changed controller implementation. next steps: commands to control movement --- .../subsystems/launcher/Pivot.java | 243 +++++++++--------- 1 file changed, 125 insertions(+), 118 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 81929ca..5e5adae 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -1,74 +1,63 @@ /* (C) Robolancers 2024 */ package org.robolancers321.subsystems.launcher; -import static org.robolancers321.util.TunableSet.Tunable.tune; +import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; import com.revrobotics.*; +import com.revrobotics.SparkAbsoluteEncoder.Type; + +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; -import org.robolancers321.util.TunableSet; - -public class Pivot extends ProfiledPIDSubsystem { - - private static class PivotFeedForward { - private double kG; - - private PivotFeedForward(double kG) { - this.kG = kG; - } +import edu.wpi.first.wpilibj2.command.SubsystemBase; - public void setkG(double kG) { - this.kG = kG; - } +public class Pivot extends SubsystemBase { + /* + * Singleton + */ - // setpoint is in degrees - public double calculate(double setpoint) { - return kG * Math.cos(setpoint); - } - } - - private static Pivot INSTANCE; + private static Pivot instance; public static Pivot getInstance() { - if (INSTANCE == null) { - INSTANCE = new Pivot(); - } - return INSTANCE; - } - - /* Constants */ - - private static String tunablePrefix = "Tune Pivot"; - - private static boolean tuning = false; - - private static TunableSet tuner = new TunableSet("Pivot"); - private static double kP = 0.0; + if (instance == null) instance = new Pivot(); - private static double kI = 0.0; - - private static double kD = 0.0; + return instance; + } + + /* + * Constants + */ - private static double kG = 0.0; + private static final int kMotorPort = 0; - private static double kMaxAngle = 270; + private static final boolean kInvertMotor = false; + private static final boolean kInvertEncoder = false; + private static final int kCurrentLimit = 40; - private static double kMinAngle = 0.0; - private static final int kCurrentLimit = 0; + private static final double kGearRatio = 360.0; + private static final double kRotPerMinToDegPerSec = kGearRatio / 60.0; - private static final int kNominalVoltage = 12; + private static final float kMinAngle = 0.0f; + private static final float kMaxAngle = 270.0f; - private static final double kDegreesPerRot = 360; + private static final double kS = 0.0; + private static final double kG = 0.0; + private static final double kV = 0.0; - private static final double kTolerance = 0.0; + private static final double kP = 0.0; + private static final double kI = 0.0; + private static final double kD = 0.0; + private static final double kMaxVelocityDeg = 0.0; + private static final double kMaxAccelerationDeg = 0.0; - private static int kDeviceID = 0; + private static final double kToleranceDeg = 0.0; - public enum PivotSetpoint { - SPEAKER(0.0), - AMP(0.0); + private static enum PivotSetpoint { + kRetracted(0.0), + kMating(0.0), + kSpeaker(0.0), + kAmp(0.0); private double angle; @@ -80,105 +69,123 @@ public double getAngle() { return this.angle; } } + + /* + * Implementation + */ - private static double kMaxVelocity = 0.0; - - private static double kMaxAcceleration = 0.0; - - private static PivotFeedForward pivotFF; - - private static CANSparkMax m_pivotMotor; - - private static AbsoluteEncoder m_absoluteEncoder; + private CANSparkMax motor; + private AbsoluteEncoder absoluteEncoder; + private ArmFeedforward feedforwardController; + private ProfiledPIDController feedbackController; private Pivot() { - // TODO: Determine real PID values needed and configure them here - // as well as the TrapezoidProfile 'maxVelocity' & 'maxAcceleration' constraints - super( - new ProfiledPIDController( - kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration))); + this.motor = new CANSparkMax(kMotorPort, kBrushless); + this.absoluteEncoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); + this.feedforwardController = new ArmFeedforward(kS, kG, kV); + this.feedbackController = new ProfiledPIDController(kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocityDeg, kMaxAccelerationDeg)); + + this.configureMotor(); + this.configureEncoder(); + this.configureController(); + } - pivotFF = new PivotFeedForward(kG); + private void configureMotor() { + this.motor.setInverted(kInvertMotor); + this.motor.setIdleMode(CANSparkBase.IdleMode.kBrake); + this.motor.setSmartCurrentLimit(kCurrentLimit); + this.motor.enableVoltageCompensation(12); + + this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, (float) kMaxAngle); + this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, (float) kMinAngle); + this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); + this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); + } - m_pivotMotor = new CANSparkMax(kDeviceID, CANSparkLowLevel.MotorType.kBrushless); + private void configureEncoder(){ + this.absoluteEncoder.setInverted(kInvertEncoder); + this.absoluteEncoder.setPositionConversionFactor(kGearRatio); + this.absoluteEncoder.setVelocityConversionFactor(kRotPerMinToDegPerSec); + } - m_absoluteEncoder = m_pivotMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); + private void configureController(){ + this.feedbackController.setTolerance(kToleranceDeg); + } - m_absoluteEncoder.setPositionConversionFactor(kDegreesPerRot); + public double getPositionDeg(){ + return this.absoluteEncoder.getPosition(); + } - configureMotor(); + public double getPositionRad(){ + return this.getPositionDeg() * Math.PI / 180.0; } - private void configureMotor() { - m_pivotMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); - m_pivotMotor.setSmartCurrentLimit(kCurrentLimit); - m_pivotMotor.enableVoltageCompensation(kNominalVoltage); - m_pivotMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, (float) kMaxAngle); - m_pivotMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, (float) kMinAngle); - m_pivotMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); - m_pivotMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); + public double getVelocityDeg(){ + return this.absoluteEncoder.getVelocity(); + } - super.m_controller.setTolerance(kTolerance); - super.m_controller.enableContinuousInput(kMinAngle, kMinAngle); + public double getVelocityRad(){ + return this.getVelocityDeg() * Math.PI / 180.0; } - // used for tuning - private void configureMotorValues() { - super.m_controller.setPID(kP, kI, kD); - pivotFF.setkG(kG); + private boolean atGoal(){ + return this.feedbackController.atGoal(); } - private void setTunables() { - tuning = tune(tunablePrefix, tuning); + private void setGoal(double goal){ + this.feedbackController.setGoal(new TrapezoidProfile.State(goal, 0.0)); + } - if (tuning) { - kP = tuner.tune("kP", kP); - kI = tuner.tune("kI", kI); - kD = tuner.tune("kD", kD); - kG = tuner.tune("kG", kG); + private void setGoal(PivotSetpoint goal){ + this.setGoal(goal.getAngle()); + } - kMaxAcceleration = tuner.tune("kMaxAcceleration", kMaxAcceleration); - kMaxVelocity = tuner.tune("kMaxVelocity", kMaxVelocity); + private void useController(){ + // TODO: docs are not specific as to units of measurement for velocity + double feedforwardOutput = this.feedforwardController.calculate(this.getPositionRad(), this.getVelocityRad()); - configureMotorValues(); - } - } + double feedbackOutput = this.feedbackController.calculate(this.getPositionDeg()); - public void setAngleGoal(PivotSetpoint goal) { - super.setGoal(new TrapezoidProfile.State(goal.getAngle(), 0)); + double controllerOutput = feedforwardOutput + feedbackOutput; + + this.motor.setVoltage(controllerOutput); } - public void setAngleGoal(double goal) { - super.setGoal(new TrapezoidProfile.State(goal, 0)); + public void doSendables(){ + SmartDashboard.putBoolean("Pivot At Goal", this.atGoal()); + SmartDashboard.putNumber("Pivot Position (deg)", this.getPositionDeg()); } @Override - public void periodic() { - setTunables(); - doSendables(); + public void periodic(){ + this.useController(); + this.doSendables(); } - @Override - public double getMeasurement() { + public void initTuning(){ + SmartDashboard.putNumber("pivot ks", SmartDashboard.getNumber("pivot ks", kS)); + SmartDashboard.putNumber("pivot kg", SmartDashboard.getNumber("pivot kg", kG)); + SmartDashboard.putNumber("pivot kv", SmartDashboard.getNumber("pivot kv", kV)); - return m_absoluteEncoder.getPosition(); + SmartDashboard.putNumber("pivot kp", SmartDashboard.getNumber("pivot kp", kP)); + SmartDashboard.putNumber("pivot ki", SmartDashboard.getNumber("pivot ki", kI)); + SmartDashboard.putNumber("pivot kd", SmartDashboard.getNumber("pivot kd", kD)); } - @Override - protected void useOutput(double output, TrapezoidProfile.State setpoint) { + public void tune(){ + double tunedS = SmartDashboard.getNumber("pivot ks", kS); + double tunedG = SmartDashboard.getNumber("pivot kg", kG); + double tunedV = SmartDashboard.getNumber("pivot kv", kV); - m_pivotMotor.setVoltage(output + pivotFF.calculate(setpoint.position)); - } + // TODO: is there a better way to update this? + if(tunedS != this.feedforwardController.ks || tunedG != this.feedforwardController.kg || tunedV != this.feedforwardController.kv){ + this.feedforwardController = new ArmFeedforward(tunedS, tunedG, tunedV); + } - public boolean atSetpoint() { - return super.m_controller.atSetpoint(); - } + double tunedP = SmartDashboard.getNumber("pivot kp", kP); + double tunedI = SmartDashboard.getNumber("pivot ki", kI); + double tunedD = SmartDashboard.getNumber("pivot kd", kD); - public void doSendables() { - SmartDashboard.putBoolean("Pivot at setpoint", atSetpoint()); - SmartDashboard.putNumber("Pivot position", getMeasurement()); - SmartDashboard.putNumber("Pivot voltage", m_pivotMotor.getBusVoltage()); - SmartDashboard.putNumber("Pivot amperage", m_pivotMotor.getOutputCurrent()); - SmartDashboard.putNumber("Pivot motor temp", m_pivotMotor.getMotorTemperature()); + this.feedbackController.setPID(tunedP, tunedI, tunedD); } } From 1f36c624a19b310669d569fe974fd239d578c37b Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Thu, 18 Jan 2024 23:12:04 -0500 Subject: [PATCH 10/57] fixed use controller to use motion profile setpoint as feed forward input --- .../java/org/robolancers321/subsystems/launcher/Pivot.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 5e5adae..e5a53eb 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -141,8 +142,8 @@ private void setGoal(PivotSetpoint goal){ } private void useController(){ - // TODO: docs are not specific as to units of measurement for velocity - double feedforwardOutput = this.feedforwardController.calculate(this.getPositionRad(), this.getVelocityRad()); + State setpointState = this.feedbackController.getSetpoint(); + double feedforwardOutput = this.feedforwardController.calculate(setpointState.position, setpointState.velocity); double feedbackOutput = this.feedbackController.calculate(this.getPositionDeg()); From bf8f4759fa4e3b5c17b1b41cd9dcfb9d45148d44 Mon Sep 17 00:00:00 2001 From: koolfyn Date: Sat, 20 Jan 2024 13:06:20 -0500 Subject: [PATCH 11/57] intakeretractor feat? --- README.md | 2 +- build.gradle | 18 +-- .../org/robolancers321/BuildConstants.java | 18 +++ .../subsystems/IntakeRetractor.java | 109 ++++++++++++++++++ .../commands/retractor/retractIntake.java | 9 ++ vendordeps/REVLib.json | 74 ++++++++++++ 6 files changed, 220 insertions(+), 10 deletions(-) create mode 100644 src/main/java/org/robolancers321/BuildConstants.java create mode 100644 src/main/java/org/robolancers321/subsystems/IntakeRetractor.java create mode 100644 src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java create mode 100644 vendordeps/REVLib.json diff --git a/README.md b/README.md index ecf7e73..2212a7a 100644 --- a/README.md +++ b/README.md @@ -1 +1 @@ -## WIP \ No newline at end of file +## WIP diff --git a/build.gradle b/build.gradle index 1f93411..5b90b01 100644 --- a/build.gradle +++ b/build.gradle @@ -156,12 +156,12 @@ spotless { } } -project.compileJava.dependsOn(createVersionFile) -gversion { - srcDir = "src/main/java/" - classPackage = "org.robolancers321" - className = "BuildConstants" - dateFormat = "yyyy-MM-dd HH:mm:ss z" - timeZone = "America/New_York" // Use preferred time zone - indent = " " -} +// project.compileJava.dependsOn(createVersionFile) +// // gversion { +// // srcDir = "src/main/java/" +// // classPackage = "org.robolancers321" +// // className = "BuildConstants" +// // dateFormat = "yyyy-MM-dd HH:mm:ss z" +// // timeZone = "America/New_York" // Use preferred time zone +// // indent = " " +// // } diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java new file mode 100644 index 0000000..f5162d2 --- /dev/null +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -0,0 +1,18 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321; + +/** Automatically generated file containing build version information. */ +public final class BuildConstants { + public static final String MAVEN_GROUP = ""; + public static final String MAVEN_NAME = "321-Crescendo-2024"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 4; + public static final String GIT_SHA = "2de603cd5ac29e8182034c832bc185d3a6f14393"; + public static final String GIT_DATE = "2024-01-08 20:28:49 EST"; + public static final String GIT_BRANCH = "main"; + public static final String BUILD_DATE = "2024-01-10 17:47:24 EST"; + public static final long BUILD_UNIX_TIME = 1704926844560L; + public static final int DIRTY = 1; + + private BuildConstants() {} +} diff --git a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java new file mode 100644 index 0000000..a79a6e4 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java @@ -0,0 +1,109 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems; +import edu.wpi.first.math.proto.Controller; +import edu.wpi.first.wpilibj.DigitalInput; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; + +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class IntakeRetractor extends SubsystemBase { + + public CANSparkMax retractorMotor; + private DigitalInput retractorLimitSwitch; + private DigitalInput retractorBeamBreak; + private Trigger retractorTrigger; + SparkPIDController retractorPIDController; + AbsoluteEncoder retractorEncoder1; + RelativeEncoder retractorEncoder2; + + public double referencePosition = 0; + public double retractPosition = 90; + + public static final double kP = 0.000; + public static final double kI = 0.000; + public static final double kD = 0.000; + public static final double kFF = 0.0001; + public static final double kErrorThreshold = 2.0; + + public static final int kPort = 0; + public static final int kRetractorLimitSwitch = 0; + private static final Type kDutyCycle = null; + + + public IntakeRetractor() { + this.retractorMotor = new CANSparkMax(kPort, MotorType.kBrushless); + this.retractorEncoder1 = retractorMotor.getAbsoluteEncoder(kDutyCycle); + this.retractorEncoder2 = retractorMotor.getEncoder(); + retractorPIDController = retractorMotor.getPIDController(); + retractorLimitSwitch = new DigitalInput(0); + retractorBeamBreak = new DigitalInput(0); + + this.retractorEncoder1.setPositionConversionFactor(1.0); + + retractorPIDController.setD(kD); + retractorPIDController.setP(kP); + retractorPIDController.setI(kI); + + this.retractorMotor.setSmartCurrentLimit(20); + + + // this.retractorLimitSwitch = new DigitalInput(kRetractorLimitSwitch); + // this.retractorTrigger = new Trigger(this::limitSwitchTriggered); + // this.retractorTrigger.whileTrue(new InstantCommand(() -> { + // this.resetEncoder(); + // }, this)); + } + + @Override + public void periodic() { + + retractorPIDController.setD(kD); + retractorPIDController.setP(kP); + retractorPIDController.setI(kI); + retractorPIDController.setFF(kFF); + + retractorMotor.getOutputCurrent(); + retractorMotor.set(100); + + // pivotcontrolleroutput = controller.calculate(stuff) + // pivotMotor.set(Output) + + } + +// public boolean limitSwitchTriggered() { +// return !this.retractorLimitSwitch.get(); +// } + + +public void setRetractorPosition() { + retractorEncoder2.setPosition(retractPosition); +} + +public double getRetractPosition() { + return retractorEncoder2.getPosition(); +} + +public boolean isRetractorAtCorrectPos(double position) { + return Math.abs(this.getRetractPosition() - position) < kErrorThreshold; +} + + +public void resetEncoder() { + retractorEncoder2.setPosition(0); +} + + public boolean isBeamBroken() { + return !this.retractorBeamBreak.get(); + } + +} diff --git a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java new file mode 100644 index 0000000..2df6665 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java @@ -0,0 +1,9 @@ +package org.robolancers321.subsystems.commands.retractor; + +import edu.wpi.first.wpilibj2.command.Command; +import org.robolancers321.subsystems.IntakeRetractor; + +public class retractIntake extends Command { + public IntakeRetractor intakeRetractor; + +} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..0f3520e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.0", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.0", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From c279ee0ba899d80fd3fb6014938537d48d90cc27 Mon Sep 17 00:00:00 2001 From: koolfyn Date: Sat, 20 Jan 2024 13:18:23 -0500 Subject: [PATCH 12/57] intakeretractor: feat?? --- .../subsystems/IntakeRetractor.java | 27 ++++++++++--------- .../commands/retractor/retractIntake.java | 23 ++++++++++++++++ 2 files changed, 38 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java index a79a6e4..bdcd164 100644 --- a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java +++ b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java @@ -26,8 +26,8 @@ public class IntakeRetractor extends SubsystemBase { AbsoluteEncoder retractorEncoder1; RelativeEncoder retractorEncoder2; - public double referencePosition = 0; - public double retractPosition = 90; + public double extendedPosition = 90; + public double retractedPosition = 0; public static final double kP = 0.000; public static final double kI = 0.000; @@ -36,6 +36,7 @@ public class IntakeRetractor extends SubsystemBase { public static final double kErrorThreshold = 2.0; public static final int kPort = 0; + public static final int kRetractorBeamBreakPort = 0; public static final int kRetractorLimitSwitch = 0; private static final Type kDutyCycle = null; @@ -45,9 +46,9 @@ public IntakeRetractor() { this.retractorEncoder1 = retractorMotor.getAbsoluteEncoder(kDutyCycle); this.retractorEncoder2 = retractorMotor.getEncoder(); retractorPIDController = retractorMotor.getPIDController(); + this.retractorBeamBreak = new DigitalInput(kRetractorBeamBreakPort); retractorLimitSwitch = new DigitalInput(0); - retractorBeamBreak = new DigitalInput(0); - + this.retractorEncoder1.setPositionConversionFactor(1.0); retractorPIDController.setD(kD); @@ -84,26 +85,28 @@ public void periodic() { // return !this.retractorLimitSwitch.get(); // } +public void retractedPosition() { + retractorEncoder2.setPosition(retractedPosition); +} -public void setRetractorPosition() { - retractorEncoder2.setPosition(retractPosition); +public void extendRetractor() { + retractorEncoder2.setPosition(extendedPosition); } public double getRetractPosition() { return retractorEncoder2.getPosition(); } -public boolean isRetractorAtCorrectPos(double position) { +public boolean RetractorAtCorrectPos(double position) { return Math.abs(this.getRetractPosition() - position) < kErrorThreshold; -} - +} public void resetEncoder() { retractorEncoder2.setPosition(0); } - public boolean isBeamBroken() { - return !this.retractorBeamBreak.get(); - } +public boolean beamBroken() { + return !this.retractorBeamBreak.get(); +} } diff --git a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java index 2df6665..299a2f0 100644 --- a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java +++ b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java @@ -5,5 +5,28 @@ public class retractIntake extends Command { public IntakeRetractor intakeRetractor; + + public retractIntake() { + addRequirements(this.intakeRetractor); + } + @Override + public void initialize() { + intakeRetractor.resetEncoder(); + } + + @Override + public void execute() { + intakeRetractor.extendRetractor(); + } + + @Override + public boolean isFinished() { + return intakeRetractor.beamBroken(); + } + + @Override + public void end(boolean interrupted) { + intakeRetractor.retractedPosition(); + } } From 995665ccda84593c46c7ec4fa5e5c4dce1bafcff Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Sat, 20 Jan 2024 14:34:38 -0500 Subject: [PATCH 13/57] major refactoring: singleton pattern, changed a lot of names and modifiers, added abstractions for configuration, and added framework for feedforward control and tuning. next steps: small changes where indicated and implement feedforward control, motion profiling, and tuning --- .../commands/intake/RetractIntake.java | 28 +++ .../subsystems/IntakeRetractor.java | 112 ---------- .../commands/retractor/retractIntake.java | 32 --- .../subsystems/intake/IntakeRetractor.java | 204 ++++++++++++++++++ 4 files changed, 232 insertions(+), 144 deletions(-) create mode 100644 src/main/java/org/robolancers321/commands/intake/RetractIntake.java delete mode 100644 src/main/java/org/robolancers321/subsystems/IntakeRetractor.java delete mode 100644 src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java create mode 100644 src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java diff --git a/src/main/java/org/robolancers321/commands/intake/RetractIntake.java b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java new file mode 100644 index 0000000..a68d09d --- /dev/null +++ b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java @@ -0,0 +1,28 @@ +package org.robolancers321.commands.intake; + +import org.robolancers321.subsystems.intake.IntakeRetractor; +import org.robolancers321.subsystems.intake.IntakeRetractor.RetractorSetpoint; + +import edu.wpi.first.wpilibj2.command.Command; + +public class RetractIntake extends Command { + private IntakeRetractor intakeRetractor; + + public RetractIntake() { + this.intakeRetractor = IntakeRetractor.getInstance(); + + addRequirements(this.intakeRetractor); + } + + @Override + public void initialize() { + this.intakeRetractor.setSetpoint(RetractorSetpoint.kRetracted); + } + + @Override + public boolean isFinished() { + // TODO: encoder vs limit switch? limit switch MAY be more accurate but is also subject to physical damage + + return this.intakeRetractor.isAtSetpoint(); + } +} diff --git a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java deleted file mode 100644 index bdcd164..0000000 --- a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java +++ /dev/null @@ -1,112 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.subsystems; -import edu.wpi.first.math.proto.Controller; -import edu.wpi.first.wpilibj.DigitalInput; - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkAbsoluteEncoder.Type; -import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkLowLevel.MotorType; - -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -public class IntakeRetractor extends SubsystemBase { - - public CANSparkMax retractorMotor; - private DigitalInput retractorLimitSwitch; - private DigitalInput retractorBeamBreak; - private Trigger retractorTrigger; - SparkPIDController retractorPIDController; - AbsoluteEncoder retractorEncoder1; - RelativeEncoder retractorEncoder2; - - public double extendedPosition = 90; - public double retractedPosition = 0; - - public static final double kP = 0.000; - public static final double kI = 0.000; - public static final double kD = 0.000; - public static final double kFF = 0.0001; - public static final double kErrorThreshold = 2.0; - - public static final int kPort = 0; - public static final int kRetractorBeamBreakPort = 0; - public static final int kRetractorLimitSwitch = 0; - private static final Type kDutyCycle = null; - - - public IntakeRetractor() { - this.retractorMotor = new CANSparkMax(kPort, MotorType.kBrushless); - this.retractorEncoder1 = retractorMotor.getAbsoluteEncoder(kDutyCycle); - this.retractorEncoder2 = retractorMotor.getEncoder(); - retractorPIDController = retractorMotor.getPIDController(); - this.retractorBeamBreak = new DigitalInput(kRetractorBeamBreakPort); - retractorLimitSwitch = new DigitalInput(0); - - this.retractorEncoder1.setPositionConversionFactor(1.0); - - retractorPIDController.setD(kD); - retractorPIDController.setP(kP); - retractorPIDController.setI(kI); - - this.retractorMotor.setSmartCurrentLimit(20); - - - // this.retractorLimitSwitch = new DigitalInput(kRetractorLimitSwitch); - // this.retractorTrigger = new Trigger(this::limitSwitchTriggered); - // this.retractorTrigger.whileTrue(new InstantCommand(() -> { - // this.resetEncoder(); - // }, this)); - } - - @Override - public void periodic() { - - retractorPIDController.setD(kD); - retractorPIDController.setP(kP); - retractorPIDController.setI(kI); - retractorPIDController.setFF(kFF); - - retractorMotor.getOutputCurrent(); - retractorMotor.set(100); - - // pivotcontrolleroutput = controller.calculate(stuff) - // pivotMotor.set(Output) - - } - -// public boolean limitSwitchTriggered() { -// return !this.retractorLimitSwitch.get(); -// } - -public void retractedPosition() { - retractorEncoder2.setPosition(retractedPosition); -} - -public void extendRetractor() { - retractorEncoder2.setPosition(extendedPosition); -} - -public double getRetractPosition() { - return retractorEncoder2.getPosition(); -} - -public boolean RetractorAtCorrectPos(double position) { - return Math.abs(this.getRetractPosition() - position) < kErrorThreshold; -} - -public void resetEncoder() { - retractorEncoder2.setPosition(0); -} - -public boolean beamBroken() { - return !this.retractorBeamBreak.get(); -} - -} diff --git a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java deleted file mode 100644 index 299a2f0..0000000 --- a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.robolancers321.subsystems.commands.retractor; - -import edu.wpi.first.wpilibj2.command.Command; -import org.robolancers321.subsystems.IntakeRetractor; - -public class retractIntake extends Command { - public IntakeRetractor intakeRetractor; - - public retractIntake() { - addRequirements(this.intakeRetractor); - } - - @Override - public void initialize() { - intakeRetractor.resetEncoder(); - } - - @Override - public void execute() { - intakeRetractor.extendRetractor(); - } - - @Override - public boolean isFinished() { - return intakeRetractor.beamBroken(); - } - - @Override - public void end(boolean interrupted) { - intakeRetractor.retractedPosition(); - } -} diff --git a/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java new file mode 100644 index 0000000..0243641 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java @@ -0,0 +1,204 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.intake; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class IntakeRetractor extends SubsystemBase { + /* + * Singleton + */ + + private static IntakeRetractor instance = null; + + public static IntakeRetractor getInstance(){ + if (instance == null) instance = new IntakeRetractor(); + + return instance; + } + + /* + * Constants + */ + + private static final int kMotorPort = 0; + private static final int kLimitSwitchPort = 0; + private static final int kBeamBreakPort = 0; + + // TODO: put constants for motor and encoder configuration here + + private static final double kP = 0.000; + private static final double kI = 0.000; + private static final double kD = 0.000; + + // TODO: feed forward controller using wpilib ArmFeedforward class + + private static final double kErrorThreshold = 2.0; + + public static enum RetractorSetpoint { + kRetracted(0), + kMating(0), + kIntake(0); + + private double angle; + + private RetractorSetpoint(double angle){ + this.angle = angle; + } + + public double getAngle(){ + return this.angle; + } + } + + /* + * Implementation + */ + + private CANSparkMax motor; + private AbsoluteEncoder absoluteEncoder; + private RelativeEncoder relativeEncoder; + private SparkPIDController feedbackController; + + private DigitalInput limitSwitch; + private DigitalInput beamBreak; + + private RetractorSetpoint setpoint; + + private IntakeRetractor() { + this.motor = new CANSparkMax(kMotorPort, MotorType.kBrushless); + this.absoluteEncoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); + this.relativeEncoder = this.motor.getEncoder(); + this.feedbackController = this.motor.getPIDController(); + + this.limitSwitch = new DigitalInput(kLimitSwitchPort); + this.beamBreak = new DigitalInput(kBeamBreakPort); + + this.configureMotor(); + this.configureEncoder(); + this.configureController(); + + this.configureLimitSwitchResponder(); + } + + private void configureMotor(){ + /* + * TODO + * + * using constants set: + * motor inversion, current limit (30?) + * + * without constants set: + * idle mode (brake), voltage compensation (12) + */ + } + + private void configureEncoder(){ + /* + * TODO + * + * set position conversion rate (store this rate as a constant), set encoder inversion + * + * this should probably be done for both encoders just in case + */ + } + + private void configureController(){ + this.feedbackController.setD(kD); + this.feedbackController.setP(kP); + this.feedbackController.setI(kI); + + // TODO: also configure feed forward controller once it it written + } + + private void configureLimitSwitchResponder(){ + new Trigger(this::isLimitSwitchTriggered).whileTrue(new RunCommand(this::resetEncoder)); + } + + public double getPosition() { + return this.absoluteEncoder.getPosition(); + + // if using relative encoder + // return this.relativeEncoder.getPosition(); + } + + public boolean isLimitSwitchTriggered() { + // TODO: is this negation correct? + + return !this.limitSwitch.get(); + } + + public boolean isBeamBroken(){ + // TODO: is this negation correct? + return !this.beamBreak.get(); + } + + // TODO: this method is totally fine but may be scrapped when we move to a motion profiling implementation + public boolean isAtPosition(double position) { + return Math.abs(this.getPosition() - position) < kErrorThreshold; + } + + public boolean isAtSetpoint(){ + return this.isAtPosition(this.setpoint.getAngle()); + } + + public void resetEncoder(double position){ + this.relativeEncoder.setPosition(position); + + // TODO: does it make sense to implement this for the mag offset of the absolute encoder? + } + + public void resetEncoder(){ + this.resetEncoder(RetractorSetpoint.kRetracted.getAngle()); + } + + public void setSetpoint(RetractorSetpoint setpoint){ + this.setpoint = setpoint; + } + + private void useController(){ + /* + * TODO + * + * this method will be called periodically to set the motor voltage based on feedback and feedforward controllers + * + * add the outputs of the feedback controller (pid) and feedforward controller (once it is written) and supply them to the motor via set() + * + * good examples of this code are this years shooter pivot and last years arm + */ + } + + private void doSendables(){ + // TODO: use smart dashboard to log the position, limit switch status, beam break status, etc. + } + + @Override + public void periodic() { + useController(); + doSendables(); + } + + public void initTuning(){ + SmartDashboard.putNumber("retractor kp", SmartDashboard.getNumber("retractor kp", kP)); + + // TODO: ^ do this for kI, kD, and any constants for the feedforward controller + } + + public void tune(){ + double tunedP = SmartDashboard.getNumber("retractor kp", kP); + + this.feedbackController.setP(tunedP); + + // TODO: ^ do this for kI, kD, and any constants for the feedforward controller + } +} From 6377635d69d5713f8f8b5400fc7a136abe5c431e Mon Sep 17 00:00:00 2001 From: Raphael Hoang <116760801+Floofyer@users.noreply.github.com> Date: Sun, 21 Jan 2024 18:36:14 -0500 Subject: [PATCH 14/57] Add files via upload --- Intake.java | 44 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 Intake.java diff --git a/Intake.java b/Intake.java new file mode 100644 index 0000000..0d0e57f --- /dev/null +++ b/Intake.java @@ -0,0 +1,44 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystem; + +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkPIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase { + + public CANSparkMax intakemotor; + SparkPIDController IntakePIDController; + + public int kport = 0; + public double power = 0; + public double kFF = 0; + + public void intake() { + + this.intakemotor = new CANSparkMax(kport, MotorType.kBrushless); + this.intakemotor.setSmartCurrentLimit(20); + SmartDashboard.putNumber("intake-kFF", kFF); + } + + @Override + public void periodic() { + + intakemotor.set(power); + double kFF = SmartDashboard.getNumber("kFF", 0); + IntakePIDController.setFF(kFF); + } + + public Command on (double power) { + + return runOnce(() -> intakemotor.set(power)); + } + + public Command off (double power) { + + return runOnce(() -> intakemotor.set(0)); + } +} From 17f7cb4065f78c4413adc6df4f90dffed5d784d6 Mon Sep 17 00:00:00 2001 From: Raphael Hoang <116760801+Floofyer@users.noreply.github.com> Date: Mon, 22 Jan 2024 20:00:09 -0500 Subject: [PATCH 15/57] Add files via upload --- Intake.java | 45 +++++++-------------------------------------- 1 file changed, 7 insertions(+), 38 deletions(-) diff --git a/Intake.java b/Intake.java index 0d0e57f..2f394f0 100644 --- a/Intake.java +++ b/Intake.java @@ -1,44 +1,13 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.subsystem; +package org.robolancers321.subsystems; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkPIDController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +public class Intake extends SubsystemBase{ -public class Intake extends SubsystemBase { + public CANSparkMax intakeMotor; + - public CANSparkMax intakemotor; - SparkPIDController IntakePIDController; + public Intake () { - public int kport = 0; - public double power = 0; - public double kFF = 0; + this.intakeMotor = new intakeMotor(0, brushless) - public void intake() { - - this.intakemotor = new CANSparkMax(kport, MotorType.kBrushless); - this.intakemotor.setSmartCurrentLimit(20); - SmartDashboard.putNumber("intake-kFF", kFF); - } - - @Override - public void periodic() { - - intakemotor.set(power); - double kFF = SmartDashboard.getNumber("kFF", 0); - IntakePIDController.setFF(kFF); - } - - public Command on (double power) { - - return runOnce(() -> intakemotor.set(power)); - } - - public Command off (double power) { - - return runOnce(() -> intakemotor.set(0)); - } + } } From 79930588ea8415f5516d7272a8343d8b827d407f Mon Sep 17 00:00:00 2001 From: Raphael Hoang Date: Wed, 24 Jan 2024 15:09:09 -0500 Subject: [PATCH 16/57] Added intake --- 321-Crescendo-2024 | 1 + Intake.java | 33 ++++++- README.md | 2 +- networktables.json | 1 + simgui-ds.json | 92 +++++++++++++++++++ simgui.json | 10 ++ .../org/robolancers321/BuildConstants.java | 19 ++++ .../org/robolancers321/RobotContainer.java | 4 + .../org/robolancers321/subsystem/Intake.java | 38 ++++++++ vendordeps/REVLib.json | 74 +++++++++++++++ 10 files changed, 268 insertions(+), 6 deletions(-) create mode 160000 321-Crescendo-2024 create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json create mode 100644 src/main/java/org/robolancers321/BuildConstants.java create mode 100644 src/main/java/org/robolancers321/subsystem/Intake.java create mode 100644 vendordeps/REVLib.json diff --git a/321-Crescendo-2024 b/321-Crescendo-2024 new file mode 160000 index 0000000..17f7cb4 --- /dev/null +++ b/321-Crescendo-2024 @@ -0,0 +1 @@ +Subproject commit 17f7cb4065f78c4413adc6df4f90dffed5d784d6 diff --git a/Intake.java b/Intake.java index 2f394f0..859cfea 100644 --- a/Intake.java +++ b/Intake.java @@ -2,12 +2,35 @@ public class Intake extends SubsystemBase{ - public CANSparkMax intakeMotor; - + public CANSparkMax intakemotor; + SparkPIDController IntakePIDController; - public Intake () { + public int kport = 0; + public double power = 0; + public double kFF = 0; - this.intakeMotor = new intakeMotor(0, brushless) + public void intake() { - } + this.intakemotor = new CANSparkMax(kport, MotorType.kBrushless); + this.intakemotor.setSmartCurrentLimit(20); + SmartDashboard.putNumber("intake-kFF", kFF); + } + + @Override + public void periodic() { + + intakemotor.set(power); + double kFF = SmartDashboard.getNumber("kFF", 0); + IntakePIDController.setFF(kFF); + } + + public Command on(double power) { + + return runOnce(() -> intakemotor.set(power)); + } + + public Command off(double power) { + + return runOnce(() -> intakemotor.set(0)); + } } diff --git a/README.md b/README.md index ecf7e73..2212a7a 100644 --- a/README.md +++ b/README.md @@ -1 +1 @@ -## WIP \ No newline at end of file +## WIP diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..5f9d275 --- /dev/null +++ b/simgui.json @@ -0,0 +1,10 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java new file mode 100644 index 0000000..7a9440c --- /dev/null +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -0,0 +1,19 @@ +package org.robolancers321; + +/** + * Automatically generated file containing build version information. + */ +public final class BuildConstants { + public static final String MAVEN_GROUP = ""; + public static final String MAVEN_NAME = "321-Crescendo-2024"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 6; + public static final String GIT_SHA = "6377635d69d5713f8f8b5400fc7a136abe5c431e"; + public static final String GIT_DATE = "2024-01-21 18:36:14 EST"; + public static final String GIT_BRANCH = "dev-intake"; + public static final String BUILD_DATE = "2024-01-24 08:18:40 EST"; + public static final long BUILD_UNIX_TIME = 1706102320667L; + public static final int DIRTY = 0; + + private BuildConstants(){} +} diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 55b784c..5e03f9d 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -3,8 +3,12 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import org.robolancers321.subsystem.Intake; public class RobotContainer { + + public Intake intake = new Intake(); + public RobotContainer() { configureBindings(); } diff --git a/src/main/java/org/robolancers321/subsystem/Intake.java b/src/main/java/org/robolancers321/subsystem/Intake.java new file mode 100644 index 0000000..bf6f122 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystem/Intake.java @@ -0,0 +1,38 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystem; + +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkPIDController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase { + + public CANSparkMax intakemotor; + SparkPIDController IntakePIDController; + + public int kport = 0; + public double power = 0; + + public void intake() { + + this.intakemotor = new CANSparkMax(kport, MotorType.kBrushless); + this.intakemotor.setSmartCurrentLimit(20); + } + + public Command in(double power) { + + return runOnce(() -> intakemotor.set(power)); + } + + public Command out(double power) { + + return runOnce(() -> intakemotor.set(-power)); + } + + public Command off(double power) { + + return runOnce(() -> intakemotor.set(0)); + } +} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..0f3520e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.0", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.0", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From d7f5a071320caae2dbf837f6d4b8063c990ef911 Mon Sep 17 00:00:00 2001 From: Raphael Hoang Date: Wed, 24 Jan 2024 15:46:58 -0500 Subject: [PATCH 17/57] Fixed some github problems --- 321-Crescendo-2024 | 1 - 1 file changed, 1 deletion(-) delete mode 160000 321-Crescendo-2024 diff --git a/321-Crescendo-2024 b/321-Crescendo-2024 deleted file mode 160000 index 17f7cb4..0000000 --- a/321-Crescendo-2024 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 17f7cb4065f78c4413adc6df4f90dffed5d784d6 From 7f7b8e373147f598df049fe120e153253030a457 Mon Sep 17 00:00:00 2001 From: koolfyn Date: Wed, 24 Jan 2024 17:08:45 -0500 Subject: [PATCH 18/57] dababy --- .../commands/intake/RetractIntake.java | 2 - .../subsystems/intake/IntakeRetractor.java | 105 ++++++++++-------- 2 files changed, 59 insertions(+), 48 deletions(-) diff --git a/src/main/java/org/robolancers321/commands/intake/RetractIntake.java b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java index a68d09d..394c4ef 100644 --- a/src/main/java/org/robolancers321/commands/intake/RetractIntake.java +++ b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java @@ -21,8 +21,6 @@ public void initialize() { @Override public boolean isFinished() { - // TODO: encoder vs limit switch? limit switch MAY be more accurate but is also subject to physical damage - return this.intakeRetractor.isAtSetpoint(); } } diff --git a/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java index 0243641..eeb1d5c 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java @@ -1,15 +1,17 @@ /* (C) Robolancers 2024 */ package org.robolancers321.subsystems.intake; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAbsoluteEncoder.Type; -import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -35,14 +37,20 @@ public static IntakeRetractor getInstance(){ private static final int kLimitSwitchPort = 0; private static final int kBeamBreakPort = 0; - // TODO: put constants for motor and encoder configuration here + private static final boolean kInvertMotor = false; + private static final int kCurrentLimit = 20; private static final double kP = 0.000; private static final double kI = 0.000; private static final double kD = 0.000; - - // TODO: feed forward controller using wpilib ArmFeedforward class + private static final double kS = 0.000; + private static final double kV = 0.000; + private static final double kA = 0.000; + private static final double kG = 0.000; + private static final double kMaxVelocityDeg = 0.0; + private static final double kMaxAccelerationDeg = 0.0; + private static final double kErrorThreshold = 2.0; public static enum RetractorSetpoint { @@ -68,7 +76,8 @@ public double getAngle(){ private CANSparkMax motor; private AbsoluteEncoder absoluteEncoder; private RelativeEncoder relativeEncoder; - private SparkPIDController feedbackController; + private ProfiledPIDController feedbackController; + private ArmFeedforward feedforwardController; private DigitalInput limitSwitch; private DigitalInput beamBreak; @@ -79,31 +88,31 @@ private IntakeRetractor() { this.motor = new CANSparkMax(kMotorPort, MotorType.kBrushless); this.absoluteEncoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); this.relativeEncoder = this.motor.getEncoder(); - this.feedbackController = this.motor.getPIDController(); + this.feedbackController = new ProfiledPIDController(kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocityDeg, kMaxAccelerationDeg)); this.limitSwitch = new DigitalInput(kLimitSwitchPort); this.beamBreak = new DigitalInput(kBeamBreakPort); this.configureMotor(); this.configureEncoder(); - this.configureController(); - + this.configureController(); this.configureLimitSwitchResponder(); } private void configureMotor(){ - /* - * TODO - * - * using constants set: - * motor inversion, current limit (30?) - * - * without constants set: - * idle mode (brake), voltage compensation (12) - */ + this.motor.setInverted(kInvertMotor); + this.motor.setIdleMode(CANSparkMax.IdleMode.kBrake); + this.motor.setSmartCurrentLimit(kCurrentLimit); + this.motor.enableVoltageCompensation(12); + } private void configureEncoder(){ + this.relativeEncoder.setInverted(kInvertMotor); + this.absoluteEncoder.setInverted(kInvertMotor); + this.relativeEncoder.setPositionConversionFactor(360); + this.absoluteEncoder.setPositionConversionFactor(360); + /* * TODO * @@ -114,11 +123,11 @@ private void configureEncoder(){ } private void configureController(){ - this.feedbackController.setD(kD); + this.feedforwardController = new ArmFeedforward(kS, kG, kV); + this.feedbackController.setP(kP); this.feedbackController.setI(kI); - - // TODO: also configure feed forward controller once it it written + this.feedbackController.setD(kD); } private void configureLimitSwitchResponder(){ @@ -133,13 +142,10 @@ public double getPosition() { } public boolean isLimitSwitchTriggered() { - // TODO: is this negation correct? - return !this.limitSwitch.get(); } public boolean isBeamBroken(){ - // TODO: is this negation correct? return !this.beamBreak.get(); } @@ -154,8 +160,6 @@ public boolean isAtSetpoint(){ public void resetEncoder(double position){ this.relativeEncoder.setPosition(position); - - // TODO: does it make sense to implement this for the mag offset of the absolute encoder? } public void resetEncoder(){ @@ -167,20 +171,20 @@ public void setSetpoint(RetractorSetpoint setpoint){ } private void useController(){ - /* - * TODO - * - * this method will be called periodically to set the motor voltage based on feedback and feedforward controllers - * - * add the outputs of the feedback controller (pid) and feedforward controller (once it is written) and supply them to the motor via set() - * - * good examples of this code are this years shooter pivot and last years arm - */ + State setpointState = this.feedbackController.getSetpoint(); + + double feedforwardOutput = this.feedforwardController.calculate(setpointState.position, setpointState.velocity); + double feedbackOutput = this.feedbackController.calculate(this.getPosition()); + + double controllerOutput = feedforwardOutput + feedbackOutput; + this.motor.setVoltage(controllerOutput); } private void doSendables(){ - // TODO: use smart dashboard to log the position, limit switch status, beam break status, etc. - } + SmartDashboard.putNumber("current position", relativeEncoder.getPosition()); + SmartDashboard.putBoolean("note detected", this.isBeamBroken()); + SmartDashboard.putBoolean("Retractor at position(deg)", this.isAtSetpoint()); + } @Override public void periodic() { @@ -189,16 +193,25 @@ public void periodic() { } public void initTuning(){ - SmartDashboard.putNumber("retractor kp", SmartDashboard.getNumber("retractor kp", kP)); - - // TODO: ^ do this for kI, kD, and any constants for the feedforward controller + SmartDashboard.putNumber("retractor kP", SmartDashboard.getNumber("retractor kP", kP)); + SmartDashboard.putNumber("retractor kI", SmartDashboard.getNumber("retractor kI", kI)); + SmartDashboard.putNumber("retractor kD", SmartDashboard.getNumber("retractor kD", kD)); + SmartDashboard.putNumber("retractor kS", SmartDashboard.getNumber("retractor kS", kS)); + SmartDashboard.putNumber("retractor kV", SmartDashboard.getNumber("retractor kV", kV)); + SmartDashboard.putNumber("retractor kA", SmartDashboard.getNumber("retractor kA", kA)); + SmartDashboard.putNumber("retractor kG", SmartDashboard.getNumber("retractor kG", kG)); } - public void tune(){ + public void tune(){ double tunedP = SmartDashboard.getNumber("retractor kp", kP); + double tunedI = SmartDashboard.getNumber("retractor kI", kI); + double tunedD = SmartDashboard.getNumber("retractor kD", kD); + + this.feedforwardController = new ArmFeedforward(kS, kG, kV); this.feedbackController.setP(tunedP); + this.feedbackController.setI(tunedI); + this.feedbackController.setD(tunedD); - // TODO: ^ do this for kI, kD, and any constants for the feedforward controller } } From 95e468d01dc4c302cd18f481b535a77590a65937 Mon Sep 17 00:00:00 2001 From: koolfyn Date: Sat, 20 Jan 2024 13:06:20 -0500 Subject: [PATCH 19/57] intakeretractor feat? --- build.gradle | 18 +-- .../org/robolancers321/BuildConstants.java | 17 ++- .../subsystems/IntakeRetractor.java | 109 ++++++++++++++++++ .../commands/retractor/retractIntake.java | 9 ++ vendordeps/REVLib.json | 74 ++++++++++++ 5 files changed, 209 insertions(+), 18 deletions(-) create mode 100644 src/main/java/org/robolancers321/subsystems/IntakeRetractor.java create mode 100644 src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java create mode 100644 vendordeps/REVLib.json diff --git a/build.gradle b/build.gradle index c4ef2db..262c7e9 100644 --- a/build.gradle +++ b/build.gradle @@ -156,12 +156,12 @@ spotless { } } -project.compileJava.dependsOn(createVersionFile) -gversion { - srcDir = "src/main/java/" - classPackage = "org.robolancers321" - className = "BuildConstants" - dateFormat = "yyyy-MM-dd HH:mm:ss z" - timeZone = "America/New_York" // Use preferred time zone - indent = " " -} +// project.compileJava.dependsOn(createVersionFile) +// // gversion { +// // srcDir = "src/main/java/" +// // classPackage = "org.robolancers321" +// // className = "BuildConstants" +// // dateFormat = "yyyy-MM-dd HH:mm:ss z" +// // timeZone = "America/New_York" // Use preferred time zone +// // indent = " " +// // } diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index c52be06..f5162d2 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -1,19 +1,18 @@ +/* (C) Robolancers 2024 */ package org.robolancers321; -/** - * Automatically generated file containing build version information. - */ +/** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; public static final int GIT_REVISION = 4; - public static final String GIT_SHA = "890b1f13f053a4a882dc48c8de3887bfe2f9294f"; - public static final String GIT_DATE = "2024-01-18 19:40:03 EST"; - public static final String GIT_BRANCH = "dev"; - public static final String BUILD_DATE = "2024-01-18 19:43:26 EST"; - public static final long BUILD_UNIX_TIME = 1705625006821L; + public static final String GIT_SHA = "2de603cd5ac29e8182034c832bc185d3a6f14393"; + public static final String GIT_DATE = "2024-01-08 20:28:49 EST"; + public static final String GIT_BRANCH = "main"; + public static final String BUILD_DATE = "2024-01-10 17:47:24 EST"; + public static final long BUILD_UNIX_TIME = 1704926844560L; public static final int DIRTY = 1; - private BuildConstants(){} + private BuildConstants() {} } diff --git a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java new file mode 100644 index 0000000..a79a6e4 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java @@ -0,0 +1,109 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems; +import edu.wpi.first.math.proto.Controller; +import edu.wpi.first.wpilibj.DigitalInput; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; + +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class IntakeRetractor extends SubsystemBase { + + public CANSparkMax retractorMotor; + private DigitalInput retractorLimitSwitch; + private DigitalInput retractorBeamBreak; + private Trigger retractorTrigger; + SparkPIDController retractorPIDController; + AbsoluteEncoder retractorEncoder1; + RelativeEncoder retractorEncoder2; + + public double referencePosition = 0; + public double retractPosition = 90; + + public static final double kP = 0.000; + public static final double kI = 0.000; + public static final double kD = 0.000; + public static final double kFF = 0.0001; + public static final double kErrorThreshold = 2.0; + + public static final int kPort = 0; + public static final int kRetractorLimitSwitch = 0; + private static final Type kDutyCycle = null; + + + public IntakeRetractor() { + this.retractorMotor = new CANSparkMax(kPort, MotorType.kBrushless); + this.retractorEncoder1 = retractorMotor.getAbsoluteEncoder(kDutyCycle); + this.retractorEncoder2 = retractorMotor.getEncoder(); + retractorPIDController = retractorMotor.getPIDController(); + retractorLimitSwitch = new DigitalInput(0); + retractorBeamBreak = new DigitalInput(0); + + this.retractorEncoder1.setPositionConversionFactor(1.0); + + retractorPIDController.setD(kD); + retractorPIDController.setP(kP); + retractorPIDController.setI(kI); + + this.retractorMotor.setSmartCurrentLimit(20); + + + // this.retractorLimitSwitch = new DigitalInput(kRetractorLimitSwitch); + // this.retractorTrigger = new Trigger(this::limitSwitchTriggered); + // this.retractorTrigger.whileTrue(new InstantCommand(() -> { + // this.resetEncoder(); + // }, this)); + } + + @Override + public void periodic() { + + retractorPIDController.setD(kD); + retractorPIDController.setP(kP); + retractorPIDController.setI(kI); + retractorPIDController.setFF(kFF); + + retractorMotor.getOutputCurrent(); + retractorMotor.set(100); + + // pivotcontrolleroutput = controller.calculate(stuff) + // pivotMotor.set(Output) + + } + +// public boolean limitSwitchTriggered() { +// return !this.retractorLimitSwitch.get(); +// } + + +public void setRetractorPosition() { + retractorEncoder2.setPosition(retractPosition); +} + +public double getRetractPosition() { + return retractorEncoder2.getPosition(); +} + +public boolean isRetractorAtCorrectPos(double position) { + return Math.abs(this.getRetractPosition() - position) < kErrorThreshold; +} + + +public void resetEncoder() { + retractorEncoder2.setPosition(0); +} + + public boolean isBeamBroken() { + return !this.retractorBeamBreak.get(); + } + +} diff --git a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java new file mode 100644 index 0000000..2df6665 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java @@ -0,0 +1,9 @@ +package org.robolancers321.subsystems.commands.retractor; + +import edu.wpi.first.wpilibj2.command.Command; +import org.robolancers321.subsystems.IntakeRetractor; + +public class retractIntake extends Command { + public IntakeRetractor intakeRetractor; + +} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..0f3520e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.0", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.0", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From deba2c47d46873e57f7992b932aa8ec0ea6021d6 Mon Sep 17 00:00:00 2001 From: koolfyn Date: Sat, 20 Jan 2024 13:18:23 -0500 Subject: [PATCH 20/57] intakeretractor: feat?? --- .../subsystems/IntakeRetractor.java | 27 ++++++++++--------- .../commands/retractor/retractIntake.java | 23 ++++++++++++++++ 2 files changed, 38 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java index a79a6e4..bdcd164 100644 --- a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java +++ b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java @@ -26,8 +26,8 @@ public class IntakeRetractor extends SubsystemBase { AbsoluteEncoder retractorEncoder1; RelativeEncoder retractorEncoder2; - public double referencePosition = 0; - public double retractPosition = 90; + public double extendedPosition = 90; + public double retractedPosition = 0; public static final double kP = 0.000; public static final double kI = 0.000; @@ -36,6 +36,7 @@ public class IntakeRetractor extends SubsystemBase { public static final double kErrorThreshold = 2.0; public static final int kPort = 0; + public static final int kRetractorBeamBreakPort = 0; public static final int kRetractorLimitSwitch = 0; private static final Type kDutyCycle = null; @@ -45,9 +46,9 @@ public IntakeRetractor() { this.retractorEncoder1 = retractorMotor.getAbsoluteEncoder(kDutyCycle); this.retractorEncoder2 = retractorMotor.getEncoder(); retractorPIDController = retractorMotor.getPIDController(); + this.retractorBeamBreak = new DigitalInput(kRetractorBeamBreakPort); retractorLimitSwitch = new DigitalInput(0); - retractorBeamBreak = new DigitalInput(0); - + this.retractorEncoder1.setPositionConversionFactor(1.0); retractorPIDController.setD(kD); @@ -84,26 +85,28 @@ public void periodic() { // return !this.retractorLimitSwitch.get(); // } +public void retractedPosition() { + retractorEncoder2.setPosition(retractedPosition); +} -public void setRetractorPosition() { - retractorEncoder2.setPosition(retractPosition); +public void extendRetractor() { + retractorEncoder2.setPosition(extendedPosition); } public double getRetractPosition() { return retractorEncoder2.getPosition(); } -public boolean isRetractorAtCorrectPos(double position) { +public boolean RetractorAtCorrectPos(double position) { return Math.abs(this.getRetractPosition() - position) < kErrorThreshold; -} - +} public void resetEncoder() { retractorEncoder2.setPosition(0); } - public boolean isBeamBroken() { - return !this.retractorBeamBreak.get(); - } +public boolean beamBroken() { + return !this.retractorBeamBreak.get(); +} } diff --git a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java index 2df6665..299a2f0 100644 --- a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java +++ b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java @@ -5,5 +5,28 @@ public class retractIntake extends Command { public IntakeRetractor intakeRetractor; + + public retractIntake() { + addRequirements(this.intakeRetractor); + } + @Override + public void initialize() { + intakeRetractor.resetEncoder(); + } + + @Override + public void execute() { + intakeRetractor.extendRetractor(); + } + + @Override + public boolean isFinished() { + return intakeRetractor.beamBroken(); + } + + @Override + public void end(boolean interrupted) { + intakeRetractor.retractedPosition(); + } } From 80350942f74a63fcc991227742a81bf44efb5964 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Sat, 20 Jan 2024 14:34:38 -0500 Subject: [PATCH 21/57] major refactoring: singleton pattern, changed a lot of names and modifiers, added abstractions for configuration, and added framework for feedforward control and tuning. next steps: small changes where indicated and implement feedforward control, motion profiling, and tuning --- .../commands/intake/RetractIntake.java | 28 +++ .../subsystems/IntakeRetractor.java | 112 ---------- .../commands/retractor/retractIntake.java | 32 --- .../subsystems/intake/IntakeRetractor.java | 204 ++++++++++++++++++ 4 files changed, 232 insertions(+), 144 deletions(-) create mode 100644 src/main/java/org/robolancers321/commands/intake/RetractIntake.java delete mode 100644 src/main/java/org/robolancers321/subsystems/IntakeRetractor.java delete mode 100644 src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java create mode 100644 src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java diff --git a/src/main/java/org/robolancers321/commands/intake/RetractIntake.java b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java new file mode 100644 index 0000000..a68d09d --- /dev/null +++ b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java @@ -0,0 +1,28 @@ +package org.robolancers321.commands.intake; + +import org.robolancers321.subsystems.intake.IntakeRetractor; +import org.robolancers321.subsystems.intake.IntakeRetractor.RetractorSetpoint; + +import edu.wpi.first.wpilibj2.command.Command; + +public class RetractIntake extends Command { + private IntakeRetractor intakeRetractor; + + public RetractIntake() { + this.intakeRetractor = IntakeRetractor.getInstance(); + + addRequirements(this.intakeRetractor); + } + + @Override + public void initialize() { + this.intakeRetractor.setSetpoint(RetractorSetpoint.kRetracted); + } + + @Override + public boolean isFinished() { + // TODO: encoder vs limit switch? limit switch MAY be more accurate but is also subject to physical damage + + return this.intakeRetractor.isAtSetpoint(); + } +} diff --git a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java deleted file mode 100644 index bdcd164..0000000 --- a/src/main/java/org/robolancers321/subsystems/IntakeRetractor.java +++ /dev/null @@ -1,112 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.subsystems; -import edu.wpi.first.math.proto.Controller; -import edu.wpi.first.wpilibj.DigitalInput; - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkAbsoluteEncoder.Type; -import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkLowLevel.MotorType; - -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -public class IntakeRetractor extends SubsystemBase { - - public CANSparkMax retractorMotor; - private DigitalInput retractorLimitSwitch; - private DigitalInput retractorBeamBreak; - private Trigger retractorTrigger; - SparkPIDController retractorPIDController; - AbsoluteEncoder retractorEncoder1; - RelativeEncoder retractorEncoder2; - - public double extendedPosition = 90; - public double retractedPosition = 0; - - public static final double kP = 0.000; - public static final double kI = 0.000; - public static final double kD = 0.000; - public static final double kFF = 0.0001; - public static final double kErrorThreshold = 2.0; - - public static final int kPort = 0; - public static final int kRetractorBeamBreakPort = 0; - public static final int kRetractorLimitSwitch = 0; - private static final Type kDutyCycle = null; - - - public IntakeRetractor() { - this.retractorMotor = new CANSparkMax(kPort, MotorType.kBrushless); - this.retractorEncoder1 = retractorMotor.getAbsoluteEncoder(kDutyCycle); - this.retractorEncoder2 = retractorMotor.getEncoder(); - retractorPIDController = retractorMotor.getPIDController(); - this.retractorBeamBreak = new DigitalInput(kRetractorBeamBreakPort); - retractorLimitSwitch = new DigitalInput(0); - - this.retractorEncoder1.setPositionConversionFactor(1.0); - - retractorPIDController.setD(kD); - retractorPIDController.setP(kP); - retractorPIDController.setI(kI); - - this.retractorMotor.setSmartCurrentLimit(20); - - - // this.retractorLimitSwitch = new DigitalInput(kRetractorLimitSwitch); - // this.retractorTrigger = new Trigger(this::limitSwitchTriggered); - // this.retractorTrigger.whileTrue(new InstantCommand(() -> { - // this.resetEncoder(); - // }, this)); - } - - @Override - public void periodic() { - - retractorPIDController.setD(kD); - retractorPIDController.setP(kP); - retractorPIDController.setI(kI); - retractorPIDController.setFF(kFF); - - retractorMotor.getOutputCurrent(); - retractorMotor.set(100); - - // pivotcontrolleroutput = controller.calculate(stuff) - // pivotMotor.set(Output) - - } - -// public boolean limitSwitchTriggered() { -// return !this.retractorLimitSwitch.get(); -// } - -public void retractedPosition() { - retractorEncoder2.setPosition(retractedPosition); -} - -public void extendRetractor() { - retractorEncoder2.setPosition(extendedPosition); -} - -public double getRetractPosition() { - return retractorEncoder2.getPosition(); -} - -public boolean RetractorAtCorrectPos(double position) { - return Math.abs(this.getRetractPosition() - position) < kErrorThreshold; -} - -public void resetEncoder() { - retractorEncoder2.setPosition(0); -} - -public boolean beamBroken() { - return !this.retractorBeamBreak.get(); -} - -} diff --git a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java b/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java deleted file mode 100644 index 299a2f0..0000000 --- a/src/main/java/org/robolancers321/subsystems/commands/retractor/retractIntake.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.robolancers321.subsystems.commands.retractor; - -import edu.wpi.first.wpilibj2.command.Command; -import org.robolancers321.subsystems.IntakeRetractor; - -public class retractIntake extends Command { - public IntakeRetractor intakeRetractor; - - public retractIntake() { - addRequirements(this.intakeRetractor); - } - - @Override - public void initialize() { - intakeRetractor.resetEncoder(); - } - - @Override - public void execute() { - intakeRetractor.extendRetractor(); - } - - @Override - public boolean isFinished() { - return intakeRetractor.beamBroken(); - } - - @Override - public void end(boolean interrupted) { - intakeRetractor.retractedPosition(); - } -} diff --git a/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java new file mode 100644 index 0000000..0243641 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java @@ -0,0 +1,204 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.intake; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class IntakeRetractor extends SubsystemBase { + /* + * Singleton + */ + + private static IntakeRetractor instance = null; + + public static IntakeRetractor getInstance(){ + if (instance == null) instance = new IntakeRetractor(); + + return instance; + } + + /* + * Constants + */ + + private static final int kMotorPort = 0; + private static final int kLimitSwitchPort = 0; + private static final int kBeamBreakPort = 0; + + // TODO: put constants for motor and encoder configuration here + + private static final double kP = 0.000; + private static final double kI = 0.000; + private static final double kD = 0.000; + + // TODO: feed forward controller using wpilib ArmFeedforward class + + private static final double kErrorThreshold = 2.0; + + public static enum RetractorSetpoint { + kRetracted(0), + kMating(0), + kIntake(0); + + private double angle; + + private RetractorSetpoint(double angle){ + this.angle = angle; + } + + public double getAngle(){ + return this.angle; + } + } + + /* + * Implementation + */ + + private CANSparkMax motor; + private AbsoluteEncoder absoluteEncoder; + private RelativeEncoder relativeEncoder; + private SparkPIDController feedbackController; + + private DigitalInput limitSwitch; + private DigitalInput beamBreak; + + private RetractorSetpoint setpoint; + + private IntakeRetractor() { + this.motor = new CANSparkMax(kMotorPort, MotorType.kBrushless); + this.absoluteEncoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); + this.relativeEncoder = this.motor.getEncoder(); + this.feedbackController = this.motor.getPIDController(); + + this.limitSwitch = new DigitalInput(kLimitSwitchPort); + this.beamBreak = new DigitalInput(kBeamBreakPort); + + this.configureMotor(); + this.configureEncoder(); + this.configureController(); + + this.configureLimitSwitchResponder(); + } + + private void configureMotor(){ + /* + * TODO + * + * using constants set: + * motor inversion, current limit (30?) + * + * without constants set: + * idle mode (brake), voltage compensation (12) + */ + } + + private void configureEncoder(){ + /* + * TODO + * + * set position conversion rate (store this rate as a constant), set encoder inversion + * + * this should probably be done for both encoders just in case + */ + } + + private void configureController(){ + this.feedbackController.setD(kD); + this.feedbackController.setP(kP); + this.feedbackController.setI(kI); + + // TODO: also configure feed forward controller once it it written + } + + private void configureLimitSwitchResponder(){ + new Trigger(this::isLimitSwitchTriggered).whileTrue(new RunCommand(this::resetEncoder)); + } + + public double getPosition() { + return this.absoluteEncoder.getPosition(); + + // if using relative encoder + // return this.relativeEncoder.getPosition(); + } + + public boolean isLimitSwitchTriggered() { + // TODO: is this negation correct? + + return !this.limitSwitch.get(); + } + + public boolean isBeamBroken(){ + // TODO: is this negation correct? + return !this.beamBreak.get(); + } + + // TODO: this method is totally fine but may be scrapped when we move to a motion profiling implementation + public boolean isAtPosition(double position) { + return Math.abs(this.getPosition() - position) < kErrorThreshold; + } + + public boolean isAtSetpoint(){ + return this.isAtPosition(this.setpoint.getAngle()); + } + + public void resetEncoder(double position){ + this.relativeEncoder.setPosition(position); + + // TODO: does it make sense to implement this for the mag offset of the absolute encoder? + } + + public void resetEncoder(){ + this.resetEncoder(RetractorSetpoint.kRetracted.getAngle()); + } + + public void setSetpoint(RetractorSetpoint setpoint){ + this.setpoint = setpoint; + } + + private void useController(){ + /* + * TODO + * + * this method will be called periodically to set the motor voltage based on feedback and feedforward controllers + * + * add the outputs of the feedback controller (pid) and feedforward controller (once it is written) and supply them to the motor via set() + * + * good examples of this code are this years shooter pivot and last years arm + */ + } + + private void doSendables(){ + // TODO: use smart dashboard to log the position, limit switch status, beam break status, etc. + } + + @Override + public void periodic() { + useController(); + doSendables(); + } + + public void initTuning(){ + SmartDashboard.putNumber("retractor kp", SmartDashboard.getNumber("retractor kp", kP)); + + // TODO: ^ do this for kI, kD, and any constants for the feedforward controller + } + + public void tune(){ + double tunedP = SmartDashboard.getNumber("retractor kp", kP); + + this.feedbackController.setP(tunedP); + + // TODO: ^ do this for kI, kD, and any constants for the feedforward controller + } +} From f3cde3a999b7e555db9dfa1f3a75e643d893897b Mon Sep 17 00:00:00 2001 From: koolfyn Date: Wed, 24 Jan 2024 17:08:45 -0500 Subject: [PATCH 22/57] dababy --- .../commands/intake/RetractIntake.java | 2 - .../subsystems/intake/IntakeRetractor.java | 105 ++++++++++-------- 2 files changed, 59 insertions(+), 48 deletions(-) diff --git a/src/main/java/org/robolancers321/commands/intake/RetractIntake.java b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java index a68d09d..394c4ef 100644 --- a/src/main/java/org/robolancers321/commands/intake/RetractIntake.java +++ b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java @@ -21,8 +21,6 @@ public void initialize() { @Override public boolean isFinished() { - // TODO: encoder vs limit switch? limit switch MAY be more accurate but is also subject to physical damage - return this.intakeRetractor.isAtSetpoint(); } } diff --git a/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java index 0243641..eeb1d5c 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java @@ -1,15 +1,17 @@ /* (C) Robolancers 2024 */ package org.robolancers321.subsystems.intake; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAbsoluteEncoder.Type; -import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -35,14 +37,20 @@ public static IntakeRetractor getInstance(){ private static final int kLimitSwitchPort = 0; private static final int kBeamBreakPort = 0; - // TODO: put constants for motor and encoder configuration here + private static final boolean kInvertMotor = false; + private static final int kCurrentLimit = 20; private static final double kP = 0.000; private static final double kI = 0.000; private static final double kD = 0.000; - - // TODO: feed forward controller using wpilib ArmFeedforward class + private static final double kS = 0.000; + private static final double kV = 0.000; + private static final double kA = 0.000; + private static final double kG = 0.000; + private static final double kMaxVelocityDeg = 0.0; + private static final double kMaxAccelerationDeg = 0.0; + private static final double kErrorThreshold = 2.0; public static enum RetractorSetpoint { @@ -68,7 +76,8 @@ public double getAngle(){ private CANSparkMax motor; private AbsoluteEncoder absoluteEncoder; private RelativeEncoder relativeEncoder; - private SparkPIDController feedbackController; + private ProfiledPIDController feedbackController; + private ArmFeedforward feedforwardController; private DigitalInput limitSwitch; private DigitalInput beamBreak; @@ -79,31 +88,31 @@ private IntakeRetractor() { this.motor = new CANSparkMax(kMotorPort, MotorType.kBrushless); this.absoluteEncoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); this.relativeEncoder = this.motor.getEncoder(); - this.feedbackController = this.motor.getPIDController(); + this.feedbackController = new ProfiledPIDController(kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocityDeg, kMaxAccelerationDeg)); this.limitSwitch = new DigitalInput(kLimitSwitchPort); this.beamBreak = new DigitalInput(kBeamBreakPort); this.configureMotor(); this.configureEncoder(); - this.configureController(); - + this.configureController(); this.configureLimitSwitchResponder(); } private void configureMotor(){ - /* - * TODO - * - * using constants set: - * motor inversion, current limit (30?) - * - * without constants set: - * idle mode (brake), voltage compensation (12) - */ + this.motor.setInverted(kInvertMotor); + this.motor.setIdleMode(CANSparkMax.IdleMode.kBrake); + this.motor.setSmartCurrentLimit(kCurrentLimit); + this.motor.enableVoltageCompensation(12); + } private void configureEncoder(){ + this.relativeEncoder.setInverted(kInvertMotor); + this.absoluteEncoder.setInverted(kInvertMotor); + this.relativeEncoder.setPositionConversionFactor(360); + this.absoluteEncoder.setPositionConversionFactor(360); + /* * TODO * @@ -114,11 +123,11 @@ private void configureEncoder(){ } private void configureController(){ - this.feedbackController.setD(kD); + this.feedforwardController = new ArmFeedforward(kS, kG, kV); + this.feedbackController.setP(kP); this.feedbackController.setI(kI); - - // TODO: also configure feed forward controller once it it written + this.feedbackController.setD(kD); } private void configureLimitSwitchResponder(){ @@ -133,13 +142,10 @@ public double getPosition() { } public boolean isLimitSwitchTriggered() { - // TODO: is this negation correct? - return !this.limitSwitch.get(); } public boolean isBeamBroken(){ - // TODO: is this negation correct? return !this.beamBreak.get(); } @@ -154,8 +160,6 @@ public boolean isAtSetpoint(){ public void resetEncoder(double position){ this.relativeEncoder.setPosition(position); - - // TODO: does it make sense to implement this for the mag offset of the absolute encoder? } public void resetEncoder(){ @@ -167,20 +171,20 @@ public void setSetpoint(RetractorSetpoint setpoint){ } private void useController(){ - /* - * TODO - * - * this method will be called periodically to set the motor voltage based on feedback and feedforward controllers - * - * add the outputs of the feedback controller (pid) and feedforward controller (once it is written) and supply them to the motor via set() - * - * good examples of this code are this years shooter pivot and last years arm - */ + State setpointState = this.feedbackController.getSetpoint(); + + double feedforwardOutput = this.feedforwardController.calculate(setpointState.position, setpointState.velocity); + double feedbackOutput = this.feedbackController.calculate(this.getPosition()); + + double controllerOutput = feedforwardOutput + feedbackOutput; + this.motor.setVoltage(controllerOutput); } private void doSendables(){ - // TODO: use smart dashboard to log the position, limit switch status, beam break status, etc. - } + SmartDashboard.putNumber("current position", relativeEncoder.getPosition()); + SmartDashboard.putBoolean("note detected", this.isBeamBroken()); + SmartDashboard.putBoolean("Retractor at position(deg)", this.isAtSetpoint()); + } @Override public void periodic() { @@ -189,16 +193,25 @@ public void periodic() { } public void initTuning(){ - SmartDashboard.putNumber("retractor kp", SmartDashboard.getNumber("retractor kp", kP)); - - // TODO: ^ do this for kI, kD, and any constants for the feedforward controller + SmartDashboard.putNumber("retractor kP", SmartDashboard.getNumber("retractor kP", kP)); + SmartDashboard.putNumber("retractor kI", SmartDashboard.getNumber("retractor kI", kI)); + SmartDashboard.putNumber("retractor kD", SmartDashboard.getNumber("retractor kD", kD)); + SmartDashboard.putNumber("retractor kS", SmartDashboard.getNumber("retractor kS", kS)); + SmartDashboard.putNumber("retractor kV", SmartDashboard.getNumber("retractor kV", kV)); + SmartDashboard.putNumber("retractor kA", SmartDashboard.getNumber("retractor kA", kA)); + SmartDashboard.putNumber("retractor kG", SmartDashboard.getNumber("retractor kG", kG)); } - public void tune(){ + public void tune(){ double tunedP = SmartDashboard.getNumber("retractor kp", kP); + double tunedI = SmartDashboard.getNumber("retractor kI", kI); + double tunedD = SmartDashboard.getNumber("retractor kD", kD); + + this.feedforwardController = new ArmFeedforward(kS, kG, kV); this.feedbackController.setP(tunedP); + this.feedbackController.setI(tunedI); + this.feedbackController.setD(tunedD); - // TODO: ^ do this for kI, kD, and any constants for the feedforward controller } } From 9f3d8b1d5c7dc3864579f47356d6be30416ea5c6 Mon Sep 17 00:00:00 2001 From: Raphael Hoang Date: Wed, 24 Jan 2024 17:40:16 -0500 Subject: [PATCH 23/57] Fixed the mistakes --- Intake.java | 36 ------------------- .../org/robolancers321/BuildConstants.java | 10 +++--- .../org/robolancers321/subsystem/Intake.java | 18 +++++----- 3 files changed, 13 insertions(+), 51 deletions(-) delete mode 100644 Intake.java diff --git a/Intake.java b/Intake.java deleted file mode 100644 index 859cfea..0000000 --- a/Intake.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.robolancers321.subsystems; - -public class Intake extends SubsystemBase{ - - public CANSparkMax intakemotor; - SparkPIDController IntakePIDController; - - public int kport = 0; - public double power = 0; - public double kFF = 0; - - public void intake() { - - this.intakemotor = new CANSparkMax(kport, MotorType.kBrushless); - this.intakemotor.setSmartCurrentLimit(20); - SmartDashboard.putNumber("intake-kFF", kFF); - } - - @Override - public void periodic() { - - intakemotor.set(power); - double kFF = SmartDashboard.getNumber("kFF", 0); - IntakePIDController.setFF(kFF); - } - - public Command on(double power) { - - return runOnce(() -> intakemotor.set(power)); - } - - public Command off(double power) { - - return runOnce(() -> intakemotor.set(0)); - } -} diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index 7a9440c..6e9ddee 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 6; - public static final String GIT_SHA = "6377635d69d5713f8f8b5400fc7a136abe5c431e"; - public static final String GIT_DATE = "2024-01-21 18:36:14 EST"; + public static final int GIT_REVISION = 9; + public static final String GIT_SHA = "d7f5a071320caae2dbf837f6d4b8063c990ef911"; + public static final String GIT_DATE = "2024-01-24 17:03:30 EST"; public static final String GIT_BRANCH = "dev-intake"; - public static final String BUILD_DATE = "2024-01-24 08:18:40 EST"; - public static final long BUILD_UNIX_TIME = 1706102320667L; + public static final String BUILD_DATE = "2024-01-24 17:22:28 EST"; + public static final long BUILD_UNIX_TIME = 1706134948601L; public static final int DIRTY = 0; private BuildConstants(){} diff --git a/src/main/java/org/robolancers321/subsystem/Intake.java b/src/main/java/org/robolancers321/subsystem/Intake.java index bf6f122..09b5461 100644 --- a/src/main/java/org/robolancers321/subsystem/Intake.java +++ b/src/main/java/org/robolancers321/subsystem/Intake.java @@ -3,36 +3,34 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkPIDController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Intake extends SubsystemBase { - public CANSparkMax intakemotor; - SparkPIDController IntakePIDController; + public CANSparkMax intakeMotor; - public int kport = 0; + public int kPort = 0; public double power = 0; - public void intake() { + public void Intake() { - this.intakemotor = new CANSparkMax(kport, MotorType.kBrushless); - this.intakemotor.setSmartCurrentLimit(20); + this.intakeMotor = new CANSparkMax(kPort, MotorType.kBrushless); + this.intakeMotor.setSmartCurrentLimit(20); } public Command in(double power) { - return runOnce(() -> intakemotor.set(power)); + return runOnce(() -> intakeMotor.set(power)); } public Command out(double power) { - return runOnce(() -> intakemotor.set(-power)); + return runOnce(() -> intakeMotor.set(-power)); } public Command off(double power) { - return runOnce(() -> intakemotor.set(0)); + return runOnce(() -> intakeMotor.set(0)); } } From b6639991c8401cc2d5c250e57bbd8ac6d8cc112f Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 25 Jan 2024 07:53:34 -0500 Subject: [PATCH 24/57] fix(CI): Fixed formatting bug --- build.gradle | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/build.gradle b/build.gradle index c4ef2db..c1727d8 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" id 'com.diffplug.spotless' version '6.12.0' id "com.peterabeles.gversion" version "1.10" } @@ -52,6 +52,7 @@ def includeDesktopSupport = false dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() + implementation 'org.jetbrains:annotations:24.0.0' roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) @@ -104,14 +105,16 @@ spotless { java { target fileTree('.') { include '**/*.java' - exclude '**/build/**', '**/build-*/**','**/BuildConstants.java' + exclude '**/build/**', '**/build-*/**','src/main/java/org/robolancers321/BuildConstants.java' } + toggleOffOn() googleJavaFormat().reflowLongStrings() removeUnusedImports() trimTrailingWhitespace() endWithNewline() + licenseHeader '/* (C) Robolancers 2024 */' } kotlin { From 052077f5803b753b95e99e03de3dcc4dfad9d82b Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 25 Jan 2024 07:54:36 -0500 Subject: [PATCH 25/57] feat(indexer): Moved IndexNote command to Indexer --- .../commands/launcher/IndexNote.java | 36 ------------------- .../subsystems/launcher/Indexer.java | 21 ++++++----- 2 files changed, 12 insertions(+), 45 deletions(-) delete mode 100644 src/main/java/org/robolancers321/commands/launcher/IndexNote.java diff --git a/src/main/java/org/robolancers321/commands/launcher/IndexNote.java b/src/main/java/org/robolancers321/commands/launcher/IndexNote.java deleted file mode 100644 index 6345944..0000000 --- a/src/main/java/org/robolancers321/commands/launcher/IndexNote.java +++ /dev/null @@ -1,36 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.commands.launcher; - -import static org.robolancers321.util.TunableSet.Tunable.tune; - -import edu.wpi.first.wpilibj2.command.Command; -import org.robolancers321.subsystems.launcher.Indexer; - -public class IndexNote extends Command { - private final Indexer indexer = Indexer.getInstance(); - - public IndexNote() { - addRequirements(this.indexer); - } - - @Override - public void initialize() { - var velocity = indexer.tuning ? tune("Desired Velocity", 0) : 0; - indexer.setDesiredVelocity(velocity); - } - - @Override - public void execute() { - indexer.intakeJawn(); - } - - @Override - public boolean isFinished() { - return indexer.noteDetected(); - } - - @Override - public void end(boolean interrupted) { - indexer.stopSpinningJawn(); - } -} diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index ca8643b..9e6a3ca 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -3,15 +3,13 @@ import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; -import java.util.function.DoubleSupplier; - import com.revrobotics.*; import com.revrobotics.CANSparkBase.ControlType; - import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; public class Indexer extends SubsystemBase { /* @@ -19,6 +17,7 @@ public class Indexer extends SubsystemBase { */ private static Indexer instance; + public static Indexer getInstance() { if (instance == null) instance = new Indexer(); @@ -43,7 +42,7 @@ public static Indexer getInstance() { private static final double kMaxRPM = 5700; private static final double kIntakeSpeedRPM = 500; private static final double kOuttakeSpeedRPM = -500; - + /* * Implementation */ @@ -73,7 +72,7 @@ private void configureMotor() { this.motor.enableVoltageCompensation(12); } - private void configureEncoder(){ + private void configureEncoder() { this.encoder.setInverted(kInvertMotor); } @@ -120,6 +119,10 @@ public Command manualIndex(double appliedSpeed) { return this.manualIndex(() -> appliedSpeed); } + public Command index() { + return run(this::intakeJawn).until(this::jawnDetected).finallyDo(this::stopSpinningJawn); + } + private void doSendables() { SmartDashboard.putNumber("Indexer Velocity (rpm)", this.getIntakeVelocityRPM()); SmartDashboard.putBoolean("Note Detected", this.jawnDetected()); @@ -130,22 +133,22 @@ public void periodic() { this.doSendables(); } - public void initTuning(){ + public void initTuning() { SmartDashboard.putNumber("indexer kp", SmartDashboard.getNumber("indexer kp", kP)); SmartDashboard.putNumber("indexer kp", SmartDashboard.getNumber("indexer ki", kI)); SmartDashboard.putNumber("indexer kp", SmartDashboard.getNumber("indexer kd", kD)); SmartDashboard.putNumber("indexer kff", SmartDashboard.getNumber("indexer kff", kFF)); } - public void tune(){ + public void tune() { double tunedP = SmartDashboard.getNumber("indexer kp", kP); double tunedI = SmartDashboard.getNumber("indexer ki", kI); double tunedD = SmartDashboard.getNumber("indexer kd", kD); double tunedFF = SmartDashboard.getNumber("indexer kff", kFF); - + this.controller.setP(tunedP); this.controller.setI(tunedI); this.controller.setD(tunedD); this.controller.setFF(tunedFF); } -} \ No newline at end of file +} From 3ee9bb82c05b754ba0ad5f98d25549c1e72e807d Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 25 Jan 2024 07:55:18 -0500 Subject: [PATCH 26/57] feat(launcher): Added InterpolationTable and MathUtils --- .../util/InterpolationTable.java | 86 +++++++++++++++++++ .../org/robolancers321/util/MathUtils.java | 9 ++ 2 files changed, 95 insertions(+) create mode 100644 src/main/java/org/robolancers321/util/InterpolationTable.java create mode 100644 src/main/java/org/robolancers321/util/MathUtils.java diff --git a/src/main/java/org/robolancers321/util/InterpolationTable.java b/src/main/java/org/robolancers321/util/InterpolationTable.java new file mode 100644 index 0000000..a8bd131 --- /dev/null +++ b/src/main/java/org/robolancers321/util/InterpolationTable.java @@ -0,0 +1,86 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.util; + +import java.util.LinkedHashMap; +import java.util.List; +import java.util.NavigableSet; +import java.util.TreeSet; + +public class InterpolationTable { + public static final class AimCharacteristic { + private final double leftSpeed; + private final double rightSpeed; + private final double pitchAngle; + + public AimCharacteristic(double leftSpeed, double rightSpeed, double pitchAngle) { + this.leftSpeed = leftSpeed; + this.rightSpeed = rightSpeed; + this.pitchAngle = pitchAngle; + } + + public double getLeftSpeed() { + return leftSpeed; + } + + public double getRightSpeed() { + return rightSpeed; + } + + public double getPitchAngle() { + return pitchAngle; + } + } + + public static LinkedHashMap table = + new LinkedHashMap<>() { + { + put(1.0, new AimCharacteristic(1000, 1000, 10)); + put(2.0, new AimCharacteristic(2000, 2000, 20)); + } + }; + + public static AimCharacteristic interpolate(double independent) { + List keys = table.keySet().stream().toList(); + double lowerBound = keys.get(0); + double upperBound = keys.get(keys.size() - 1); + + if ((independent < lowerBound)) independent = lowerBound; + else if (independent > upperBound) independent = upperBound; + + NavigableSet values = new TreeSet<>(keys); + + double lowerKey = values.floor(independent); + double upperKey = lowerKey + 1; + + AimCharacteristic lowerValue = table.get(lowerKey); + AimCharacteristic upperValue = table.get(upperKey); + + double leftSpeed = + calculateCharacteristic( + lowerKey, upperKey, lowerValue.getLeftSpeed(), upperValue.getLeftSpeed(), independent); + + double rightSpeed = + calculateCharacteristic( + lowerKey, + upperKey, + lowerValue.getRightSpeed(), + upperValue.getRightSpeed(), + independent); + + double pitchAngle = + calculateCharacteristic( + lowerKey, + upperKey, + lowerValue.getPitchAngle(), + upperValue.getPitchAngle(), + independent); + + return new AimCharacteristic(leftSpeed, rightSpeed, pitchAngle); + } + + private static double calculateCharacteristic( + double lowerKey, double upperKey, double lower, double upper, double independent) { + return ((lower * (upperKey - independent) + upper * (independent - lowerKey)) / upperKey + - lowerKey); + } +} diff --git a/src/main/java/org/robolancers321/util/MathUtils.java b/src/main/java/org/robolancers321/util/MathUtils.java new file mode 100644 index 0000000..08ebe60 --- /dev/null +++ b/src/main/java/org/robolancers321/util/MathUtils.java @@ -0,0 +1,9 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.util; + +public class MathUtils { + + public static boolean epsilonEquals(double a, double b, double epsilon) { + return Math.abs(b - a) == epsilon; + } +} From 7d4224692a5477f20139d6555f43f17ea585ad21 Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 25 Jan 2024 07:56:27 -0500 Subject: [PATCH 27/57] feat(pivot): Added Interpolation to pivot pitch angle and basic commands. Reverted to inheriting ProfiledPIDSubsystem --- .../subsystems/launcher/Pivot.java | 104 ++++++++++++------ 1 file changed, 69 insertions(+), 35 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index e5a53eb..6e6be06 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -2,18 +2,20 @@ package org.robolancers321.subsystems.launcher; import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; +import static org.robolancers321.util.MathUtils.epsilonEquals; import com.revrobotics.*; import com.revrobotics.SparkAbsoluteEncoder.Type; - import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; +import java.util.function.DoubleSupplier; +import org.robolancers321.util.InterpolationTable; -public class Pivot extends SubsystemBase { +public class Pivot extends ProfiledPIDSubsystem { /* * Singleton */ @@ -25,7 +27,7 @@ public static Pivot getInstance() { return instance; } - + /* * Constants */ @@ -54,7 +56,13 @@ public static Pivot getInstance() { private static final double kToleranceDeg = 0.0; - private static enum PivotSetpoint { + private static final double kInterpolationThreshold = 0.0; + + private double latest_distance = 0.0; + + private InterpolationTable.AimCharacteristic latest_characteristic; + + public enum PivotSetpoint { kRetracted(0.0), kMating(0.0), kSpeaker(0.0), @@ -70,7 +78,7 @@ public double getAngle() { return this.angle; } } - + /* * Implementation */ @@ -78,13 +86,15 @@ public double getAngle() { private CANSparkMax motor; private AbsoluteEncoder absoluteEncoder; private ArmFeedforward feedforwardController; - private ProfiledPIDController feedbackController; private Pivot() { + super( + new ProfiledPIDController( + kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocityDeg, kMaxAccelerationDeg))); + this.motor = new CANSparkMax(kMotorPort, kBrushless); this.absoluteEncoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); this.feedforwardController = new ArmFeedforward(kS, kG, kV); - this.feedbackController = new ProfiledPIDController(kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocityDeg, kMaxAccelerationDeg)); this.configureMotor(); this.configureEncoder(); @@ -103,67 +113,89 @@ private void configureMotor() { this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); } - private void configureEncoder(){ + private void configureEncoder() { this.absoluteEncoder.setInverted(kInvertEncoder); this.absoluteEncoder.setPositionConversionFactor(kGearRatio); this.absoluteEncoder.setVelocityConversionFactor(kRotPerMinToDegPerSec); } - private void configureController(){ - this.feedbackController.setTolerance(kToleranceDeg); + @Override + public double getMeasurement() { + return getVelocityRad(); + } + + private void configureController() { + super.m_controller.setTolerance(kToleranceDeg); } - public double getPositionDeg(){ + public double getPositionDeg() { return this.absoluteEncoder.getPosition(); } - public double getPositionRad(){ - return this.getPositionDeg() * Math.PI / 180.0; + public double getPositionRad() { + return Math.toRadians(this.getPositionDeg()); } - public double getVelocityDeg(){ + public double getVelocityDeg() { return this.absoluteEncoder.getVelocity(); } - public double getVelocityRad(){ - return this.getVelocityDeg() * Math.PI / 180.0; + public double getVelocityRad() { + return Math.toRadians(this.getVelocityDeg()); } - private boolean atGoal(){ - return this.feedbackController.atGoal(); + private boolean atGoal() { + return super.m_controller.atGoal(); } - private void setGoal(double goal){ - this.feedbackController.setGoal(new TrapezoidProfile.State(goal, 0.0)); + public void setAngleGoal(double goal) { + super.setGoal(new TrapezoidProfile.State(goal, 0.0)); } - private void setGoal(PivotSetpoint goal){ - this.setGoal(goal.getAngle()); + public void setAngleGoal(PivotSetpoint goal) { + super.setGoal(goal.getAngle()); } - private void useController(){ - State setpointState = this.feedbackController.getSetpoint(); - double feedforwardOutput = this.feedforwardController.calculate(setpointState.position, setpointState.velocity); + public void setAngleGoal(DoubleSupplier distance) { + if (!epsilonEquals(distance.getAsDouble(), latest_distance, kInterpolationThreshold)) + latest_characteristic = InterpolationTable.interpolate(distance.getAsDouble()); - double feedbackOutput = this.feedbackController.calculate(this.getPositionDeg()); + setAngleGoal(latest_characteristic.getPitchAngle()); + } + + public Command aimAtSpeaker(DoubleSupplier distance) { + + return run(() -> setAngleGoal(distance)).until(this::atGoal); + } + + public Command positionAmp() { + return run(() -> setAngleGoal(PivotSetpoint.kAmp)).until(this::atGoal); + } + + @Override + protected void useOutput(double output, TrapezoidProfile.State setpoint) { + double feedforwardOutput = + this.feedforwardController.calculate(setpoint.position, setpoint.velocity); + + double feedbackOutput = super.m_controller.calculate(this.getPositionDeg()); double controllerOutput = feedforwardOutput + feedbackOutput; this.motor.setVoltage(controllerOutput); } - public void doSendables(){ + public void doSendables() { SmartDashboard.putBoolean("Pivot At Goal", this.atGoal()); SmartDashboard.putNumber("Pivot Position (deg)", this.getPositionDeg()); } @Override - public void periodic(){ - this.useController(); + public void periodic() { + this.doSendables(); } - public void initTuning(){ + public void initTuning() { SmartDashboard.putNumber("pivot ks", SmartDashboard.getNumber("pivot ks", kS)); SmartDashboard.putNumber("pivot kg", SmartDashboard.getNumber("pivot kg", kG)); SmartDashboard.putNumber("pivot kv", SmartDashboard.getNumber("pivot kv", kV)); @@ -173,13 +205,15 @@ public void initTuning(){ SmartDashboard.putNumber("pivot kd", SmartDashboard.getNumber("pivot kd", kD)); } - public void tune(){ + public void tune() { double tunedS = SmartDashboard.getNumber("pivot ks", kS); double tunedG = SmartDashboard.getNumber("pivot kg", kG); double tunedV = SmartDashboard.getNumber("pivot kv", kV); // TODO: is there a better way to update this? - if(tunedS != this.feedforwardController.ks || tunedG != this.feedforwardController.kg || tunedV != this.feedforwardController.kv){ + if (tunedS != this.feedforwardController.ks + || tunedG != this.feedforwardController.kg + || tunedV != this.feedforwardController.kv) { this.feedforwardController = new ArmFeedforward(tunedS, tunedG, tunedV); } @@ -187,6 +221,6 @@ public void tune(){ double tunedI = SmartDashboard.getNumber("pivot ki", kI); double tunedD = SmartDashboard.getNumber("pivot kd", kD); - this.feedbackController.setPID(tunedP, tunedI, tunedD); + super.m_controller.setPID(tunedP, tunedI, tunedD); } } From d8715d486c3f7b92451a2c8b283b37cc9e3fd0a1 Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 25 Jan 2024 07:58:59 -0500 Subject: [PATCH 28/57] feat(shooter): Added basic shooter with interpolation and FF --- .../subsystems/launcher/Shooter.java | 200 +++++++++++++++++- 1 file changed, 194 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java b/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java index 7ce2b32..63037f5 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java @@ -1,7 +1,16 @@ /* (C) Robolancers 2024 */ package org.robolancers321.subsystems.launcher; +import static org.robolancers321.util.MathUtils.epsilonEquals; + +import com.revrobotics.*; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; +import org.robolancers321.util.InterpolationTable; public class Shooter extends SubsystemBase { private static Shooter INSTANCE; @@ -13,12 +22,191 @@ public static Shooter getInstance() { return INSTANCE; } + /* Constants */ + + private final int kLeftMotorID = 0; + private final int kRightMotorID = 0; + + private int kLeftCurrentLimit = 40; + + private int kRightCurrentLimit = 40; + + private boolean kInvertLeftMotor = false; + + private boolean kInvertRightMotor = false; + + private double kLeftRotPerMinToDegPerSec = 0.0; + + private double kRightRotPerMinToDegPerSec = 0.0; + + private double kLeftFF = 0.0; + + private double kRightFF = 0.0; + + private double kAmpLeftSpeed = 0.0; + + private double kRightAmpSpeed = 0.0; + + private double kRampUpRate = 0.5; + + private double kErrorTolerance = 0.0; + + private double kInterpolationThreshold = 0.0; + + private int kBeamBreakChannelPort = 0; + private double leftSetpointSpeed; + + private double rightSetpointSpeed; + + private CANSparkMax left_motor; + + private CANSparkMax right_motor; + + private AbsoluteEncoder left_encoder; + + private AbsoluteEncoder right_encoder; + + private double latest_distance = 0.0; + + private InterpolationTable.AimCharacteristic latest_characteristic; + + private SparkPIDController left_controller; + private SparkPIDController right_controller; + + private SlewRateLimiter left_limiter; + private SlewRateLimiter right_limiter; + + private DigitalInput beam_break; + private Shooter() { - // TODO: Set the default command, if any, for this subsystem by calling - // setDefaultCommand(command) - // in the constructor or in the robot coordination class, such as RobotContainer. - // Also, you can call addChild(name, sendableChild) to associate sendables with the - // subsystem - // such as SpeedControllers, Encoders, DigitalInputs, etc. + + beam_break = new DigitalInput(kBeamBreakChannelPort); + + left_motor = new CANSparkMax(kLeftMotorID, CANSparkLowLevel.MotorType.kBrushless); + right_motor = new CANSparkMax(kRightMotorID, CANSparkLowLevel.MotorType.kBrushless); + + left_encoder = left_motor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); + right_encoder = right_motor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); + + left_controller = left_motor.getPIDController(); + right_controller = right_motor.getPIDController(); + + left_limiter = new SlewRateLimiter(kRampUpRate); + right_limiter = new SlewRateLimiter(kRampUpRate); + + configureMotors(); + configureControllers(); + configureEncoders(); + } + + private void configureControllers() { + left_controller.setFF(kLeftFF); + right_controller.setFF(kRightFF); + } + + private void configureMotors() { + left_motor.setInverted(kInvertLeftMotor); + left_motor.setIdleMode(CANSparkBase.IdleMode.kBrake); + left_motor.setSmartCurrentLimit(kLeftCurrentLimit); + left_motor.enableVoltageCompensation(12); + left_motor.burnFlash(); + + right_motor.setInverted(kInvertRightMotor); + right_motor.setIdleMode(CANSparkBase.IdleMode.kBrake); + right_motor.setSmartCurrentLimit(kRightCurrentLimit); + right_motor.enableVoltageCompensation(12); + right_motor.burnFlash(); + } + + private void configureEncoders() { + left_encoder.setInverted(kInvertLeftMotor); + left_encoder.setVelocityConversionFactor(kLeftRotPerMinToDegPerSec); + + right_encoder.setInverted(kInvertRightMotor); + right_encoder.setVelocityConversionFactor(kRightRotPerMinToDegPerSec); + } + + private double getLeftVelocity() { + return left_encoder.getVelocity(); + } + + private double getRightVelocity() { + return right_encoder.getVelocity(); + } + + private void setGoalLR(double leftSpeed, double rightSpeed) { + left_controller.setReference( + left_limiter.calculate(leftSpeed), CANSparkBase.ControlType.kVelocity); + right_controller.setReference( + right_limiter.calculate(rightSpeed), CANSparkBase.ControlType.kVelocity); + } + + public void yeetNoteSpeaker(double distance) { + if (!epsilonEquals(distance, latest_distance, kInterpolationThreshold)) + latest_characteristic = InterpolationTable.interpolate(distance); + + leftSetpointSpeed = latest_characteristic.getLeftSpeed(); + rightSetpointSpeed = latest_characteristic.getRightSpeed(); + + setGoalLR(leftSetpointSpeed, rightSetpointSpeed); + } + + public Command yeetNoteSpeaker(DoubleSupplier distance) { + return run(() -> yeetNoteSpeaker(distance.getAsDouble())).until(beam_break::get); + } + + public void setRampUpRate(double rate) { + kRampUpRate = rate; + } + + public boolean atLeftSetpoint() { + return epsilonEquals(getLeftVelocity(), leftSetpointSpeed, kErrorTolerance); + } + + public boolean atRightSetpoint() { + return epsilonEquals(getRightVelocity(), leftSetpointSpeed, kErrorTolerance); + } + + public boolean atSetpoint() { + return atLeftSetpoint() && atLeftSetpoint(); + } + + public void yeet(double speed) { + left_motor.set(speed); + right_motor.set(-speed); + } + + public void yeetNoteAmp(double distance) { + leftSetpointSpeed = kAmpLeftSpeed; + rightSetpointSpeed = kRightAmpSpeed; + setGoalLR(kAmpLeftSpeed, kRightAmpSpeed); + } + + public Command yeetNoteAmp() { + return run(this::yeetNoteAmp).until(beam_break::get); + } + + @Override + public void periodic() { + doSendables(); + } + + private void doSendables() { + + SmartDashboard.putNumber("Left flywheel velocity", getLeftVelocity()); + SmartDashboard.putNumber("Right flywheel velocity", getRightVelocity()); + } + + public void initTuning() { + SmartDashboard.putNumber("Left FF", kLeftFF); + SmartDashboard.putNumber("Right FF", kRightFF); + } + + public void tune() { + double leftFF = SmartDashboard.getNumber("Left FF", kLeftFF); + double rightFF = SmartDashboard.getNumber("Right FF", kRightFF); + left_controller.setFF(leftFF); + + right_controller.setFF(rightFF); } } From 432552cbaa74f263aad47f8c7d46e169555a7241 Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 25 Jan 2024 07:59:19 -0500 Subject: [PATCH 29/57] feat(launcher): Added basic yeet commands --- .../commands/launcher/YeetAmp.java | 21 +++++++++++++++ .../commands/launcher/YeetSpeaker.java | 27 +++++++++++++++++++ 2 files changed, 48 insertions(+) create mode 100644 src/main/java/org/robolancers321/commands/launcher/YeetAmp.java create mode 100644 src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java diff --git a/src/main/java/org/robolancers321/commands/launcher/YeetAmp.java b/src/main/java/org/robolancers321/commands/launcher/YeetAmp.java new file mode 100644 index 0000000..6763c91 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/launcher/YeetAmp.java @@ -0,0 +1,21 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.launcher; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.robolancers321.subsystems.launcher.Indexer; +import org.robolancers321.subsystems.launcher.Pivot; +import org.robolancers321.subsystems.launcher.Shooter; + +public class YeetAmp extends SequentialCommandGroup { + + private Indexer indexer = Indexer.getInstance(); + private Pivot pivot = Pivot.getInstance(); + private Shooter shooter = Shooter.getInstance(); + + public YeetAmp() { + addRequirements(indexer, pivot, shooter); + addCommands( + pivot.positionAmp(), new ParallelCommandGroup(shooter.yeetNoteAmp(), indexer.index())); + } +} diff --git a/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java b/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java new file mode 100644 index 0000000..3c232ff --- /dev/null +++ b/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java @@ -0,0 +1,27 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.launcher; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import java.util.function.DoubleSupplier; +import org.robolancers321.subsystems.launcher.Indexer; +import org.robolancers321.subsystems.launcher.Pivot; +import org.robolancers321.subsystems.launcher.Shooter; + +public class YeetSpeaker extends ParallelCommandGroup { + + private Indexer indexer = Indexer.getInstance(); + + private Pivot pivot = Pivot.getInstance(); + + private Shooter shooter = Shooter.getInstance(); + + public YeetSpeaker(DoubleSupplier distanceFromSpeaker) { + + addRequirements(indexer, pivot, shooter); + // Run shooter and aim at the same time, index only after at setpoint + addCommands( + shooter.yeetNoteSpeaker(distanceFromSpeaker), + new SequentialCommandGroup(pivot.aimAtSpeaker(distanceFromSpeaker), indexer.index())); + } +} From e1e6570ec057cc69b9d7b44bfe257a3f5ac03812 Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Thu, 25 Jan 2024 08:03:07 -0500 Subject: [PATCH 30/57] feat(launcher): Made stuff final --- .../subsystems/launcher/Indexer.java | 8 ++-- .../subsystems/launcher/Pivot.java | 8 ++-- .../subsystems/launcher/Shooter.java | 46 +++++++++---------- 3 files changed, 31 insertions(+), 31 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index 9e6a3ca..b972db1 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -47,11 +47,11 @@ public static Indexer getInstance() { * Implementation */ - private CANSparkMax motor; - private SparkPIDController controller; - private RelativeEncoder encoder; + private final CANSparkMax motor; + private final SparkPIDController controller; + private final RelativeEncoder encoder; - private DigitalInput beamBreak; + private final DigitalInput beamBreak; private Indexer() { this.motor = new CANSparkMax(kMotorPort, kBrushless); diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 6e6be06..c21d8b9 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -58,7 +58,7 @@ public static Pivot getInstance() { private static final double kInterpolationThreshold = 0.0; - private double latest_distance = 0.0; + private final double latest_distance = 0.0; private InterpolationTable.AimCharacteristic latest_characteristic; @@ -68,7 +68,7 @@ public enum PivotSetpoint { kSpeaker(0.0), kAmp(0.0); - private double angle; + private final double angle; PivotSetpoint(double angle) { this.angle = angle; @@ -83,8 +83,8 @@ public double getAngle() { * Implementation */ - private CANSparkMax motor; - private AbsoluteEncoder absoluteEncoder; + private final CANSparkMax motor; + private final AbsoluteEncoder absoluteEncoder; private ArmFeedforward feedforwardController; private Pivot() { diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java b/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java index 63037f5..ed9b877 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java @@ -27,56 +27,56 @@ public static Shooter getInstance() { private final int kLeftMotorID = 0; private final int kRightMotorID = 0; - private int kLeftCurrentLimit = 40; + private final int kLeftCurrentLimit = 40; - private int kRightCurrentLimit = 40; + private final int kRightCurrentLimit = 40; - private boolean kInvertLeftMotor = false; + private final boolean kInvertLeftMotor = false; - private boolean kInvertRightMotor = false; + private final boolean kInvertRightMotor = false; - private double kLeftRotPerMinToDegPerSec = 0.0; + private final double kLeftRotPerMinToDegPerSec = 0.0; - private double kRightRotPerMinToDegPerSec = 0.0; + private final double kRightRotPerMinToDegPerSec = 0.0; - private double kLeftFF = 0.0; + private final double kLeftFF = 0.0; - private double kRightFF = 0.0; + private final double kRightFF = 0.0; - private double kAmpLeftSpeed = 0.0; + private final double kAmpLeftSpeed = 0.0; - private double kRightAmpSpeed = 0.0; + private final double kRightAmpSpeed = 0.0; private double kRampUpRate = 0.5; - private double kErrorTolerance = 0.0; + private final double kErrorTolerance = 0.0; - private double kInterpolationThreshold = 0.0; + private final double kInterpolationThreshold = 0.0; - private int kBeamBreakChannelPort = 0; + private final int kBeamBreakChannelPort = 0; private double leftSetpointSpeed; private double rightSetpointSpeed; - private CANSparkMax left_motor; + private final CANSparkMax left_motor; - private CANSparkMax right_motor; + private final CANSparkMax right_motor; - private AbsoluteEncoder left_encoder; + private final AbsoluteEncoder left_encoder; - private AbsoluteEncoder right_encoder; + private final AbsoluteEncoder right_encoder; - private double latest_distance = 0.0; + private final double latest_distance = 0.0; private InterpolationTable.AimCharacteristic latest_characteristic; - private SparkPIDController left_controller; - private SparkPIDController right_controller; + private final SparkPIDController left_controller; + private final SparkPIDController right_controller; - private SlewRateLimiter left_limiter; - private SlewRateLimiter right_limiter; + private final SlewRateLimiter left_limiter; + private final SlewRateLimiter right_limiter; - private DigitalInput beam_break; + private final DigitalInput beam_break; private Shooter() { From e17908b92c1aa16e698c35c31299200c3cb927fc Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Fri, 26 Jan 2024 08:55:19 -0500 Subject: [PATCH 31/57] made changes and todos for launcher. refactored some names, modifiers, and organization. todos mostly focus on control flow for commands --- gradlew | 0 .../org/robolancers321/BuildConstants.java | 10 +- .../commands/launcher/YeetSpeaker.java | 5 +- .../subsystems/launcher/Indexer.java | 4 +- .../subsystems/launcher/Pivot.java | 9 +- .../subsystems/launcher/Shooter.java | 209 ++++++++---------- 6 files changed, 115 insertions(+), 122 deletions(-) mode change 100644 => 100755 gradlew diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index 443621a..b1985e0 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 3; - public static final String GIT_SHA = "d13c62ef0ceab7cdac174d739f9809756c7f7ca4"; - public static final String GIT_DATE = "2024-01-08 20:26:15 EST"; + public static final int GIT_REVISION = 22; + public static final String GIT_SHA = "e1e6570ec057cc69b9d7b44bfe257a3f5ac03812"; + public static final String GIT_DATE = "2024-01-25 08:03:07 EST"; public static final String GIT_BRANCH = "dev-launcher-4"; - public static final String BUILD_DATE = "2024-01-18 19:16:36 EST"; - public static final long BUILD_UNIX_TIME = 1705623396997L; + public static final String BUILD_DATE = "2024-01-26 08:51:31 EST"; + public static final long BUILD_UNIX_TIME = 1706277091728L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java b/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java index 3c232ff..936cd7c 100644 --- a/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java +++ b/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java @@ -22,6 +22,9 @@ public YeetSpeaker(DoubleSupplier distanceFromSpeaker) { // Run shooter and aim at the same time, index only after at setpoint addCommands( shooter.yeetNoteSpeaker(distanceFromSpeaker), - new SequentialCommandGroup(pivot.aimAtSpeaker(distanceFromSpeaker), indexer.index())); + new SequentialCommandGroup( + pivot.aimAtSpeaker(distanceFromSpeaker), + indexer.index() // TODO: only run this when shooter is at speed + )); } } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index b972db1..ef58679 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -16,7 +16,7 @@ public class Indexer extends SubsystemBase { * Singleton */ - private static Indexer instance; + private static Indexer instance = null; public static Indexer getInstance() { if (instance == null) instance = new Indexer(); @@ -120,6 +120,8 @@ public Command manualIndex(double appliedSpeed) { } public Command index() { + // TODO: until beam break goes from true to false, also maybe add a time delay + // TODO: should this beam break be the same as shooter? return run(this::intakeJawn).until(this::jawnDetected).finallyDo(this::stopSpinningJawn); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index c21d8b9..92a33f1 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -20,7 +20,7 @@ public class Pivot extends ProfiledPIDSubsystem { * Singleton */ - private static Pivot instance; + private static Pivot instance = null; public static Pivot getInstance() { if (instance == null) instance = new Pivot(); @@ -58,7 +58,7 @@ public static Pivot getInstance() { private static final double kInterpolationThreshold = 0.0; - private final double latest_distance = 0.0; + private double latest_distance = 0.0; private InterpolationTable.AimCharacteristic latest_characteristic; @@ -157,14 +157,15 @@ public void setAngleGoal(PivotSetpoint goal) { } public void setAngleGoal(DoubleSupplier distance) { - if (!epsilonEquals(distance.getAsDouble(), latest_distance, kInterpolationThreshold)) + if (!epsilonEquals(distance.getAsDouble(), latest_distance, kInterpolationThreshold)) { latest_characteristic = InterpolationTable.interpolate(distance.getAsDouble()); + latest_distance = distance.getAsDouble(); + } setAngleGoal(latest_characteristic.getPitchAngle()); } public Command aimAtSpeaker(DoubleSupplier distance) { - return run(() -> setAngleGoal(distance)).until(this::atGoal); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java b/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java index ed9b877..7cf514c 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java @@ -13,182 +13,166 @@ import org.robolancers321.util.InterpolationTable; public class Shooter extends SubsystemBase { - private static Shooter INSTANCE; + private static Shooter instance = null; public static Shooter getInstance() { - if (INSTANCE == null) { - INSTANCE = new Shooter(); - } - return INSTANCE; + if (instance == null) instance = new Shooter(); + + return instance; } /* Constants */ - private final int kLeftMotorID = 0; - private final int kRightMotorID = 0; - - private final int kLeftCurrentLimit = 40; - - private final int kRightCurrentLimit = 40; - - private final boolean kInvertLeftMotor = false; - - private final boolean kInvertRightMotor = false; - - private final double kLeftRotPerMinToDegPerSec = 0.0; + private static final int kLeftMotorID = 0; + private static final int kRightMotorID = 0; + private static final int kBeamBreakPort = 0; - private final double kRightRotPerMinToDegPerSec = 0.0; + private static final int kCurrentLimit = 40; - private final double kLeftFF = 0.0; + private static final boolean kInvertLeftMotor = false; + private static final boolean kInvertRightMotor = false; - private final double kRightFF = 0.0; + // TODO: store this better? + private static final double kAmpLeftSpeed = 0.0; + private static final double kRightAmpSpeed = 0.0; - private final double kAmpLeftSpeed = 0.0; + private static double kRampUpRate = 0.5; - private final double kRightAmpSpeed = 0.0; - - private double kRampUpRate = 0.5; + private static final double kFF = 0.0; private final double kErrorTolerance = 0.0; + private final double kInterpolationCacheThreshold = + 0.0; // the distance at which interpolation table recalculates setpoints - private final double kInterpolationThreshold = 0.0; - - private final int kBeamBreakChannelPort = 0; - private double leftSetpointSpeed; - - private double rightSetpointSpeed; + /* + * Implementation + */ - private final CANSparkMax left_motor; + private CANSparkMax leftMotor; + private CANSparkMax rightMotor; - private final CANSparkMax right_motor; + private RelativeEncoder leftEncoder; + private RelativeEncoder rightEncoder; - private final AbsoluteEncoder left_encoder; + private SparkPIDController leftController; + private SparkPIDController rightController; - private final AbsoluteEncoder right_encoder; + private SlewRateLimiter leftLimiter; + private SlewRateLimiter rightLimiter; - private final double latest_distance = 0.0; + private DigitalInput beamBreak; - private InterpolationTable.AimCharacteristic latest_characteristic; + private double latestDistance = 0.0; + private InterpolationTable.AimCharacteristic latestCharacteristic = null; - private final SparkPIDController left_controller; - private final SparkPIDController right_controller; - - private final SlewRateLimiter left_limiter; - private final SlewRateLimiter right_limiter; - - private final DigitalInput beam_break; + private double leftSetpoint = 0.0; + private double rightSetpoint = 0.0; private Shooter() { + this.leftMotor = new CANSparkMax(kLeftMotorID, CANSparkLowLevel.MotorType.kBrushless); + this.rightMotor = new CANSparkMax(kRightMotorID, CANSparkLowLevel.MotorType.kBrushless); - beam_break = new DigitalInput(kBeamBreakChannelPort); - - left_motor = new CANSparkMax(kLeftMotorID, CANSparkLowLevel.MotorType.kBrushless); - right_motor = new CANSparkMax(kRightMotorID, CANSparkLowLevel.MotorType.kBrushless); + this.leftEncoder = this.leftMotor.getEncoder(); + this.rightEncoder = this.rightMotor.getEncoder(); - left_encoder = left_motor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); - right_encoder = right_motor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); + this.leftController = this.leftMotor.getPIDController(); + this.rightController = this.rightMotor.getPIDController(); - left_controller = left_motor.getPIDController(); - right_controller = right_motor.getPIDController(); + this.leftLimiter = new SlewRateLimiter(kRampUpRate); + this.rightLimiter = new SlewRateLimiter(kRampUpRate); - left_limiter = new SlewRateLimiter(kRampUpRate); - right_limiter = new SlewRateLimiter(kRampUpRate); + this.beamBreak = new DigitalInput(kBeamBreakPort); - configureMotors(); - configureControllers(); - configureEncoders(); + this.configureMotors(); + this.configureControllers(); + this.configureEncoders(); } private void configureControllers() { - left_controller.setFF(kLeftFF); - right_controller.setFF(kRightFF); + this.leftController.setFF(kFF); + this.rightController.setFF(kFF); } private void configureMotors() { - left_motor.setInverted(kInvertLeftMotor); - left_motor.setIdleMode(CANSparkBase.IdleMode.kBrake); - left_motor.setSmartCurrentLimit(kLeftCurrentLimit); - left_motor.enableVoltageCompensation(12); - left_motor.burnFlash(); + this.leftMotor.setInverted(kInvertLeftMotor); + this.leftMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + this.leftMotor.setSmartCurrentLimit(kCurrentLimit); + this.leftMotor.enableVoltageCompensation(12); + this.leftMotor.burnFlash(); - right_motor.setInverted(kInvertRightMotor); - right_motor.setIdleMode(CANSparkBase.IdleMode.kBrake); - right_motor.setSmartCurrentLimit(kRightCurrentLimit); - right_motor.enableVoltageCompensation(12); - right_motor.burnFlash(); + this.rightMotor.setInverted(kInvertRightMotor); + this.rightMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + this.rightMotor.setSmartCurrentLimit(kCurrentLimit); + this.rightMotor.enableVoltageCompensation(12); + this.rightMotor.burnFlash(); } private void configureEncoders() { - left_encoder.setInverted(kInvertLeftMotor); - left_encoder.setVelocityConversionFactor(kLeftRotPerMinToDegPerSec); - - right_encoder.setInverted(kInvertRightMotor); - right_encoder.setVelocityConversionFactor(kRightRotPerMinToDegPerSec); + this.leftEncoder.setInverted(kInvertLeftMotor); + this.rightEncoder.setInverted(kInvertRightMotor); } private double getLeftVelocity() { - return left_encoder.getVelocity(); + // TODO: filter here? + return this.leftEncoder.getVelocity(); } private double getRightVelocity() { - return right_encoder.getVelocity(); + // TODO: filter here? + return this.rightEncoder.getVelocity(); } - private void setGoalLR(double leftSpeed, double rightSpeed) { - left_controller.setReference( - left_limiter.calculate(leftSpeed), CANSparkBase.ControlType.kVelocity); - right_controller.setReference( - right_limiter.calculate(rightSpeed), CANSparkBase.ControlType.kVelocity); + private void useControllers(double leftSpeed, double rightSpeed) { + this.leftController.setReference( + this.leftLimiter.calculate(leftSpeed), CANSparkBase.ControlType.kVelocity); + this.rightController.setReference( + this.rightLimiter.calculate(rightSpeed), CANSparkBase.ControlType.kVelocity); } public void yeetNoteSpeaker(double distance) { - if (!epsilonEquals(distance, latest_distance, kInterpolationThreshold)) - latest_characteristic = InterpolationTable.interpolate(distance); + if (!epsilonEquals(distance, latestDistance, kInterpolationCacheThreshold)) { + latestDistance = distance; + latestCharacteristic = InterpolationTable.interpolate(distance); + } - leftSetpointSpeed = latest_characteristic.getLeftSpeed(); - rightSetpointSpeed = latest_characteristic.getRightSpeed(); + this.leftSetpoint = latestCharacteristic.getLeftSpeed(); + this.rightSetpoint = latestCharacteristic.getRightSpeed(); - setGoalLR(leftSetpointSpeed, rightSetpointSpeed); + this.useControllers(this.leftSetpoint, this.rightSetpoint); } public Command yeetNoteSpeaker(DoubleSupplier distance) { - return run(() -> yeetNoteSpeaker(distance.getAsDouble())).until(beam_break::get); - } - - public void setRampUpRate(double rate) { - kRampUpRate = rate; + // TODO: until beam break goes from true to false, also maybe add a time delay + return run(() -> this.yeetNoteSpeaker(distance.getAsDouble())).until(beamBreak::get); } public boolean atLeftSetpoint() { - return epsilonEquals(getLeftVelocity(), leftSetpointSpeed, kErrorTolerance); + return epsilonEquals(this.getLeftVelocity(), this.leftSetpoint, kErrorTolerance); } public boolean atRightSetpoint() { - return epsilonEquals(getRightVelocity(), leftSetpointSpeed, kErrorTolerance); + return epsilonEquals(this.getRightVelocity(), this.rightSetpoint, kErrorTolerance); } public boolean atSetpoint() { - return atLeftSetpoint() && atLeftSetpoint(); + return this.atLeftSetpoint() && this.atLeftSetpoint(); } - public void yeet(double speed) { - left_motor.set(speed); - right_motor.set(-speed); + public void dangerouslyYeet(double leftSpeed, double rightSpeed) { + this.leftMotor.set(leftSpeed); + this.rightMotor.set(rightSpeed); } public void yeetNoteAmp(double distance) { - leftSetpointSpeed = kAmpLeftSpeed; - rightSetpointSpeed = kRightAmpSpeed; - setGoalLR(kAmpLeftSpeed, kRightAmpSpeed); - } + this.leftSetpoint = kAmpLeftSpeed; + this.rightSetpoint = kRightAmpSpeed; - public Command yeetNoteAmp() { - return run(this::yeetNoteAmp).until(beam_break::get); + this.useControllers(this.leftSetpoint, this.rightSetpoint); } - @Override - public void periodic() { - doSendables(); + public Command yeetNoteAmp() { + // TODO: is this the right end condition? + return run(this::yeetNoteAmp).until(beamBreak::get); } private void doSendables() { @@ -197,16 +181,19 @@ private void doSendables() { SmartDashboard.putNumber("Right flywheel velocity", getRightVelocity()); } + @Override + public void periodic() { + this.doSendables(); + } + public void initTuning() { - SmartDashboard.putNumber("Left FF", kLeftFF); - SmartDashboard.putNumber("Right FF", kRightFF); + SmartDashboard.putNumber("shooter kff", SmartDashboard.getNumber("shooter kff", kFF)); } public void tune() { - double leftFF = SmartDashboard.getNumber("Left FF", kLeftFF); - double rightFF = SmartDashboard.getNumber("Right FF", kRightFF); - left_controller.setFF(leftFF); + double tunedFF = SmartDashboard.getNumber("shooter kff", kFF); - right_controller.setFF(rightFF); + leftController.setFF(tunedFF); + rightController.setFF(tunedFF); } } From af3d686666834029126650af613228443bf6d083 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Fri, 26 Jan 2024 08:57:51 -0500 Subject: [PATCH 32/57] small change to instantiation of subsystems in commands --- .../robolancers321/commands/launcher/YeetAmp.java | 10 +++++++--- .../commands/launcher/YeetSpeaker.java | 12 ++++++------ 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/robolancers321/commands/launcher/YeetAmp.java b/src/main/java/org/robolancers321/commands/launcher/YeetAmp.java index 6763c91..e4db16b 100644 --- a/src/main/java/org/robolancers321/commands/launcher/YeetAmp.java +++ b/src/main/java/org/robolancers321/commands/launcher/YeetAmp.java @@ -9,11 +9,15 @@ public class YeetAmp extends SequentialCommandGroup { - private Indexer indexer = Indexer.getInstance(); - private Pivot pivot = Pivot.getInstance(); - private Shooter shooter = Shooter.getInstance(); + private Indexer indexer; + private Pivot pivot; + private Shooter shooter; public YeetAmp() { + this.indexer = Indexer.getInstance(); + this.pivot = Pivot.getInstance(); + this.shooter = Shooter.getInstance(); + addRequirements(indexer, pivot, shooter); addCommands( pivot.positionAmp(), new ParallelCommandGroup(shooter.yeetNoteAmp(), indexer.index())); diff --git a/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java b/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java index 936cd7c..c7cb53d 100644 --- a/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java +++ b/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java @@ -9,14 +9,14 @@ import org.robolancers321.subsystems.launcher.Shooter; public class YeetSpeaker extends ParallelCommandGroup { - - private Indexer indexer = Indexer.getInstance(); - - private Pivot pivot = Pivot.getInstance(); - - private Shooter shooter = Shooter.getInstance(); + private Indexer indexer; + private Pivot pivot; + private Shooter shooter; public YeetSpeaker(DoubleSupplier distanceFromSpeaker) { + this.indexer = Indexer.getInstance(); + this.pivot = Pivot.getInstance(); + this.shooter = Shooter.getInstance(); addRequirements(indexer, pivot, shooter); // Run shooter and aim at the same time, index only after at setpoint From 37acd55721db0f2c63dbcb6a46ecead69c08fa2d Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Fri, 26 Jan 2024 09:09:23 -0500 Subject: [PATCH 33/57] camel case --- .../robolancers321/subsystems/launcher/Pivot.java | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 92a33f1..fb98e58 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -58,9 +58,8 @@ public static Pivot getInstance() { private static final double kInterpolationThreshold = 0.0; - private double latest_distance = 0.0; - - private InterpolationTable.AimCharacteristic latest_characteristic; + private double latestDistance = 0.0; + private InterpolationTable.AimCharacteristic latestCharacteristic; public enum PivotSetpoint { kRetracted(0.0), @@ -157,12 +156,12 @@ public void setAngleGoal(PivotSetpoint goal) { } public void setAngleGoal(DoubleSupplier distance) { - if (!epsilonEquals(distance.getAsDouble(), latest_distance, kInterpolationThreshold)) { - latest_characteristic = InterpolationTable.interpolate(distance.getAsDouble()); - latest_distance = distance.getAsDouble(); + if (!epsilonEquals(distance.getAsDouble(), latestDistance, kInterpolationThreshold)) { + latestCharacteristic = InterpolationTable.interpolate(distance.getAsDouble()); + latestDistance = distance.getAsDouble(); } - setAngleGoal(latest_characteristic.getPitchAngle()); + setAngleGoal(latestCharacteristic.getPitchAngle()); } public Command aimAtSpeaker(DoubleSupplier distance) { From b341dc6dcde44223ca9a67273f2bd1b9f27d7d18 Mon Sep 17 00:00:00 2001 From: koolfyn Date: Fri, 26 Jan 2024 15:57:51 -0500 Subject: [PATCH 34/57] chore(intake): Ran spotless --- .../commands/intake/RetractIntake.java | 32 +++++----- .../subsystems/intake/IntakeRetractor.java | 60 ++++++++++--------- 2 files changed, 47 insertions(+), 45 deletions(-) diff --git a/src/main/java/org/robolancers321/commands/intake/RetractIntake.java b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java index 394c4ef..ff33627 100644 --- a/src/main/java/org/robolancers321/commands/intake/RetractIntake.java +++ b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java @@ -1,26 +1,26 @@ +/* (C) Robolancers 2024 */ package org.robolancers321.commands.intake; +import edu.wpi.first.wpilibj2.command.Command; import org.robolancers321.subsystems.intake.IntakeRetractor; import org.robolancers321.subsystems.intake.IntakeRetractor.RetractorSetpoint; -import edu.wpi.first.wpilibj2.command.Command; - public class RetractIntake extends Command { - private IntakeRetractor intakeRetractor; + private IntakeRetractor intakeRetractor; + + public RetractIntake() { + this.intakeRetractor = IntakeRetractor.getInstance(); - public RetractIntake() { - this.intakeRetractor = IntakeRetractor.getInstance(); + addRequirements(this.intakeRetractor); + } - addRequirements(this.intakeRetractor); - } - - @Override - public void initialize() { - this.intakeRetractor.setSetpoint(RetractorSetpoint.kRetracted); - } + @Override + public void initialize() { + this.intakeRetractor.setSetpoint(RetractorSetpoint.kRetracted); + } - @Override - public boolean isFinished() { - return this.intakeRetractor.isAtSetpoint(); - } + @Override + public boolean isFinished() { + return this.intakeRetractor.isAtSetpoint(); + } } diff --git a/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java index eeb1d5c..8eb24fe 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java @@ -1,17 +1,17 @@ /* (C) Robolancers 2024 */ package org.robolancers321.subsystems.intake; + import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAbsoluteEncoder.Type; - import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -23,7 +23,7 @@ public class IntakeRetractor extends SubsystemBase { private static IntakeRetractor instance = null; - public static IntakeRetractor getInstance(){ + public static IntakeRetractor getInstance() { if (instance == null) instance = new IntakeRetractor(); return instance; @@ -50,7 +50,7 @@ public static IntakeRetractor getInstance(){ private static final double kMaxVelocityDeg = 0.0; private static final double kMaxAccelerationDeg = 0.0; - + private static final double kErrorThreshold = 2.0; public static enum RetractorSetpoint { @@ -60,11 +60,11 @@ public static enum RetractorSetpoint { private double angle; - private RetractorSetpoint(double angle){ + private RetractorSetpoint(double angle) { this.angle = angle; } - public double getAngle(){ + public double getAngle() { return this.angle; } } @@ -88,26 +88,27 @@ private IntakeRetractor() { this.motor = new CANSparkMax(kMotorPort, MotorType.kBrushless); this.absoluteEncoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); this.relativeEncoder = this.motor.getEncoder(); - this.feedbackController = new ProfiledPIDController(kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocityDeg, kMaxAccelerationDeg)); + this.feedbackController = + new ProfiledPIDController( + kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocityDeg, kMaxAccelerationDeg)); this.limitSwitch = new DigitalInput(kLimitSwitchPort); this.beamBreak = new DigitalInput(kBeamBreakPort); this.configureMotor(); this.configureEncoder(); - this.configureController(); + this.configureController(); this.configureLimitSwitchResponder(); } - private void configureMotor(){ + private void configureMotor() { this.motor.setInverted(kInvertMotor); this.motor.setIdleMode(CANSparkMax.IdleMode.kBrake); this.motor.setSmartCurrentLimit(kCurrentLimit); this.motor.enableVoltageCompensation(12); - } - private void configureEncoder(){ + private void configureEncoder() { this.relativeEncoder.setInverted(kInvertMotor); this.absoluteEncoder.setInverted(kInvertMotor); this.relativeEncoder.setPositionConversionFactor(360); @@ -115,14 +116,14 @@ private void configureEncoder(){ /* * TODO - * + * * set position conversion rate (store this rate as a constant), set encoder inversion - * + * * this should probably be done for both encoders just in case */ } - private void configureController(){ + private void configureController() { this.feedforwardController = new ArmFeedforward(kS, kG, kV); this.feedbackController.setP(kP); @@ -130,7 +131,7 @@ private void configureController(){ this.feedbackController.setD(kD); } - private void configureLimitSwitchResponder(){ + private void configureLimitSwitchResponder() { new Trigger(this::isLimitSwitchTriggered).whileTrue(new RunCommand(this::resetEncoder)); } @@ -145,46 +146,48 @@ public boolean isLimitSwitchTriggered() { return !this.limitSwitch.get(); } - public boolean isBeamBroken(){ + public boolean isBeamBroken() { return !this.beamBreak.get(); } - // TODO: this method is totally fine but may be scrapped when we move to a motion profiling implementation + // TODO: this method is totally fine but may be scrapped when we move to a motion profiling + // implementation public boolean isAtPosition(double position) { return Math.abs(this.getPosition() - position) < kErrorThreshold; } - public boolean isAtSetpoint(){ + public boolean isAtSetpoint() { return this.isAtPosition(this.setpoint.getAngle()); } - public void resetEncoder(double position){ + public void resetEncoder(double position) { this.relativeEncoder.setPosition(position); } - public void resetEncoder(){ + public void resetEncoder() { this.resetEncoder(RetractorSetpoint.kRetracted.getAngle()); } - public void setSetpoint(RetractorSetpoint setpoint){ + public void setSetpoint(RetractorSetpoint setpoint) { this.setpoint = setpoint; } - private void useController(){ + private void useController() { State setpointState = this.feedbackController.getSetpoint(); - double feedforwardOutput = this.feedforwardController.calculate(setpointState.position, setpointState.velocity); + double feedforwardOutput = + this.feedforwardController.calculate(setpointState.position, setpointState.velocity); double feedbackOutput = this.feedbackController.calculate(this.getPosition()); double controllerOutput = feedforwardOutput + feedbackOutput; this.motor.setVoltage(controllerOutput); } - private void doSendables(){ + private void doSendables() { SmartDashboard.putNumber("current position", relativeEncoder.getPosition()); SmartDashboard.putBoolean("note detected", this.isBeamBroken()); SmartDashboard.putBoolean("Retractor at position(deg)", this.isAtSetpoint()); - } + } @Override public void periodic() { @@ -192,7 +195,7 @@ public void periodic() { doSendables(); } - public void initTuning(){ + public void initTuning() { SmartDashboard.putNumber("retractor kP", SmartDashboard.getNumber("retractor kP", kP)); SmartDashboard.putNumber("retractor kI", SmartDashboard.getNumber("retractor kI", kI)); SmartDashboard.putNumber("retractor kD", SmartDashboard.getNumber("retractor kD", kD)); @@ -202,7 +205,7 @@ public void initTuning(){ SmartDashboard.putNumber("retractor kG", SmartDashboard.getNumber("retractor kG", kG)); } - public void tune(){ + public void tune() { double tunedP = SmartDashboard.getNumber("retractor kp", kP); double tunedI = SmartDashboard.getNumber("retractor kI", kI); double tunedD = SmartDashboard.getNumber("retractor kD", kD); @@ -212,6 +215,5 @@ public void tune(){ this.feedbackController.setP(tunedP); this.feedbackController.setI(tunedI); this.feedbackController.setD(tunedD); - } } From 97ab51f790a6fd67687c02526f80a2836a7c72e3 Mon Sep 17 00:00:00 2001 From: Raphael Hoang Date: Fri, 26 Jan 2024 17:14:07 -0500 Subject: [PATCH 35/57] Added FF to intake --- .../java/org/robolancers321/BuildConstants.java | 7 ------- .../org/robolancers321/subsystem/Intake.java | 17 +++++++++++++++-- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index 98b3bb3..fb65396 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,16 +7,9 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; -<<<<<<< HEAD public static final int GIT_REVISION = 9; public static final String GIT_SHA = "d7f5a071320caae2dbf837f6d4b8063c990ef911"; public static final String GIT_DATE = "2024-01-24 17:03:30 EST"; -======= - - public static final int GIT_REVISION = 6; - public static final String GIT_SHA = "6377635d69d5713f8f8b5400fc7a136abe5c431e"; - public static final String GIT_DATE = "2024-01-21 18:36:14 EST"; ->>>>>>> 61e5548f8a28f491003a4d55329b23a68f4bb910 public static final String GIT_BRANCH = "dev-intake"; public static final String BUILD_DATE = "2024-01-24 17:22:28 EST"; public static final long BUILD_UNIX_TIME = 1706134948601L; diff --git a/src/main/java/org/robolancers321/subsystem/Intake.java b/src/main/java/org/robolancers321/subsystem/Intake.java index 09b5461..6d88b0a 100644 --- a/src/main/java/org/robolancers321/subsystem/Intake.java +++ b/src/main/java/org/robolancers321/subsystem/Intake.java @@ -3,30 +3,43 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkPIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Intake extends SubsystemBase { public CANSparkMax intakeMotor; + SparkPIDController intakPidController; public int kPort = 0; public double power = 0; + public double kFF = 0; public void Intake() { this.intakeMotor = new CANSparkMax(kPort, MotorType.kBrushless); this.intakeMotor.setSmartCurrentLimit(20); + intakPidController = intakeMotor.getPIDController(); + SmartDashboard.putNumber("kFF", kFF); + } + + @Override + public void periodic() { + + double kFF = SmartDashboard.getNumber("kFF", 0); + intakPidController.setFF(kFF); } public Command in(double power) { - return runOnce(() -> intakeMotor.set(power)); + return runOnce(() -> intakeMotor.set(/*Math.abs*/(power))); } public Command out(double power) { - return runOnce(() -> intakeMotor.set(-power)); + return runOnce(() -> intakeMotor.set(-/*Math.abs*/(power))); } public Command off(double power) { From 996ae50adb1efafc86dd1e444c5108e7adde399c Mon Sep 17 00:00:00 2001 From: Raphael Hoang Date: Fri, 26 Jan 2024 17:16:22 -0500 Subject: [PATCH 36/57] Spotless done --- src/main/java/org/robolancers321/subsystem/Intake.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystem/Intake.java b/src/main/java/org/robolancers321/subsystem/Intake.java index 6d88b0a..e8180b5 100644 --- a/src/main/java/org/robolancers321/subsystem/Intake.java +++ b/src/main/java/org/robolancers321/subsystem/Intake.java @@ -34,12 +34,12 @@ public void periodic() { public Command in(double power) { - return runOnce(() -> intakeMotor.set(/*Math.abs*/(power))); + return runOnce(() -> intakeMotor.set(/*Math.abs*/ (power))); } public Command out(double power) { - return runOnce(() -> intakeMotor.set(-/*Math.abs*/(power))); + return runOnce(() -> intakeMotor.set(- /*Math.abs*/(power))); } public Command off(double power) { From f1ac5560dbe6e96bd7b6227c4ac7895348a08ae1 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Sun, 28 Jan 2024 16:33:52 -0500 Subject: [PATCH 37/57] refactored naming, changed modifiers, and reoganized a bit. configuration and tuning methods still need to be written --- .../org/robolancers321/RobotContainer.java | 10 +- .../org/robolancers321/subsystem/Intake.java | 49 -------- .../org/robolancers321/subsystem/Sucker.java | 112 ++++++++++++++++++ 3 files changed, 119 insertions(+), 52 deletions(-) delete mode 100644 src/main/java/org/robolancers321/subsystem/Intake.java create mode 100644 src/main/java/org/robolancers321/subsystem/Sucker.java diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 5e03f9d..8df9fb9 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -1,15 +1,19 @@ /* (C) Robolancers 2024 */ package org.robolancers321; +import org.robolancers321.subsystem.Sucker; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import org.robolancers321.subsystem.Intake; public class RobotContainer { - - public Intake intake = new Intake(); + private Sucker sucker; public RobotContainer() { + this.sucker = Sucker.getInstance(); + + this.sucker.setDefaultCommand(this.sucker.tuneController()); + configureBindings(); } diff --git a/src/main/java/org/robolancers321/subsystem/Intake.java b/src/main/java/org/robolancers321/subsystem/Intake.java deleted file mode 100644 index e8180b5..0000000 --- a/src/main/java/org/robolancers321/subsystem/Intake.java +++ /dev/null @@ -1,49 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.subsystem; - -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkPIDController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Intake extends SubsystemBase { - - public CANSparkMax intakeMotor; - SparkPIDController intakPidController; - - public int kPort = 0; - public double power = 0; - public double kFF = 0; - - public void Intake() { - - this.intakeMotor = new CANSparkMax(kPort, MotorType.kBrushless); - this.intakeMotor.setSmartCurrentLimit(20); - intakPidController = intakeMotor.getPIDController(); - SmartDashboard.putNumber("kFF", kFF); - } - - @Override - public void periodic() { - - double kFF = SmartDashboard.getNumber("kFF", 0); - intakPidController.setFF(kFF); - } - - public Command in(double power) { - - return runOnce(() -> intakeMotor.set(/*Math.abs*/ (power))); - } - - public Command out(double power) { - - return runOnce(() -> intakeMotor.set(- /*Math.abs*/(power))); - } - - public Command off(double power) { - - return runOnce(() -> intakeMotor.set(0)); - } -} diff --git a/src/main/java/org/robolancers321/subsystem/Sucker.java b/src/main/java/org/robolancers321/subsystem/Sucker.java new file mode 100644 index 0000000..af3c156 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystem/Sucker.java @@ -0,0 +1,112 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystem; + +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Sucker extends SubsystemBase { + /* + * Singleton + */ + + private static Sucker instance = null; + + public static Sucker getInstance(){ + if (instance == null) instance = new Sucker(); + + return instance; + } + + /* + * Constants + */ + + private static final int kMotorPort = 0; + + private static final boolean kInvertMotor = false; + private static final int kCurrentLimit = 20; + + private static final double kP = 0.0; + private static final double kI = 0.0; + private static final double kD = 0.0; + private static final double kFF = 0.01; + + private static final double kInRPM = -2000; + private static final double kOutRPM = 1000; + + /* + * Implementation + */ + + private CANSparkMax motor; + private RelativeEncoder encoder; + private SparkPIDController controller; + + private Sucker() { + this.motor = new CANSparkMax(kMotorPort, MotorType.kBrushless); + this.encoder = this.motor.getEncoder(); + this.controller = this.motor.getPIDController(); + + this.configureMotor(); + this.configureEncoder(); + this.configureController(); + } + + private void configureMotor(){ + // TODO: inversion, current limit, idle mode, voltage compensation + } + + public void configureEncoder(){ + // TODO: set velocity conversion factor to 1 + } + + public void configureController(){ + // TODO: set kP, kI, kD, kFF on controller + } + + public double getVelocityRPM(){ + return this.encoder.getVelocity(); + } + + private void doSendables(){ + SmartDashboard.putNumber("sucker velocity rpm", this.getVelocityRPM()); + } + + @Override + public void periodic() { + this.doSendables(); + } + + public void initTuning(){ + // TODO: put default values for kP, kI, kD, kFF on SmartDashboard + } + + public void tune(){ + // TODO: read the values for kP, kI, kD, kFF from SmartDashboard and update your controller + } + + public Command in() { + return run(() -> this.controller.setReference(kInRPM, ControlType.kVelocity)); + } + + public Command out(double power) { + return run(() -> this.controller.setReference(kOutRPM, ControlType.kVelocity)); + } + + public Command off() { + return runOnce(() -> this.controller.setReference(0.0, ControlType.kVelocity)); + } + + public Command tuneController(){ + initTuning(); + + return run(this::tune); + } +} From b9c55d082934ec22c1970b5c2a8d9e4c7860e043 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Sun, 28 Jan 2024 17:07:16 -0500 Subject: [PATCH 38/57] final refactor for retractor. changed class name, enum name for clarity, changed isAtGoal implementation to use controller, made all of the commands callable from the subsystem --- .../org/robolancers321/RobotContainer.java | 8 + .../commands/intake/RetractIntake.java | 26 --- .../{IntakeRetractor.java => Retractor.java} | 149 ++++++++---------- 3 files changed, 75 insertions(+), 108 deletions(-) delete mode 100644 src/main/java/org/robolancers321/commands/intake/RetractIntake.java rename src/main/java/org/robolancers321/subsystems/intake/{IntakeRetractor.java => Retractor.java} (56%) diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 55b784c..f818f1c 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -1,11 +1,19 @@ /* (C) Robolancers 2024 */ package org.robolancers321; +import org.robolancers321.subsystems.intake.Retractor; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; public class RobotContainer { + Retractor retractor; + public RobotContainer() { + this.retractor = Retractor.getInstance(); + + this.retractor.setDefaultCommand(this.retractor.tuneControllers()); + configureBindings(); } diff --git a/src/main/java/org/robolancers321/commands/intake/RetractIntake.java b/src/main/java/org/robolancers321/commands/intake/RetractIntake.java deleted file mode 100644 index ff33627..0000000 --- a/src/main/java/org/robolancers321/commands/intake/RetractIntake.java +++ /dev/null @@ -1,26 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.commands.intake; - -import edu.wpi.first.wpilibj2.command.Command; -import org.robolancers321.subsystems.intake.IntakeRetractor; -import org.robolancers321.subsystems.intake.IntakeRetractor.RetractorSetpoint; - -public class RetractIntake extends Command { - private IntakeRetractor intakeRetractor; - - public RetractIntake() { - this.intakeRetractor = IntakeRetractor.getInstance(); - - addRequirements(this.intakeRetractor); - } - - @Override - public void initialize() { - this.intakeRetractor.setSetpoint(RetractorSetpoint.kRetracted); - } - - @Override - public boolean isFinished() { - return this.intakeRetractor.isAtSetpoint(); - } -} diff --git a/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java similarity index 56% rename from src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java rename to src/main/java/org/robolancers321/subsystems/intake/Retractor.java index 8eb24fe..8c2cdde 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/IntakeRetractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -4,27 +4,25 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAbsoluteEncoder.Type; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class IntakeRetractor extends SubsystemBase { +public class Retractor extends SubsystemBase { /* * Singleton */ - private static IntakeRetractor instance = null; + private static Retractor instance = null; - public static IntakeRetractor getInstance() { - if (instance == null) instance = new IntakeRetractor(); + public static Retractor getInstance() { + if (instance == null) instance = new Retractor(); return instance; } @@ -34,8 +32,6 @@ public static IntakeRetractor getInstance() { */ private static final int kMotorPort = 0; - private static final int kLimitSwitchPort = 0; - private static final int kBeamBreakPort = 0; private static final boolean kInvertMotor = false; private static final int kCurrentLimit = 20; @@ -43,24 +39,24 @@ public static IntakeRetractor getInstance() { private static final double kP = 0.000; private static final double kI = 0.000; private static final double kD = 0.000; + private static final double kS = 0.000; - private static final double kV = 0.000; - private static final double kA = 0.000; private static final double kG = 0.000; + private static final double kV = 0.000; private static final double kMaxVelocityDeg = 0.0; private static final double kMaxAccelerationDeg = 0.0; private static final double kErrorThreshold = 2.0; - public static enum RetractorSetpoint { + public static enum RetractorPosition { kRetracted(0), kMating(0), kIntake(0); private double angle; - private RetractorSetpoint(double angle) { + private RetractorPosition(double angle) { this.angle = angle; } @@ -74,31 +70,20 @@ public double getAngle() { */ private CANSparkMax motor; - private AbsoluteEncoder absoluteEncoder; - private RelativeEncoder relativeEncoder; + private AbsoluteEncoder encoder; private ProfiledPIDController feedbackController; private ArmFeedforward feedforwardController; - private DigitalInput limitSwitch; - private DigitalInput beamBreak; - - private RetractorSetpoint setpoint; - - private IntakeRetractor() { + private Retractor() { this.motor = new CANSparkMax(kMotorPort, MotorType.kBrushless); - this.absoluteEncoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); - this.relativeEncoder = this.motor.getEncoder(); + this.encoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); this.feedbackController = new ProfiledPIDController( kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocityDeg, kMaxAccelerationDeg)); - this.limitSwitch = new DigitalInput(kLimitSwitchPort); - this.beamBreak = new DigitalInput(kBeamBreakPort); - this.configureMotor(); this.configureEncoder(); this.configureController(); - this.configureLimitSwitchResponder(); } private void configureMotor() { @@ -109,67 +94,33 @@ private void configureMotor() { } private void configureEncoder() { - this.relativeEncoder.setInverted(kInvertMotor); - this.absoluteEncoder.setInverted(kInvertMotor); - this.relativeEncoder.setPositionConversionFactor(360); - this.absoluteEncoder.setPositionConversionFactor(360); - - /* - * TODO - * - * set position conversion rate (store this rate as a constant), set encoder inversion - * - * this should probably be done for both encoders just in case - */ + this.encoder.setInverted(kInvertMotor); + this.encoder.setPositionConversionFactor(360); // TODO: potentially a gear ratio on this } private void configureController() { - this.feedforwardController = new ArmFeedforward(kS, kG, kV); - this.feedbackController.setP(kP); this.feedbackController.setI(kI); this.feedbackController.setD(kD); - } + this.feedbackController.setTolerance(kErrorThreshold); - private void configureLimitSwitchResponder() { - new Trigger(this::isLimitSwitchTriggered).whileTrue(new RunCommand(this::resetEncoder)); + this.feedforwardController = new ArmFeedforward(kS, kG, kV); } public double getPosition() { - return this.absoluteEncoder.getPosition(); - - // if using relative encoder - // return this.relativeEncoder.getPosition(); - } - - public boolean isLimitSwitchTriggered() { - return !this.limitSwitch.get(); - } - - public boolean isBeamBroken() { - return !this.beamBreak.get(); - } - - // TODO: this method is totally fine but may be scrapped when we move to a motion profiling - // implementation - public boolean isAtPosition(double position) { - return Math.abs(this.getPosition() - position) < kErrorThreshold; - } - - public boolean isAtSetpoint() { - return this.isAtPosition(this.setpoint.getAngle()); + return this.encoder.getPosition(); } - public void resetEncoder(double position) { - this.relativeEncoder.setPosition(position); + public boolean isAtGoal() { + return this.feedbackController.atGoal(); } - public void resetEncoder() { - this.resetEncoder(RetractorSetpoint.kRetracted.getAngle()); + private void setGoal(double goal){ + this.feedbackController.setGoal(goal); } - public void setSetpoint(RetractorSetpoint setpoint) { - this.setpoint = setpoint; + private void setGoal(RetractorPosition goal){ + this.setGoal(goal.getAngle()); } private void useController() { @@ -180,13 +131,13 @@ private void useController() { double feedbackOutput = this.feedbackController.calculate(this.getPosition()); double controllerOutput = feedforwardOutput + feedbackOutput; - this.motor.setVoltage(controllerOutput); + + this.motor.set(controllerOutput); } private void doSendables() { - SmartDashboard.putNumber("current position", relativeEncoder.getPosition()); - SmartDashboard.putBoolean("note detected", this.isBeamBroken()); - SmartDashboard.putBoolean("Retractor at position(deg)", this.isAtSetpoint()); + SmartDashboard.putNumber("current position (deg)", this.getPosition()); + SmartDashboard.putBoolean("retractor at goal", this.isAtGoal()); } @Override @@ -195,25 +146,59 @@ public void periodic() { doSendables(); } - public void initTuning() { + private void initTuning() { SmartDashboard.putNumber("retractor kP", SmartDashboard.getNumber("retractor kP", kP)); SmartDashboard.putNumber("retractor kI", SmartDashboard.getNumber("retractor kI", kI)); SmartDashboard.putNumber("retractor kD", SmartDashboard.getNumber("retractor kD", kD)); + SmartDashboard.putNumber("retractor kS", SmartDashboard.getNumber("retractor kS", kS)); SmartDashboard.putNumber("retractor kV", SmartDashboard.getNumber("retractor kV", kV)); - SmartDashboard.putNumber("retractor kA", SmartDashboard.getNumber("retractor kA", kA)); SmartDashboard.putNumber("retractor kG", SmartDashboard.getNumber("retractor kG", kG)); + + SmartDashboard.putNumber("target retractor position", this.getPosition()); } - public void tune() { + private void tune() { double tunedP = SmartDashboard.getNumber("retractor kp", kP); double tunedI = SmartDashboard.getNumber("retractor kI", kI); double tunedD = SmartDashboard.getNumber("retractor kD", kD); - this.feedforwardController = new ArmFeedforward(kS, kG, kV); - this.feedbackController.setP(tunedP); this.feedbackController.setI(tunedI); this.feedbackController.setD(tunedD); + + double tunedS = SmartDashboard.getNumber("retractor kS", kS); + double tunedV = SmartDashboard.getNumber("retractor kV", kV); + double tunedG = SmartDashboard.getNumber("retractor kG", kG); + + this.feedforwardController = new ArmFeedforward(tunedS, tunedG, tunedV); + + double targetRetractorPosition = SmartDashboard.getNumber("target retractor position", this.getPosition()); + + this.setGoal(targetRetractorPosition); + } + + private Command moveToPosition(RetractorPosition position){ + this.setGoal(position); + + return new WaitUntilCommand(this::isAtGoal); + } + + public Command moveToRetracted(){ + return this.moveToPosition(RetractorPosition.kRetracted); + } + + public Command moveToMating(){ + return this.moveToPosition(RetractorPosition.kMating); + } + + public Command moveToIntake(){ + return this.moveToPosition(RetractorPosition.kIntake); + } + + public Command tuneControllers(){ + this.initTuning(); + + return run(this::tune); } } From abf90d962cf4ef8c4b171c3c84593b5efacaea64 Mon Sep 17 00:00:00 2001 From: Raphael Hoang Date: Mon, 29 Jan 2024 08:43:02 -0500 Subject: [PATCH 39/57] Did the todo's and made a minor change to commands. --- .../org/robolancers321/subsystem/Sucker.java | 37 ++++++++++++++++--- 1 file changed, 32 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystem/Sucker.java b/src/main/java/org/robolancers321/subsystem/Sucker.java index af3c156..cf12bd1 100644 --- a/src/main/java/org/robolancers321/subsystem/Sucker.java +++ b/src/main/java/org/robolancers321/subsystem/Sucker.java @@ -60,14 +60,26 @@ private Sucker() { } private void configureMotor(){ + + this.motor.setInverted(kInvertMotor); + this.motor.setIdleMode(CANSparkMax.IdleMode.kBrake); + this.motor.setSmartCurrentLimit(kCurrentLimit); + this.motor.enableVoltageCompensation(12); // TODO: inversion, current limit, idle mode, voltage compensation } public void configureEncoder(){ + + this.encoder.setVelocityConversionFactor(1); // TODO: set velocity conversion factor to 1 } public void configureController(){ + + this.controller.setP(kP); + this.controller.setI(kI); + this.controller.setD(kD); + this.controller.setFF(kFF); // TODO: set kP, kI, kD, kFF on controller } @@ -85,24 +97,39 @@ public void periodic() { } public void initTuning(){ + + SmartDashboard.putNumber("kP", SmartDashboard.getNumber("kP", kP)); + SmartDashboard.putNumber("kI", SmartDashboard.getNumber("kI", kI)); + SmartDashboard.putNumber("kD", SmartDashboard.getNumber("kD", kD)); + SmartDashboard.putNumber("kFF", SmartDashboard.getNumber("kFF", kFF)); // TODO: put default values for kP, kI, kD, kFF on SmartDashboard } public void tune(){ + + double PTuned = SmartDashboard.getNumber("kP", kP); + double ITuned = SmartDashboard.getNumber("kI", kI); + double DTuned = SmartDashboard.getNumber("kD", kD); + double FFTuned = SmartDashboard.getNumber("kFF", kFF); + + this.controller.setP(PTuned); + this.controller.setI(ITuned); + this.controller.setD(DTuned); + this.controller.setFF(FFTuned); // TODO: read the values for kP, kI, kD, kFF from SmartDashboard and update your controller } public Command in() { - return run(() -> this.controller.setReference(kInRPM, ControlType.kVelocity)); + return run(() -> this.controller.setReference(kInRPM, ControlType.kVelocity)).andThen(() -> this.controller.setReference(0, ControlType.kVelocity)); } public Command out(double power) { - return run(() -> this.controller.setReference(kOutRPM, ControlType.kVelocity)); + return run(() -> this.controller.setReference(kOutRPM, ControlType.kVelocity)).andThen(() -> this.controller.setReference(0, ControlType.kVelocity)); } - public Command off() { - return runOnce(() -> this.controller.setReference(0.0, ControlType.kVelocity)); - } + // public Command off() { + // return runOnce(() -> this.controller.setReference(0.0, ControlType.kVelocity)); + // } public Command tuneController(){ initTuning(); From ad923f7a483f8aaff03065c4aebdc217d5ca5d78 Mon Sep 17 00:00:00 2001 From: Raphael Hoang Date: Mon, 29 Jan 2024 08:46:59 -0500 Subject: [PATCH 40/57] Spotless --- .../org/robolancers321/RobotContainer.java | 3 +- .../org/robolancers321/subsystem/Sucker.java | 29 ++++++++++--------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 8df9fb9..99f80ff 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -1,10 +1,9 @@ /* (C) Robolancers 2024 */ package org.robolancers321; -import org.robolancers321.subsystem.Sucker; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import org.robolancers321.subsystem.Sucker; public class RobotContainer { private Sucker sucker; diff --git a/src/main/java/org/robolancers321/subsystem/Sucker.java b/src/main/java/org/robolancers321/subsystem/Sucker.java index cf12bd1..9767e6c 100644 --- a/src/main/java/org/robolancers321/subsystem/Sucker.java +++ b/src/main/java/org/robolancers321/subsystem/Sucker.java @@ -1,12 +1,11 @@ /* (C) Robolancers 2024 */ package org.robolancers321.subsystem; +import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkBase.ControlType; - import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -18,7 +17,7 @@ public class Sucker extends SubsystemBase { private static Sucker instance = null; - public static Sucker getInstance(){ + public static Sucker getInstance() { if (instance == null) instance = new Sucker(); return instance; @@ -59,7 +58,7 @@ private Sucker() { this.configureController(); } - private void configureMotor(){ + private void configureMotor() { this.motor.setInverted(kInvertMotor); this.motor.setIdleMode(CANSparkMax.IdleMode.kBrake); @@ -68,13 +67,13 @@ private void configureMotor(){ // TODO: inversion, current limit, idle mode, voltage compensation } - public void configureEncoder(){ + public void configureEncoder() { this.encoder.setVelocityConversionFactor(1); // TODO: set velocity conversion factor to 1 } - public void configureController(){ + public void configureController() { this.controller.setP(kP); this.controller.setI(kI); @@ -83,11 +82,11 @@ public void configureController(){ // TODO: set kP, kI, kD, kFF on controller } - public double getVelocityRPM(){ + public double getVelocityRPM() { return this.encoder.getVelocity(); } - private void doSendables(){ + private void doSendables() { SmartDashboard.putNumber("sucker velocity rpm", this.getVelocityRPM()); } @@ -95,8 +94,8 @@ private void doSendables(){ public void periodic() { this.doSendables(); } - - public void initTuning(){ + + public void initTuning() { SmartDashboard.putNumber("kP", SmartDashboard.getNumber("kP", kP)); SmartDashboard.putNumber("kI", SmartDashboard.getNumber("kI", kI)); @@ -105,7 +104,7 @@ public void initTuning(){ // TODO: put default values for kP, kI, kD, kFF on SmartDashboard } - public void tune(){ + public void tune() { double PTuned = SmartDashboard.getNumber("kP", kP); double ITuned = SmartDashboard.getNumber("kI", kI); @@ -120,18 +119,20 @@ public void tune(){ } public Command in() { - return run(() -> this.controller.setReference(kInRPM, ControlType.kVelocity)).andThen(() -> this.controller.setReference(0, ControlType.kVelocity)); + return run(() -> this.controller.setReference(kInRPM, ControlType.kVelocity)) + .andThen(() -> this.controller.setReference(0, ControlType.kVelocity)); } public Command out(double power) { - return run(() -> this.controller.setReference(kOutRPM, ControlType.kVelocity)).andThen(() -> this.controller.setReference(0, ControlType.kVelocity)); + return run(() -> this.controller.setReference(kOutRPM, ControlType.kVelocity)) + .andThen(() -> this.controller.setReference(0, ControlType.kVelocity)); } // public Command off() { // return runOnce(() -> this.controller.setReference(0.0, ControlType.kVelocity)); // } - public Command tuneController(){ + public Command tuneController() { initTuning(); return run(this::tune); From 1fa642ccbcd6472115248f4f9a049f827b2c1de1 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Mon, 29 Jan 2024 10:27:05 -0500 Subject: [PATCH 41/57] final refactor, ready to be merged with retractor --- .../org/robolancers321/subsystem/Sucker.java | 60 +++++++++---------- 1 file changed, 28 insertions(+), 32 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystem/Sucker.java b/src/main/java/org/robolancers321/subsystem/Sucker.java index 9767e6c..da02ed9 100644 --- a/src/main/java/org/robolancers321/subsystem/Sucker.java +++ b/src/main/java/org/robolancers321/subsystem/Sucker.java @@ -59,33 +59,31 @@ private Sucker() { } private void configureMotor() { - this.motor.setInverted(kInvertMotor); this.motor.setIdleMode(CANSparkMax.IdleMode.kBrake); this.motor.setSmartCurrentLimit(kCurrentLimit); this.motor.enableVoltageCompensation(12); - // TODO: inversion, current limit, idle mode, voltage compensation } - public void configureEncoder() { - + private void configureEncoder() { this.encoder.setVelocityConversionFactor(1); - // TODO: set velocity conversion factor to 1 } - public void configureController() { - + private void configureController() { this.controller.setP(kP); this.controller.setI(kI); this.controller.setD(kD); this.controller.setFF(kFF); - // TODO: set kP, kI, kD, kFF on controller } public double getVelocityRPM() { return this.encoder.getVelocity(); } + private void useController(double desiredRPM){ + this.controller.setReference(desiredRPM, ControlType.kVelocity); + } + private void doSendables() { SmartDashboard.putNumber("sucker velocity rpm", this.getVelocityRPM()); } @@ -95,43 +93,41 @@ public void periodic() { this.doSendables(); } - public void initTuning() { + private void initTuning() { + SmartDashboard.putNumber("sucker kP", SmartDashboard.getNumber("sucker kP", kP)); + SmartDashboard.putNumber("sucker kI", SmartDashboard.getNumber("sucker kI", kI)); + SmartDashboard.putNumber("sucker kD", SmartDashboard.getNumber("sucker kD", kD)); + SmartDashboard.putNumber("sucker kFF", SmartDashboard.getNumber("sucker kFF", kFF)); - SmartDashboard.putNumber("kP", SmartDashboard.getNumber("kP", kP)); - SmartDashboard.putNumber("kI", SmartDashboard.getNumber("kI", kI)); - SmartDashboard.putNumber("kD", SmartDashboard.getNumber("kD", kD)); - SmartDashboard.putNumber("kFF", SmartDashboard.getNumber("kFF", kFF)); - // TODO: put default values for kP, kI, kD, kFF on SmartDashboard + SmartDashboard.putNumber("target sucker rpm", 0.0); } - public void tune() { + private void tune() { + double tunedP = SmartDashboard.getNumber("sucker kP", kP); + double tunedI = SmartDashboard.getNumber("sucker kI", kI); + double tunedD = SmartDashboard.getNumber("sucker kD", kD); + double tunedFF = SmartDashboard.getNumber("sucker kFF", kFF); + + this.controller.setP(tunedP); + this.controller.setI(tunedI); + this.controller.setD(tunedD); + this.controller.setFF(tunedFF); - double PTuned = SmartDashboard.getNumber("kP", kP); - double ITuned = SmartDashboard.getNumber("kI", kI); - double DTuned = SmartDashboard.getNumber("kD", kD); - double FFTuned = SmartDashboard.getNumber("kFF", kFF); + double targetRPM = SmartDashboard.getNumber("target sucker rpm", 0.0); - this.controller.setP(PTuned); - this.controller.setI(ITuned); - this.controller.setD(DTuned); - this.controller.setFF(FFTuned); - // TODO: read the values for kP, kI, kD, kFF from SmartDashboard and update your controller + this.useController(targetRPM); } public Command in() { - return run(() -> this.controller.setReference(kInRPM, ControlType.kVelocity)) - .andThen(() -> this.controller.setReference(0, ControlType.kVelocity)); + return run(() -> this.useController(kInRPM)) + .finallyDo(() -> this.useController(0.0)); } public Command out(double power) { - return run(() -> this.controller.setReference(kOutRPM, ControlType.kVelocity)) - .andThen(() -> this.controller.setReference(0, ControlType.kVelocity)); + return run(() -> this.useController(kOutRPM)) + .finallyDo(() -> this.useController(0.0)); } - // public Command off() { - // return runOnce(() -> this.controller.setReference(0.0, ControlType.kVelocity)); - // } - public Command tuneController() { initTuning(); From 30829d1b76ad65e8ae6ea171e9a30dfe81d1c020 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Mon, 29 Jan 2024 10:28:46 -0500 Subject: [PATCH 42/57] spotless --- gradlew | 0 .../java/org/robolancers321/BuildConstants.java | 13 ++++++------- .../java/org/robolancers321/subsystem/Sucker.java | 8 +++----- 3 files changed, 9 insertions(+), 12 deletions(-) mode change 100644 => 100755 gradlew diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index fb65396..132cd5c 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,14 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 9; - public static final String GIT_SHA = "d7f5a071320caae2dbf837f6d4b8063c990ef911"; - public static final String GIT_DATE = "2024-01-24 17:03:30 EST"; + public static final int GIT_REVISION = 21; + public static final String GIT_SHA = "1fa642ccbcd6472115248f4f9a049f827b2c1de1"; + public static final String GIT_DATE = "2024-01-29 10:27:05 EST"; public static final String GIT_BRANCH = "dev-intake"; - public static final String BUILD_DATE = "2024-01-24 17:22:28 EST"; - public static final long BUILD_UNIX_TIME = 1706134948601L; - public static final int DIRTY = 0; - + public static final String BUILD_DATE = "2024-01-29 10:28:35 EST"; + public static final long BUILD_UNIX_TIME = 1706542115729L; + public static final int DIRTY = 1; private BuildConstants(){} } diff --git a/src/main/java/org/robolancers321/subsystem/Sucker.java b/src/main/java/org/robolancers321/subsystem/Sucker.java index da02ed9..a3ffb4c 100644 --- a/src/main/java/org/robolancers321/subsystem/Sucker.java +++ b/src/main/java/org/robolancers321/subsystem/Sucker.java @@ -80,7 +80,7 @@ public double getVelocityRPM() { return this.encoder.getVelocity(); } - private void useController(double desiredRPM){ + private void useController(double desiredRPM) { this.controller.setReference(desiredRPM, ControlType.kVelocity); } @@ -119,13 +119,11 @@ private void tune() { } public Command in() { - return run(() -> this.useController(kInRPM)) - .finallyDo(() -> this.useController(0.0)); + return run(() -> this.useController(kInRPM)).finallyDo(() -> this.useController(0.0)); } public Command out(double power) { - return run(() -> this.useController(kOutRPM)) - .finallyDo(() -> this.useController(0.0)); + return run(() -> this.useController(kOutRPM)).finallyDo(() -> this.useController(0.0)); } public Command tuneController() { From b4887b9c18b9af41bb108944c4c6b770ed3e6da8 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Mon, 29 Jan 2024 10:34:11 -0500 Subject: [PATCH 43/57] spotless --- gradlew | 0 .../org/robolancers321/RobotContainer.java | 5 ++-- .../subsystems/intake/Retractor.java | 23 ++++++++++--------- 3 files changed, 14 insertions(+), 14 deletions(-) mode change 100644 => 100755 gradlew diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index f818f1c..3691a8d 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -1,17 +1,16 @@ /* (C) Robolancers 2024 */ package org.robolancers321; -import org.robolancers321.subsystems.intake.Retractor; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import org.robolancers321.subsystems.intake.Retractor; public class RobotContainer { Retractor retractor; public RobotContainer() { this.retractor = Retractor.getInstance(); - + this.retractor.setDefaultCommand(this.retractor.tuneControllers()); configureBindings(); diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java index 8c2cdde..76a5b91 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -39,7 +39,7 @@ public static Retractor getInstance() { private static final double kP = 0.000; private static final double kI = 0.000; private static final double kD = 0.000; - + private static final double kS = 0.000; private static final double kG = 0.000; private static final double kV = 0.000; @@ -115,11 +115,11 @@ public boolean isAtGoal() { return this.feedbackController.atGoal(); } - private void setGoal(double goal){ + private void setGoal(double goal) { this.feedbackController.setGoal(goal); } - private void setGoal(RetractorPosition goal){ + private void setGoal(RetractorPosition goal) { this.setGoal(goal.getAngle()); } @@ -172,31 +172,32 @@ private void tune() { double tunedG = SmartDashboard.getNumber("retractor kG", kG); this.feedforwardController = new ArmFeedforward(tunedS, tunedG, tunedV); - - double targetRetractorPosition = SmartDashboard.getNumber("target retractor position", this.getPosition()); - + + double targetRetractorPosition = + SmartDashboard.getNumber("target retractor position", this.getPosition()); + this.setGoal(targetRetractorPosition); } - private Command moveToPosition(RetractorPosition position){ + private Command moveToPosition(RetractorPosition position) { this.setGoal(position); return new WaitUntilCommand(this::isAtGoal); } - public Command moveToRetracted(){ + public Command moveToRetracted() { return this.moveToPosition(RetractorPosition.kRetracted); } - public Command moveToMating(){ + public Command moveToMating() { return this.moveToPosition(RetractorPosition.kMating); } - public Command moveToIntake(){ + public Command moveToIntake() { return this.moveToPosition(RetractorPosition.kIntake); } - public Command tuneControllers(){ + public Command tuneControllers() { this.initTuning(); return run(this::tune); From 98ec876acf210c51a7146e70cdaf9765448c192a Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Mon, 29 Jan 2024 15:02:18 -0500 Subject: [PATCH 44/57] combined launcher subsystems, need to clean up beam break usage and review command structures --- .../org/robolancers321/RobotContainer.java | 6 + .../commands/launcher/YeetAmp.java | 25 --- .../commands/launcher/YeetSpeaker.java | 30 --- .../subsystems/launcher/AimTable.java | 47 ++++ .../subsystems/launcher/Flywheel.java | 203 ++++++++++++++++++ .../subsystems/launcher/Indexer.java | 105 +++++---- .../subsystems/launcher/Launcher.java | 76 +++++++ .../subsystems/launcher/Pivot.java | 119 +++++----- .../subsystems/launcher/Shooter.java | 199 ----------------- .../util/InterpolationTable.java | 86 -------- .../org/robolancers321/util/MathUtils.java | 2 +- .../org/robolancers321/util/TunableSet.java | 63 ------ 12 files changed, 452 insertions(+), 509 deletions(-) delete mode 100644 src/main/java/org/robolancers321/commands/launcher/YeetAmp.java delete mode 100644 src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java create mode 100644 src/main/java/org/robolancers321/subsystems/launcher/AimTable.java create mode 100644 src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java create mode 100644 src/main/java/org/robolancers321/subsystems/launcher/Launcher.java delete mode 100644 src/main/java/org/robolancers321/subsystems/launcher/Shooter.java delete mode 100644 src/main/java/org/robolancers321/util/InterpolationTable.java delete mode 100644 src/main/java/org/robolancers321/util/TunableSet.java diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 55b784c..f67f650 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -1,11 +1,17 @@ /* (C) Robolancers 2024 */ package org.robolancers321; +import org.robolancers321.subsystems.launcher.Launcher; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; public class RobotContainer { + Launcher launcher; + public RobotContainer() { + this.launcher = Launcher.getInstance(); + configureBindings(); } diff --git a/src/main/java/org/robolancers321/commands/launcher/YeetAmp.java b/src/main/java/org/robolancers321/commands/launcher/YeetAmp.java deleted file mode 100644 index e4db16b..0000000 --- a/src/main/java/org/robolancers321/commands/launcher/YeetAmp.java +++ /dev/null @@ -1,25 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.commands.launcher; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import org.robolancers321.subsystems.launcher.Indexer; -import org.robolancers321.subsystems.launcher.Pivot; -import org.robolancers321.subsystems.launcher.Shooter; - -public class YeetAmp extends SequentialCommandGroup { - - private Indexer indexer; - private Pivot pivot; - private Shooter shooter; - - public YeetAmp() { - this.indexer = Indexer.getInstance(); - this.pivot = Pivot.getInstance(); - this.shooter = Shooter.getInstance(); - - addRequirements(indexer, pivot, shooter); - addCommands( - pivot.positionAmp(), new ParallelCommandGroup(shooter.yeetNoteAmp(), indexer.index())); - } -} diff --git a/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java b/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java deleted file mode 100644 index c7cb53d..0000000 --- a/src/main/java/org/robolancers321/commands/launcher/YeetSpeaker.java +++ /dev/null @@ -1,30 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.commands.launcher; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import java.util.function.DoubleSupplier; -import org.robolancers321.subsystems.launcher.Indexer; -import org.robolancers321.subsystems.launcher.Pivot; -import org.robolancers321.subsystems.launcher.Shooter; - -public class YeetSpeaker extends ParallelCommandGroup { - private Indexer indexer; - private Pivot pivot; - private Shooter shooter; - - public YeetSpeaker(DoubleSupplier distanceFromSpeaker) { - this.indexer = Indexer.getInstance(); - this.pivot = Pivot.getInstance(); - this.shooter = Shooter.getInstance(); - - addRequirements(indexer, pivot, shooter); - // Run shooter and aim at the same time, index only after at setpoint - addCommands( - shooter.yeetNoteSpeaker(distanceFromSpeaker), - new SequentialCommandGroup( - pivot.aimAtSpeaker(distanceFromSpeaker), - indexer.index() // TODO: only run this when shooter is at speed - )); - } -} diff --git a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java new file mode 100644 index 0000000..21c5f86 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java @@ -0,0 +1,47 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.launcher; + +public class AimTable { + public static class AimCharacteristic { + public final double angle; + public final double rpm; + + public AimCharacteristic(double angle, double rpm) { + this.angle = angle; + this.rpm = rpm; + } + } + + private static class AimCharacteristicTableEntry { + public final double distance; + public final AimCharacteristic aimCharacteristic; + + public AimCharacteristicTableEntry(double distance, AimCharacteristic aimCharacteristic){ + this.distance = distance; + this.aimCharacteristic = aimCharacteristic; + } + } + + // TODO: tune + public static AimCharacteristic kRetractedAimCharacteristic = new AimCharacteristic(0.0, 0.0); + public static AimCharacteristic kMatingAimCharacteristic = new AimCharacteristic(0.0, 0.0); + public static AimCharacteristic kAmpAimCharacteristic = new AimCharacteristic(0.0, 0.0); + + // TODO: tune + private static AimCharacteristicTableEntry[] table = { + new AimCharacteristicTableEntry(0.0, new AimCharacteristic(0.0, 0.0)), + new AimCharacteristicTableEntry(1.0, new AimCharacteristic(0.0, 0.0)) + }; + + private static double interpolate(double lowKey, double lowValue, double highKey, double highValue, double x){ + double percent = (x - lowKey) / (highKey - lowKey); + + return lowKey + percent * (highValue - lowValue); + } + + public static AimCharacteristic getSpeakerAimCharacteristic(double distance){ + // TODO: write this correctly (not with the tree because it has edge cases at bounds) + + return new AimCharacteristic(0.0, 0.0); + } +} diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java new file mode 100644 index 0000000..8e3034b --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java @@ -0,0 +1,203 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.launcher; + +import static org.robolancers321.util.MathUtils.epsilonEquals; + +import com.revrobotics.*; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; + +public class Flywheel extends SubsystemBase { + private static Flywheel instance = null; + + public static Flywheel getInstance() { + if (instance == null) instance = new Flywheel(); + + return instance; + } + + /* Constants */ + + private static final int kTopMotorPort = 0; + private static final int kBottomMotorPort = 0; + private static final int kBeamBreakPort = 0; + + private static final int kCurrentLimit = 40; + + private static final boolean kInvertTopMotor = false; + private static final boolean kInvertBottomMotor = false; + + private static double kRampUpRate = 0.5; + + private static final double kFF = 0.0; + + private final double kErrorTolerance = 0.0; + private final double kInterpolationCacheThreshold = + 0.0; // the distance at which interpolation table recalculates setpoints + + /* + * Implementation + */ + + private CANSparkMax topMotor; + private CANSparkMax bottomMotor; + + private RelativeEncoder topEncoder; + private RelativeEncoder bottomEncoder; + + private SparkPIDController topController; + private SparkPIDController bottomController; + + private SlewRateLimiter topLimiter; + private SlewRateLimiter bottomLimiter; + + private DigitalInput beamBreak; + + private double goalRPM = 0.0; + + private Flywheel() { + this.topMotor = new CANSparkMax(kTopMotorPort, CANSparkLowLevel.MotorType.kBrushless); + this.bottomMotor = new CANSparkMax(kBottomMotorPort, CANSparkLowLevel.MotorType.kBrushless); + + this.topEncoder = this.topMotor.getEncoder(); + this.bottomEncoder = this.bottomMotor.getEncoder(); + + this.topController = this.topMotor.getPIDController(); + this.bottomController = this.bottomMotor.getPIDController(); + + this.topLimiter = new SlewRateLimiter(kRampUpRate); + this.bottomLimiter = new SlewRateLimiter(kRampUpRate); + + this.beamBreak = new DigitalInput(kBeamBreakPort); + + this.configureMotors(); + this.configureEncoders(); + this.configureControllers(); + } + + private void configureMotors() { + this.topMotor.setInverted(kInvertTopMotor); + this.topMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + this.topMotor.setSmartCurrentLimit(kCurrentLimit); + this.topMotor.enableVoltageCompensation(12); + this.topMotor.burnFlash(); + + this.bottomMotor.setInverted(kInvertBottomMotor); + this.bottomMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + this.bottomMotor.setSmartCurrentLimit(kCurrentLimit); + this.bottomMotor.enableVoltageCompensation(12); + this.bottomMotor.burnFlash(); + } + + private void configureEncoders() { + this.topEncoder.setVelocityConversionFactor(1.0); + this.bottomEncoder.setVelocityConversionFactor(1.0); + } + + private void configureControllers() { + this.topController.setP(0.0); + this.topController.setI(0.0); + this.topController.setD(0.0); + this.topController.setFF(kFF); + + this.bottomController.setP(0.0); + this.bottomController.setI(0.0); + this.bottomController.setD(0.0); + this.bottomController.setFF(kFF); + } + + private double getTopRPM() { + // TODO: filter here? + return this.topEncoder.getVelocity(); + } + + private double getBottomRPM() { + // TODO: filter here? + return this.bottomEncoder.getVelocity(); + } + + private void useControllers() { + this.topController.setReference( + this.topLimiter.calculate(this.goalRPM), CANSparkBase.ControlType.kVelocity); + this.bottomController.setReference( + this.bottomLimiter.calculate(this.goalRPM), CANSparkBase.ControlType.kVelocity); + } + + private boolean isTopRevved(){ + return epsilonEquals(this.getTopRPM(), this.goalRPM, kErrorTolerance); + } + + private boolean isBottomRevved(){ + return epsilonEquals(this.getBottomRPM(), this.goalRPM, kErrorTolerance); + } + + public boolean isRevved() { + return this.isTopRevved() && this.isBottomRevved(); + } + + private void dangerouslySetSpeed(double speed) { + this.topMotor.set(speed); + this.bottomMotor.set(speed); + } + + private void doSendables() { + SmartDashboard.putNumber("top flywheel rpm", this.getTopRPM()); + SmartDashboard.putNumber("bottom flywheel rpm", this.getBottomRPM()); + } + + @Override + public void periodic() { + this.doSendables(); + } + + private void initTuning() { + SmartDashboard.putNumber("shooter kff", SmartDashboard.getNumber("shooter kff", kFF)); + + SmartDashboard.putNumber("target shooter rpm", 0.0); + } + + private void tune() { + double tunedFF = SmartDashboard.getNumber("shooter kff", kFF); + + this.topController.setFF(tunedFF); + this.bottomController.setFF(tunedFF); + + this.goalRPM = SmartDashboard.getNumber("target shooter rpm", 0.0); + + this.useControllers(); + } + + public Command dangerouslyYeet(double speed){ + return run(() -> this.dangerouslySetSpeed(speed)).finallyDo(() -> this.dangerouslySetSpeed(0.0)); + } + + private Command off(){ + return runOnce(() -> { + this.goalRPM = 0.0; + this.useControllers(); + }); + } + + public Command yeetNoteAmp() { + this.goalRPM = AimTable.kAmpAimCharacteristic.rpm; + + return run(this::useControllers).finallyDo(this::off); + } + + public Command yeetNoteSpeaker(DoubleSupplier rpmSupplier) { + return run(() -> { + this.goalRPM = rpmSupplier.getAsDouble(); + this.useControllers(); + }).finallyDo(this::off); + } + + public Command tuneController(){ + this.initTuning(); + + return run(this::tune).finallyDo(this::off); + } +} diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index ef58679..f6712b6 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -5,10 +5,17 @@ import com.revrobotics.*; import com.revrobotics.CANSparkBase.ControlType; + import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; public class Indexer extends SubsystemBase { @@ -39,9 +46,9 @@ public static Indexer getInstance() { private static final double kD = 0; private static final double kFF = 0; - private static final double kMaxRPM = 5700; - private static final double kIntakeSpeedRPM = 500; - private static final double kOuttakeSpeedRPM = -500; + private static final double kHandoffRPM = 1500; + private static final double kReindexRPM = 500; + private static final double kOuttakeRPM = 5000; /* * Implementation @@ -83,7 +90,7 @@ private void configureController() { this.controller.setFF(kFF); } - public double getIntakeVelocityRPM() { + public double getRPM() { return this.encoder.getVelocity(); } @@ -91,43 +98,17 @@ public boolean jawnDetected() { return this.beamBreak.get(); } - public void dangerouslySetRPM(double rpm) { - this.motor.set(rpm / kMaxRPM); - } - - public void dangerouslySetSpeed(double speed) { + private void dangerouslySetSpeed(double speed) { this.motor.set(speed); } - public void intakeJawn() { - this.controller.setReference(kIntakeSpeedRPM, ControlType.kVelocity); - } - - public void outtakeJawn() { - this.controller.setReference(kOuttakeSpeedRPM, ControlType.kVelocity); - } - - public void stopSpinningJawn() { - this.controller.setReference(0.0, ControlType.kVelocity); - } - - public Command manualIndex(DoubleSupplier appliedSpeedSupplier) { - return run(() -> dangerouslySetSpeed(appliedSpeedSupplier.getAsDouble())); - } - - public Command manualIndex(double appliedSpeed) { - return this.manualIndex(() -> appliedSpeed); - } - - public Command index() { - // TODO: until beam break goes from true to false, also maybe add a time delay - // TODO: should this beam break be the same as shooter? - return run(this::intakeJawn).until(this::jawnDetected).finallyDo(this::stopSpinningJawn); + private void setRPM(double rpm){ + this.controller.setReference(rpm, ControlType.kVelocity); } private void doSendables() { - SmartDashboard.putNumber("Indexer Velocity (rpm)", this.getIntakeVelocityRPM()); - SmartDashboard.putBoolean("Note Detected", this.jawnDetected()); + SmartDashboard.putNumber("indexer rpm", this.getRPM()); + SmartDashboard.putBoolean("note detected", this.jawnDetected()); } @Override @@ -135,14 +116,14 @@ public void periodic() { this.doSendables(); } - public void initTuning() { + private void initTuning() { SmartDashboard.putNumber("indexer kp", SmartDashboard.getNumber("indexer kp", kP)); SmartDashboard.putNumber("indexer kp", SmartDashboard.getNumber("indexer ki", kI)); SmartDashboard.putNumber("indexer kp", SmartDashboard.getNumber("indexer kd", kD)); SmartDashboard.putNumber("indexer kff", SmartDashboard.getNumber("indexer kff", kFF)); } - public void tune() { + private void tune() { double tunedP = SmartDashboard.getNumber("indexer kp", kP); double tunedI = SmartDashboard.getNumber("indexer ki", kI); double tunedD = SmartDashboard.getNumber("indexer kd", kD); @@ -153,4 +134,54 @@ public void tune() { this.controller.setD(tunedD); this.controller.setFF(tunedFF); } + + public Command manualIndex(DoubleSupplier appliedSpeedSupplier) { + return run(() -> dangerouslySetSpeed(appliedSpeedSupplier.getAsDouble())); + } + + public Command manualIndex(double appliedSpeed) { + return this.manualIndex(() -> appliedSpeed); + } + + private Command off(){ + return runOnce(() -> this.setRPM(0.0)); + } + + public Command acceptHandoff() { + return run(() -> this.setRPM(kHandoffRPM)).until(this::jawnDetected).finallyDo(this::off); + } + + public Command reindex(BooleanSupplier beamBreakStateSupplier){ + /* + * TODO + * + * sequential + * while beam is broken, setRPM(-kReindexRPM) + * while beam is not broken, setRPM(kReindexRPM) + * finally do set rpm to 0 + * + * + * this should also have a timeout for safety + * + */ + + return null; + } + + public Command outtake(BooleanSupplier beamBreakStateSupplier) { + return new ParallelRaceGroup( + run(() -> this.setRPM(kOuttakeRPM)), + new SequentialCommandGroup( + new WaitUntilCommand(() -> beamBreakStateSupplier.getAsBoolean()), + new WaitUntilCommand(() -> !beamBreakStateSupplier.getAsBoolean()) + ), + new WaitCommand(0.4) // TODO: just incase beam break fails, stop after some safe amount of time + ).finallyDo(this::off); + } + + public Command tuneController(){ + this.initTuning(); + + return run(this::tune); + } } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java new file mode 100644 index 0000000..f5c3d3c --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java @@ -0,0 +1,76 @@ +package org.robolancers321.subsystems.launcher; + +import org.robolancers321.subsystems.launcher.AimTable.AimCharacteristic; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class Launcher extends SubsystemBase { + /* + * Singleton + */ + + private static Launcher instance = null; + + public static Launcher getInstance(){ + if (instance == null) instance = new Launcher(); + + return instance; + } + + /* + * Constants + */ + + // TODO: beam break(s) + + /* + * Implementation + */ + + public Pivot pivot; + public Indexer indexer; + public Flywheel flywheel; + + // TODO: beam break(s) + + private Launcher(){ + this.pivot = Pivot.getInstance(); + this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + } + + public Command yeetAmp(){ + return new SequentialCommandGroup( + pivot.aimAtAmp(), + new ParallelCommandGroup( + flywheel.yeetNoteAmp(), + indexer.outtake(() -> true), // TODO: pass in beam break state + new WaitCommand(0.2) + ) + ); + } + + // TODO: how to go about ths for active tracking + public Command yeetSpeaker(double distance){ + AimCharacteristic aimCharacteristic = AimTable.getSpeakerAimCharacteristic(distance); + + return new SequentialCommandGroup( + indexer.reindex(() -> true), // TODO: pass in beam break state + new ParallelCommandGroup( + flywheel.yeetNoteSpeaker(() -> aimCharacteristic.rpm), + new SequentialCommandGroup( + pivot.aimAtSpeaker(() -> aimCharacteristic.angle), + new WaitUntilCommand(flywheel::isRevved), // TODO: maybe put a timeout here to be safe + indexer.outtake(() -> true), // TODO: pass in beam break state + new WaitCommand(0.2) + ) + ) + ); + } +} diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index fb98e58..812d819 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -2,7 +2,6 @@ package org.robolancers321.subsystems.launcher; import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; -import static org.robolancers321.util.MathUtils.epsilonEquals; import com.revrobotics.*; import com.revrobotics.SparkAbsoluteEncoder.Type; @@ -13,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; import java.util.function.DoubleSupplier; -import org.robolancers321.util.InterpolationTable; public class Pivot extends ProfiledPIDSubsystem { /* @@ -56,26 +54,16 @@ public static Pivot getInstance() { private static final double kToleranceDeg = 0.0; - private static final double kInterpolationThreshold = 0.0; - - private double latestDistance = 0.0; - private InterpolationTable.AimCharacteristic latestCharacteristic; - public enum PivotSetpoint { kRetracted(0.0), kMating(0.0), - kSpeaker(0.0), kAmp(0.0); - private final double angle; + public final double angle; PivotSetpoint(double angle) { this.angle = angle; } - - public double getAngle() { - return this.angle; - } } /* @@ -118,60 +106,27 @@ private void configureEncoder() { this.absoluteEncoder.setVelocityConversionFactor(kRotPerMinToDegPerSec); } - @Override - public double getMeasurement() { - return getVelocityRad(); - } - private void configureController() { super.m_controller.setTolerance(kToleranceDeg); } - public double getPositionDeg() { - return this.absoluteEncoder.getPosition(); + @Override + public double getMeasurement() { + return this.getPositionDeg(); } - public double getPositionRad() { - return Math.toRadians(this.getPositionDeg()); + public double getPositionDeg() { + return this.absoluteEncoder.getPosition(); } public double getVelocityDeg() { return this.absoluteEncoder.getVelocity(); } - public double getVelocityRad() { - return Math.toRadians(this.getVelocityDeg()); - } - private boolean atGoal() { return super.m_controller.atGoal(); } - public void setAngleGoal(double goal) { - super.setGoal(new TrapezoidProfile.State(goal, 0.0)); - } - - public void setAngleGoal(PivotSetpoint goal) { - super.setGoal(goal.getAngle()); - } - - public void setAngleGoal(DoubleSupplier distance) { - if (!epsilonEquals(distance.getAsDouble(), latestDistance, kInterpolationThreshold)) { - latestCharacteristic = InterpolationTable.interpolate(distance.getAsDouble()); - latestDistance = distance.getAsDouble(); - } - - setAngleGoal(latestCharacteristic.getPitchAngle()); - } - - public Command aimAtSpeaker(DoubleSupplier distance) { - return run(() -> setAngleGoal(distance)).until(this::atGoal); - } - - public Command positionAmp() { - return run(() -> setAngleGoal(PivotSetpoint.kAmp)).until(this::atGoal); - } - @Override protected void useOutput(double output, TrapezoidProfile.State setpoint) { double feedforwardOutput = @@ -181,7 +136,7 @@ protected void useOutput(double output, TrapezoidProfile.State setpoint) { double controllerOutput = feedforwardOutput + feedbackOutput; - this.motor.setVoltage(controllerOutput); + this.motor.set(controllerOutput); } public void doSendables() { @@ -191,36 +146,64 @@ public void doSendables() { @Override public void periodic() { - this.doSendables(); } - public void initTuning() { + private void initTuning() { + SmartDashboard.putNumber("pivot kp", SmartDashboard.getNumber("pivot kp", kP)); + SmartDashboard.putNumber("pivot ki", SmartDashboard.getNumber("pivot ki", kI)); + SmartDashboard.putNumber("pivot kd", SmartDashboard.getNumber("pivot kd", kD)); + SmartDashboard.putNumber("pivot ks", SmartDashboard.getNumber("pivot ks", kS)); SmartDashboard.putNumber("pivot kg", SmartDashboard.getNumber("pivot kg", kG)); SmartDashboard.putNumber("pivot kv", SmartDashboard.getNumber("pivot kv", kV)); - SmartDashboard.putNumber("pivot kp", SmartDashboard.getNumber("pivot kp", kP)); - SmartDashboard.putNumber("pivot ki", SmartDashboard.getNumber("pivot ki", kI)); - SmartDashboard.putNumber("pivot kd", SmartDashboard.getNumber("pivot kd", kD)); + SmartDashboard.putNumber("target pivot position", this.getPositionDeg()); } - public void tune() { + private void tune() { + double tunedP = SmartDashboard.getNumber("pivot kp", kP); + double tunedI = SmartDashboard.getNumber("pivot ki", kI); + double tunedD = SmartDashboard.getNumber("pivot kd", kD); + + super.m_controller.setPID(tunedP, tunedI, tunedD); + double tunedS = SmartDashboard.getNumber("pivot ks", kS); double tunedG = SmartDashboard.getNumber("pivot kg", kG); double tunedV = SmartDashboard.getNumber("pivot kv", kV); + + this.feedforwardController = new ArmFeedforward(tunedS, tunedG, tunedV); - // TODO: is there a better way to update this? - if (tunedS != this.feedforwardController.ks - || tunedG != this.feedforwardController.kg - || tunedV != this.feedforwardController.kv) { - this.feedforwardController = new ArmFeedforward(tunedS, tunedG, tunedV); - } + this.setGoal(SmartDashboard.getNumber("target pivot position", this.getPositionDeg())); + } - double tunedP = SmartDashboard.getNumber("pivot kp", kP); - double tunedI = SmartDashboard.getNumber("pivot ki", kI); - double tunedD = SmartDashboard.getNumber("pivot kd", kD); + private Command moveToAngle(DoubleSupplier angleSupplier){ + return run(() -> this.setGoal(angleSupplier.getAsDouble())).until(this::atGoal); + } - super.m_controller.setPID(tunedP, tunedI, tunedD); + private Command moveToAngle(double angle){ + return this.moveToAngle(() -> angle); + } + + public Command goToRetracted(){ + return this.moveToAngle(AimTable.kRetractedAimCharacteristic.angle); + } + + public Command goToMating(){ + return this.moveToAngle(AimTable.kMatingAimCharacteristic.angle); + } + + public Command aimAtAmp(){ + return this.moveToAngle(AimTable.kAmpAimCharacteristic.angle); + } + + public Command aimAtSpeaker(DoubleSupplier angleSupplier){ + return this.moveToAngle(angleSupplier); + } + + public Command tuneControllers(){ + this.initTuning(); + + return run(this::tune); } } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java b/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java deleted file mode 100644 index 7cf514c..0000000 --- a/src/main/java/org/robolancers321/subsystems/launcher/Shooter.java +++ /dev/null @@ -1,199 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.subsystems.launcher; - -import static org.robolancers321.util.MathUtils.epsilonEquals; - -import com.revrobotics.*; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.function.DoubleSupplier; -import org.robolancers321.util.InterpolationTable; - -public class Shooter extends SubsystemBase { - private static Shooter instance = null; - - public static Shooter getInstance() { - if (instance == null) instance = new Shooter(); - - return instance; - } - - /* Constants */ - - private static final int kLeftMotorID = 0; - private static final int kRightMotorID = 0; - private static final int kBeamBreakPort = 0; - - private static final int kCurrentLimit = 40; - - private static final boolean kInvertLeftMotor = false; - private static final boolean kInvertRightMotor = false; - - // TODO: store this better? - private static final double kAmpLeftSpeed = 0.0; - private static final double kRightAmpSpeed = 0.0; - - private static double kRampUpRate = 0.5; - - private static final double kFF = 0.0; - - private final double kErrorTolerance = 0.0; - private final double kInterpolationCacheThreshold = - 0.0; // the distance at which interpolation table recalculates setpoints - - /* - * Implementation - */ - - private CANSparkMax leftMotor; - private CANSparkMax rightMotor; - - private RelativeEncoder leftEncoder; - private RelativeEncoder rightEncoder; - - private SparkPIDController leftController; - private SparkPIDController rightController; - - private SlewRateLimiter leftLimiter; - private SlewRateLimiter rightLimiter; - - private DigitalInput beamBreak; - - private double latestDistance = 0.0; - private InterpolationTable.AimCharacteristic latestCharacteristic = null; - - private double leftSetpoint = 0.0; - private double rightSetpoint = 0.0; - - private Shooter() { - this.leftMotor = new CANSparkMax(kLeftMotorID, CANSparkLowLevel.MotorType.kBrushless); - this.rightMotor = new CANSparkMax(kRightMotorID, CANSparkLowLevel.MotorType.kBrushless); - - this.leftEncoder = this.leftMotor.getEncoder(); - this.rightEncoder = this.rightMotor.getEncoder(); - - this.leftController = this.leftMotor.getPIDController(); - this.rightController = this.rightMotor.getPIDController(); - - this.leftLimiter = new SlewRateLimiter(kRampUpRate); - this.rightLimiter = new SlewRateLimiter(kRampUpRate); - - this.beamBreak = new DigitalInput(kBeamBreakPort); - - this.configureMotors(); - this.configureControllers(); - this.configureEncoders(); - } - - private void configureControllers() { - this.leftController.setFF(kFF); - this.rightController.setFF(kFF); - } - - private void configureMotors() { - this.leftMotor.setInverted(kInvertLeftMotor); - this.leftMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); - this.leftMotor.setSmartCurrentLimit(kCurrentLimit); - this.leftMotor.enableVoltageCompensation(12); - this.leftMotor.burnFlash(); - - this.rightMotor.setInverted(kInvertRightMotor); - this.rightMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); - this.rightMotor.setSmartCurrentLimit(kCurrentLimit); - this.rightMotor.enableVoltageCompensation(12); - this.rightMotor.burnFlash(); - } - - private void configureEncoders() { - this.leftEncoder.setInverted(kInvertLeftMotor); - this.rightEncoder.setInverted(kInvertRightMotor); - } - - private double getLeftVelocity() { - // TODO: filter here? - return this.leftEncoder.getVelocity(); - } - - private double getRightVelocity() { - // TODO: filter here? - return this.rightEncoder.getVelocity(); - } - - private void useControllers(double leftSpeed, double rightSpeed) { - this.leftController.setReference( - this.leftLimiter.calculate(leftSpeed), CANSparkBase.ControlType.kVelocity); - this.rightController.setReference( - this.rightLimiter.calculate(rightSpeed), CANSparkBase.ControlType.kVelocity); - } - - public void yeetNoteSpeaker(double distance) { - if (!epsilonEquals(distance, latestDistance, kInterpolationCacheThreshold)) { - latestDistance = distance; - latestCharacteristic = InterpolationTable.interpolate(distance); - } - - this.leftSetpoint = latestCharacteristic.getLeftSpeed(); - this.rightSetpoint = latestCharacteristic.getRightSpeed(); - - this.useControllers(this.leftSetpoint, this.rightSetpoint); - } - - public Command yeetNoteSpeaker(DoubleSupplier distance) { - // TODO: until beam break goes from true to false, also maybe add a time delay - return run(() -> this.yeetNoteSpeaker(distance.getAsDouble())).until(beamBreak::get); - } - - public boolean atLeftSetpoint() { - return epsilonEquals(this.getLeftVelocity(), this.leftSetpoint, kErrorTolerance); - } - - public boolean atRightSetpoint() { - return epsilonEquals(this.getRightVelocity(), this.rightSetpoint, kErrorTolerance); - } - - public boolean atSetpoint() { - return this.atLeftSetpoint() && this.atLeftSetpoint(); - } - - public void dangerouslyYeet(double leftSpeed, double rightSpeed) { - this.leftMotor.set(leftSpeed); - this.rightMotor.set(rightSpeed); - } - - public void yeetNoteAmp(double distance) { - this.leftSetpoint = kAmpLeftSpeed; - this.rightSetpoint = kRightAmpSpeed; - - this.useControllers(this.leftSetpoint, this.rightSetpoint); - } - - public Command yeetNoteAmp() { - // TODO: is this the right end condition? - return run(this::yeetNoteAmp).until(beamBreak::get); - } - - private void doSendables() { - - SmartDashboard.putNumber("Left flywheel velocity", getLeftVelocity()); - SmartDashboard.putNumber("Right flywheel velocity", getRightVelocity()); - } - - @Override - public void periodic() { - this.doSendables(); - } - - public void initTuning() { - SmartDashboard.putNumber("shooter kff", SmartDashboard.getNumber("shooter kff", kFF)); - } - - public void tune() { - double tunedFF = SmartDashboard.getNumber("shooter kff", kFF); - - leftController.setFF(tunedFF); - rightController.setFF(tunedFF); - } -} diff --git a/src/main/java/org/robolancers321/util/InterpolationTable.java b/src/main/java/org/robolancers321/util/InterpolationTable.java deleted file mode 100644 index a8bd131..0000000 --- a/src/main/java/org/robolancers321/util/InterpolationTable.java +++ /dev/null @@ -1,86 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.util; - -import java.util.LinkedHashMap; -import java.util.List; -import java.util.NavigableSet; -import java.util.TreeSet; - -public class InterpolationTable { - public static final class AimCharacteristic { - private final double leftSpeed; - private final double rightSpeed; - private final double pitchAngle; - - public AimCharacteristic(double leftSpeed, double rightSpeed, double pitchAngle) { - this.leftSpeed = leftSpeed; - this.rightSpeed = rightSpeed; - this.pitchAngle = pitchAngle; - } - - public double getLeftSpeed() { - return leftSpeed; - } - - public double getRightSpeed() { - return rightSpeed; - } - - public double getPitchAngle() { - return pitchAngle; - } - } - - public static LinkedHashMap table = - new LinkedHashMap<>() { - { - put(1.0, new AimCharacteristic(1000, 1000, 10)); - put(2.0, new AimCharacteristic(2000, 2000, 20)); - } - }; - - public static AimCharacteristic interpolate(double independent) { - List keys = table.keySet().stream().toList(); - double lowerBound = keys.get(0); - double upperBound = keys.get(keys.size() - 1); - - if ((independent < lowerBound)) independent = lowerBound; - else if (independent > upperBound) independent = upperBound; - - NavigableSet values = new TreeSet<>(keys); - - double lowerKey = values.floor(independent); - double upperKey = lowerKey + 1; - - AimCharacteristic lowerValue = table.get(lowerKey); - AimCharacteristic upperValue = table.get(upperKey); - - double leftSpeed = - calculateCharacteristic( - lowerKey, upperKey, lowerValue.getLeftSpeed(), upperValue.getLeftSpeed(), independent); - - double rightSpeed = - calculateCharacteristic( - lowerKey, - upperKey, - lowerValue.getRightSpeed(), - upperValue.getRightSpeed(), - independent); - - double pitchAngle = - calculateCharacteristic( - lowerKey, - upperKey, - lowerValue.getPitchAngle(), - upperValue.getPitchAngle(), - independent); - - return new AimCharacteristic(leftSpeed, rightSpeed, pitchAngle); - } - - private static double calculateCharacteristic( - double lowerKey, double upperKey, double lower, double upper, double independent) { - return ((lower * (upperKey - independent) + upper * (independent - lowerKey)) / upperKey - - lowerKey); - } -} diff --git a/src/main/java/org/robolancers321/util/MathUtils.java b/src/main/java/org/robolancers321/util/MathUtils.java index 08ebe60..921dd1b 100644 --- a/src/main/java/org/robolancers321/util/MathUtils.java +++ b/src/main/java/org/robolancers321/util/MathUtils.java @@ -4,6 +4,6 @@ public class MathUtils { public static boolean epsilonEquals(double a, double b, double epsilon) { - return Math.abs(b - a) == epsilon; + return Math.abs(b - a) < epsilon; } } diff --git a/src/main/java/org/robolancers321/util/TunableSet.java b/src/main/java/org/robolancers321/util/TunableSet.java deleted file mode 100644 index 2181211..0000000 --- a/src/main/java/org/robolancers321/util/TunableSet.java +++ /dev/null @@ -1,63 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.util; - -import static edu.wpi.first.wpilibj.smartdashboard.SmartDashboard.*; - -public class TunableSet { - public static class Tunable { - public static int tune(String key, int defaultValue) { - var entry = getEntry(key); - if (!entry.exists()) putNumber(key, defaultValue); - return (int) entry.getInteger(defaultValue); - } - - public static double tune(String key, double defaultValue) { - var entry = getEntry(key); - if (!entry.exists()) putNumber(key, defaultValue); - return entry.getDouble(defaultValue); - } - - public static String tune(String key, String defaultValue) { - var entry = getEntry(key); - if (!entry.exists()) putString(key, defaultValue); - return entry.getString(defaultValue); - } - - public static boolean tune(String key, boolean defaultValue) { - var entry = getEntry(key); - if (!entry.exists()) putBoolean(key, defaultValue); - return entry.getBoolean(defaultValue); - } - } - - private String prefix; - - public TunableSet(String prefix) { - this.prefix = prefix; - } - - // !TODO Add prefixes - public int tune(String key, int defaultValue) { - var entry = getEntry(prefix + " " + key); - if (!entry.exists()) putNumber(key, defaultValue); - return (int) entry.getInteger(defaultValue); - } - - public double tune(String key, double defaultValue) { - var entry = getEntry(prefix + " " + key); - if (!entry.exists()) putNumber(key, defaultValue); - return entry.getDouble(defaultValue); - } - - public String tune(String key, String defaultValue) { - var entry = getEntry(prefix + " " + key); - if (!entry.exists()) putString(key, defaultValue); - return entry.getString(defaultValue); - } - - public boolean tune(String key, boolean defaultValue) { - var entry = getEntry(prefix + " " + key); - if (!entry.exists()) putBoolean(key, defaultValue); - return entry.getBoolean(defaultValue); - } -} From 915f6daf6c82ff96960607d3e3d243cd85e281a0 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Mon, 29 Jan 2024 15:37:22 -0500 Subject: [PATCH 45/57] slight changes, todos to be done --- .../org/robolancers321/BuildConstants.java | 12 +-- .../org/robolancers321/RobotContainer.java | 3 +- .../subsystems/launcher/AimTable.java | 9 +- .../subsystems/launcher/Flywheel.java | 37 +++---- .../subsystems/launcher/Indexer.java | 43 ++++---- .../subsystems/launcher/Launcher.java | 102 +++++++++--------- .../subsystems/launcher/Pivot.java | 16 +-- 7 files changed, 106 insertions(+), 116 deletions(-) diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index b1985e0..c0cec9e 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 22; - public static final String GIT_SHA = "e1e6570ec057cc69b9d7b44bfe257a3f5ac03812"; - public static final String GIT_DATE = "2024-01-25 08:03:07 EST"; + public static final int GIT_REVISION = 26; + public static final String GIT_SHA = "98ec876acf210c51a7146e70cdaf9765448c192a"; + public static final String GIT_DATE = "2024-01-29 15:02:18 EST"; public static final String GIT_BRANCH = "dev-launcher-4"; - public static final String BUILD_DATE = "2024-01-26 08:51:31 EST"; - public static final long BUILD_UNIX_TIME = 1706277091728L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2024-01-29 15:36:21 EST"; + public static final long BUILD_UNIX_TIME = 1706560581324L; + public static final int DIRTY = 0; private BuildConstants(){} } diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index f67f650..38d5105 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -1,10 +1,9 @@ /* (C) Robolancers 2024 */ package org.robolancers321; -import org.robolancers321.subsystems.launcher.Launcher; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import org.robolancers321.subsystems.launcher.Launcher; public class RobotContainer { Launcher launcher; diff --git a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java index 21c5f86..ac9734f 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java @@ -16,7 +16,7 @@ private static class AimCharacteristicTableEntry { public final double distance; public final AimCharacteristic aimCharacteristic; - public AimCharacteristicTableEntry(double distance, AimCharacteristic aimCharacteristic){ + public AimCharacteristicTableEntry(double distance, AimCharacteristic aimCharacteristic) { this.distance = distance; this.aimCharacteristic = aimCharacteristic; } @@ -33,13 +33,14 @@ public AimCharacteristicTableEntry(double distance, AimCharacteristic aimCharact new AimCharacteristicTableEntry(1.0, new AimCharacteristic(0.0, 0.0)) }; - private static double interpolate(double lowKey, double lowValue, double highKey, double highValue, double x){ + private static double interpolate( + double lowKey, double lowValue, double highKey, double highValue, double x) { double percent = (x - lowKey) / (highKey - lowKey); return lowKey + percent * (highValue - lowValue); } - - public static AimCharacteristic getSpeakerAimCharacteristic(double distance){ + + public static AimCharacteristic getSpeakerAimCharacteristic(double distance) { // TODO: write this correctly (not with the tree because it has edge cases at bounds) return new AimCharacteristic(0.0, 0.0); diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java index 8e3034b..078df9b 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java @@ -5,7 +5,6 @@ import com.revrobotics.*; import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -24,7 +23,6 @@ public static Flywheel getInstance() { private static final int kTopMotorPort = 0; private static final int kBottomMotorPort = 0; - private static final int kBeamBreakPort = 0; private static final int kCurrentLimit = 40; @@ -36,8 +34,6 @@ public static Flywheel getInstance() { private static final double kFF = 0.0; private final double kErrorTolerance = 0.0; - private final double kInterpolationCacheThreshold = - 0.0; // the distance at which interpolation table recalculates setpoints /* * Implementation @@ -55,8 +51,6 @@ public static Flywheel getInstance() { private SlewRateLimiter topLimiter; private SlewRateLimiter bottomLimiter; - private DigitalInput beamBreak; - private double goalRPM = 0.0; private Flywheel() { @@ -72,8 +66,6 @@ private Flywheel() { this.topLimiter = new SlewRateLimiter(kRampUpRate); this.bottomLimiter = new SlewRateLimiter(kRampUpRate); - this.beamBreak = new DigitalInput(kBeamBreakPort); - this.configureMotors(); this.configureEncoders(); this.configureControllers(); @@ -127,11 +119,11 @@ private void useControllers() { this.bottomLimiter.calculate(this.goalRPM), CANSparkBase.ControlType.kVelocity); } - private boolean isTopRevved(){ + private boolean isTopRevved() { return epsilonEquals(this.getTopRPM(), this.goalRPM, kErrorTolerance); } - private boolean isBottomRevved(){ + private boolean isBottomRevved() { return epsilonEquals(this.getBottomRPM(), this.goalRPM, kErrorTolerance); } @@ -171,15 +163,17 @@ private void tune() { this.useControllers(); } - public Command dangerouslyYeet(double speed){ - return run(() -> this.dangerouslySetSpeed(speed)).finallyDo(() -> this.dangerouslySetSpeed(0.0)); + public Command dangerouslyYeet(double speed) { + return run(() -> this.dangerouslySetSpeed(speed)) + .finallyDo(() -> this.dangerouslySetSpeed(0.0)); } - private Command off(){ - return runOnce(() -> { - this.goalRPM = 0.0; - this.useControllers(); - }); + private Command off() { + return runOnce( + () -> { + this.goalRPM = 0.0; + this.useControllers(); + }); } public Command yeetNoteAmp() { @@ -190,12 +184,13 @@ public Command yeetNoteAmp() { public Command yeetNoteSpeaker(DoubleSupplier rpmSupplier) { return run(() -> { - this.goalRPM = rpmSupplier.getAsDouble(); - this.useControllers(); - }).finallyDo(this::off); + this.goalRPM = rpmSupplier.getAsDouble(); + this.useControllers(); + }) + .finallyDo(this::off); } - public Command tuneController(){ + public Command tuneController() { this.initTuning(); return run(this::tune).finallyDo(this::off); diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index f6712b6..ba3cc22 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -5,7 +5,6 @@ import com.revrobotics.*; import com.revrobotics.CANSparkBase.ControlType; - import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -14,7 +13,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -102,7 +100,7 @@ private void dangerouslySetSpeed(double speed) { this.motor.set(speed); } - private void setRPM(double rpm){ + private void setRPM(double rpm) { this.controller.setReference(rpm, ControlType.kVelocity); } @@ -143,26 +141,26 @@ public Command manualIndex(double appliedSpeed) { return this.manualIndex(() -> appliedSpeed); } - private Command off(){ + private Command off() { return runOnce(() -> this.setRPM(0.0)); } - public Command acceptHandoff() { - return run(() -> this.setRPM(kHandoffRPM)).until(this::jawnDetected).finallyDo(this::off); + public Command acceptHandoff(BooleanSupplier beamBreakStateSupplier) { + return run(() -> this.setRPM(kHandoffRPM)).until(beamBreakStateSupplier).finallyDo(this::off); } - public Command reindex(BooleanSupplier beamBreakStateSupplier){ + public Command reindex(BooleanSupplier beamBreakStateSupplier) { /* * TODO - * + * * sequential - * while beam is broken, setRPM(-kReindexRPM) - * while beam is not broken, setRPM(kReindexRPM) + * while beam is broken, setRPM(kReindexRPM) + * while beam is not broken, setRPM(-kReindexRPM) * finally do set rpm to 0 - * - * + * + * * this should also have a timeout for safety - * + * */ return null; @@ -170,18 +168,19 @@ public Command reindex(BooleanSupplier beamBreakStateSupplier){ public Command outtake(BooleanSupplier beamBreakStateSupplier) { return new ParallelRaceGroup( - run(() -> this.setRPM(kOuttakeRPM)), - new SequentialCommandGroup( - new WaitUntilCommand(() -> beamBreakStateSupplier.getAsBoolean()), - new WaitUntilCommand(() -> !beamBreakStateSupplier.getAsBoolean()) - ), - new WaitCommand(0.4) // TODO: just incase beam break fails, stop after some safe amount of time - ).finallyDo(this::off); + run(() -> this.setRPM(kOuttakeRPM)), + new SequentialCommandGroup( + new WaitUntilCommand(() -> beamBreakStateSupplier.getAsBoolean()), + new WaitUntilCommand(() -> !beamBreakStateSupplier.getAsBoolean())), + new WaitCommand( + 0.4) // TODO: just incase beam break fails, stop after some safe amount of time + ) + .finallyDo(this::off); } - public Command tuneController(){ + public Command tuneController() { this.initTuning(); - + return run(this::tune); } } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java index f5c3d3c..e586e2d 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java @@ -1,7 +1,6 @@ +/* (C) Robolancers 2024 */ package org.robolancers321.subsystems.launcher; -import org.robolancers321.subsystems.launcher.AimTable.AimCharacteristic; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; @@ -9,68 +8,65 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import org.robolancers321.subsystems.launcher.AimTable.AimCharacteristic; public class Launcher extends SubsystemBase { - /* - * Singleton - */ + /* + * Singleton + */ - private static Launcher instance = null; + private static Launcher instance = null; - public static Launcher getInstance(){ - if (instance == null) instance = new Launcher(); + public static Launcher getInstance() { + if (instance == null) instance = new Launcher(); - return instance; - } + return instance; + } - /* - * Constants - */ + /* + * Constants + */ - // TODO: beam break(s) + // TODO: beam break(s) - /* - * Implementation - */ + /* + * Implementation + */ - public Pivot pivot; - public Indexer indexer; - public Flywheel flywheel; + public Pivot pivot; + public Indexer indexer; + public Flywheel flywheel; - // TODO: beam break(s) + // TODO: beam break(s) - private Launcher(){ - this.pivot = Pivot.getInstance(); - this.indexer = Indexer.getInstance(); - this.flywheel = Flywheel.getInstance(); - } - - public Command yeetAmp(){ - return new SequentialCommandGroup( - pivot.aimAtAmp(), - new ParallelCommandGroup( - flywheel.yeetNoteAmp(), - indexer.outtake(() -> true), // TODO: pass in beam break state - new WaitCommand(0.2) - ) - ); - } + private Launcher() { + this.pivot = Pivot.getInstance(); + this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + } + + public Command yeetAmp() { + return new SequentialCommandGroup( + pivot.aimAtAmp(), + new ParallelCommandGroup( + flywheel.yeetNoteAmp(), + indexer.outtake(() -> true), // TODO: pass in beam break state + new WaitCommand(0.2))); + } - // TODO: how to go about ths for active tracking - public Command yeetSpeaker(double distance){ - AimCharacteristic aimCharacteristic = AimTable.getSpeakerAimCharacteristic(distance); + // TODO: how to go about ths for active tracking + public Command yeetSpeaker(double distance) { + AimCharacteristic aimCharacteristic = AimTable.getSpeakerAimCharacteristic(distance); - return new SequentialCommandGroup( - indexer.reindex(() -> true), // TODO: pass in beam break state - new ParallelCommandGroup( - flywheel.yeetNoteSpeaker(() -> aimCharacteristic.rpm), - new SequentialCommandGroup( - pivot.aimAtSpeaker(() -> aimCharacteristic.angle), - new WaitUntilCommand(flywheel::isRevved), // TODO: maybe put a timeout here to be safe - indexer.outtake(() -> true), // TODO: pass in beam break state - new WaitCommand(0.2) - ) - ) - ); - } + return new SequentialCommandGroup( + indexer.reindex(() -> true), // TODO: pass in beam break state + new ParallelRaceGroup( + flywheel.yeetNoteSpeaker(() -> aimCharacteristic.rpm), + new SequentialCommandGroup( + pivot.aimAtSpeaker(() -> aimCharacteristic.angle), + new WaitUntilCommand( + flywheel::isRevved), // TODO: maybe put a timeout here to be safe + indexer.outtake(() -> true), // TODO: pass in beam break state + new WaitCommand(0.2)))); + } } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 812d819..4e425a5 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -171,37 +171,37 @@ private void tune() { double tunedS = SmartDashboard.getNumber("pivot ks", kS); double tunedG = SmartDashboard.getNumber("pivot kg", kG); double tunedV = SmartDashboard.getNumber("pivot kv", kV); - + this.feedforwardController = new ArmFeedforward(tunedS, tunedG, tunedV); this.setGoal(SmartDashboard.getNumber("target pivot position", this.getPositionDeg())); } - private Command moveToAngle(DoubleSupplier angleSupplier){ + private Command moveToAngle(DoubleSupplier angleSupplier) { return run(() -> this.setGoal(angleSupplier.getAsDouble())).until(this::atGoal); } - private Command moveToAngle(double angle){ + private Command moveToAngle(double angle) { return this.moveToAngle(() -> angle); } - public Command goToRetracted(){ + public Command goToRetracted() { return this.moveToAngle(AimTable.kRetractedAimCharacteristic.angle); } - public Command goToMating(){ + public Command goToMating() { return this.moveToAngle(AimTable.kMatingAimCharacteristic.angle); } - public Command aimAtAmp(){ + public Command aimAtAmp() { return this.moveToAngle(AimTable.kAmpAimCharacteristic.angle); } - public Command aimAtSpeaker(DoubleSupplier angleSupplier){ + public Command aimAtSpeaker(DoubleSupplier angleSupplier) { return this.moveToAngle(angleSupplier); } - public Command tuneControllers(){ + public Command tuneControllers() { this.initTuning(); return run(this::tune); From 641002eff0c4b11e23d3c58822c03d0281e210bd Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Mon, 29 Jan 2024 19:59:19 -0500 Subject: [PATCH 46/57] feat(launcher): Added AimTable, refactored some code, and added indexer cocking --- .../subsystems/launcher/AimTable.java | 54 ++++++++++++++----- .../subsystems/launcher/Indexer.java | 17 ++++-- .../subsystems/launcher/Launcher.java | 33 +++++++++--- 3 files changed, 78 insertions(+), 26 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java index ac9734f..85b263a 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java @@ -1,6 +1,13 @@ /* (C) Robolancers 2024 */ package org.robolancers321.subsystems.launcher; +import static org.robolancers321.util.MathUtils.epsilonEquals; + +import java.util.LinkedHashMap; +import java.util.List; +import java.util.NavigableSet; +import java.util.TreeSet; + public class AimTable { public static class AimCharacteristic { public final double angle; @@ -12,15 +19,11 @@ public AimCharacteristic(double angle, double rpm) { } } - private static class AimCharacteristicTableEntry { - public final double distance; - public final AimCharacteristic aimCharacteristic; + private AimCharacteristic lastAimCharacteristic; - public AimCharacteristicTableEntry(double distance, AimCharacteristic aimCharacteristic) { - this.distance = distance; - this.aimCharacteristic = aimCharacteristic; - } - } + private double lastDistance = 0; + + private double kInterpolationCacheThreshold = 0.0; // TODO: tune public static AimCharacteristic kRetractedAimCharacteristic = new AimCharacteristic(0.0, 0.0); @@ -28,10 +31,13 @@ public AimCharacteristicTableEntry(double distance, AimCharacteristic aimCharact public static AimCharacteristic kAmpAimCharacteristic = new AimCharacteristic(0.0, 0.0); // TODO: tune - private static AimCharacteristicTableEntry[] table = { - new AimCharacteristicTableEntry(0.0, new AimCharacteristic(0.0, 0.0)), - new AimCharacteristicTableEntry(1.0, new AimCharacteristic(0.0, 0.0)) - }; + private static LinkedHashMap table = + new LinkedHashMap<>() { + { + put(0.0, new AimCharacteristic(0.0, 0.0)); + put(1.0, new AimCharacteristic(0.0, 0.0)); + } + }; private static double interpolate( double lowKey, double lowValue, double highKey, double highValue, double x) { @@ -41,8 +47,28 @@ private static double interpolate( } public static AimCharacteristic getSpeakerAimCharacteristic(double distance) { - // TODO: write this correctly (not with the tree because it has edge cases at bounds) + List keys = table.keySet().stream().toList(); + double lowerBound = keys.get(0); + double upperBound = keys.get(keys.size() - 1); + + if ((distance < lowerBound)) distance = lowerBound; + else if (distance > upperBound) distance = upperBound; + + NavigableSet values = new TreeSet<>(keys); + double lowKey = values.floor(distance); + double highKey = values.ceiling(distance); + + return new AimCharacteristic( + interpolate(lowKey, table.get(lowKey).angle, highKey, table.get(highKey).angle, distance), + interpolate(lowKey, table.get(lowKey).rpm, highKey, table.get(highKey).rpm, distance)); + } + + public AimCharacteristic getLastAimCharacteristic(double distance) { + if (!epsilonEquals(lastDistance, distance, kInterpolationCacheThreshold)) { + lastAimCharacteristic = getSpeakerAimCharacteristic(lastDistance); + kInterpolationCacheThreshold = lastDistance; + } - return new AimCharacteristic(0.0, 0.0); + return lastAimCharacteristic; } } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index ba3cc22..3164f6e 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -149,7 +149,8 @@ public Command acceptHandoff(BooleanSupplier beamBreakStateSupplier) { return run(() -> this.setRPM(kHandoffRPM)).until(beamBreakStateSupplier).finallyDo(this::off); } - public Command reindex(BooleanSupplier beamBreakStateSupplier) { + // It's like cocking a gun, get your mind out of the gutter + public Command cock(BooleanSupplier beamBreakStateSupplier) { /* * TODO * @@ -163,17 +164,23 @@ public Command reindex(BooleanSupplier beamBreakStateSupplier) { * */ - return null; + return new SequentialCommandGroup( + new WaitUntilCommand(beamBreakStateSupplier), + run(() -> this.setRPM(kReindexRPM)).until(beamBreakStateSupplier), + run(() -> this.setRPM(-kReindexRPM)) + .until(() -> !beamBreakStateSupplier.getAsBoolean()), + new WaitCommand(0.2)) + .finallyDo(this::off); } - public Command outtake(BooleanSupplier beamBreakStateSupplier) { + public Command feed(BooleanSupplier beamBreakStateSupplier) { return new ParallelRaceGroup( run(() -> this.setRPM(kOuttakeRPM)), new SequentialCommandGroup( - new WaitUntilCommand(() -> beamBreakStateSupplier.getAsBoolean()), + new WaitUntilCommand(beamBreakStateSupplier), new WaitUntilCommand(() -> !beamBreakStateSupplier.getAsBoolean())), new WaitCommand( - 0.4) // TODO: just incase beam break fails, stop after some safe amount of time + 0.4) // just incase beam break fails, stop after some safe amount of time ) .finallyDo(this::off); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java index e586e2d..1fef901 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java @@ -1,6 +1,8 @@ /* (C) Robolancers 2024 */ package org.robolancers321.subsystems.launcher; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; @@ -10,6 +12,8 @@ import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import org.robolancers321.subsystems.launcher.AimTable.AimCharacteristic; +import java.util.function.DoubleSupplier; + public class Launcher extends SubsystemBase { /* * Singleton @@ -27,6 +31,9 @@ public static Launcher getInstance() { * Constants */ + private static final int kBeamBreakChannelPort = 1; + + private static final double kDebounceTime = 0.05; // TODO: beam break(s) /* @@ -37,12 +44,24 @@ public static Launcher getInstance() { public Indexer indexer; public Flywheel flywheel; + public AimTable aimTable; + + public Debouncer beamBreakDebouncer = new Debouncer(kDebounceTime, Debouncer.DebounceType.kBoth); + + public DigitalInput beamBreak; + // TODO: beam break(s) private Launcher() { this.pivot = Pivot.getInstance(); this.indexer = Indexer.getInstance(); this.flywheel = Flywheel.getInstance(); + this.aimTable = new AimTable(); + this.beamBreak = new DigitalInput(kBeamBreakChannelPort); + } + + public boolean getBeamBreakState() { + return beamBreakDebouncer.calculate(beamBreak.get()); } public Command yeetAmp() { @@ -50,23 +69,23 @@ public Command yeetAmp() { pivot.aimAtAmp(), new ParallelCommandGroup( flywheel.yeetNoteAmp(), - indexer.outtake(() -> true), // TODO: pass in beam break state + indexer.feed(() -> !getBeamBreakState()), new WaitCommand(0.2))); } - // TODO: how to go about ths for active tracking - public Command yeetSpeaker(double distance) { - AimCharacteristic aimCharacteristic = AimTable.getSpeakerAimCharacteristic(distance); + public Command yeetSpeaker(DoubleSupplier distanceSupplier) { + AimCharacteristic aimCharacteristic = aimTable.getLastAimCharacteristic(distanceSupplier.getAsDouble()); return new SequentialCommandGroup( - indexer.reindex(() -> true), // TODO: pass in beam break state + indexer.cock(this::getBeamBreakState), new ParallelRaceGroup( flywheel.yeetNoteSpeaker(() -> aimCharacteristic.rpm), new SequentialCommandGroup( pivot.aimAtSpeaker(() -> aimCharacteristic.angle), new WaitUntilCommand( - flywheel::isRevved), // TODO: maybe put a timeout here to be safe - indexer.outtake(() -> true), // TODO: pass in beam break state + flywheel::isRevved), + new WaitCommand(0.1), + indexer.feed(this::getBeamBreakState), new WaitCommand(0.2)))); } } From d44f88c88fc9ecc215803182622f830d087cb1d3 Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Mon, 29 Jan 2024 21:29:05 -0500 Subject: [PATCH 47/57] feat(mating): Added MateAndShoot command. Waiting for Matt to refactor and make it nice and pretty --- settings.gradle | 4 +- .../org/robolancers321/RobotContainer.java | 3 +- .../robolancers321/commands/MateAndShoot.java | 26 +++++++++ .../subsystems/intake/Intake.java | 55 +++++++++++-------- .../subsystems/intake/Retractor.java | 12 ++-- .../subsystems/intake/Sucker.java | 8 +-- .../subsystems/launcher/AimTable.java | 4 +- .../subsystems/launcher/Flywheel.java | 18 +++--- .../subsystems/launcher/Indexer.java | 2 +- .../subsystems/launcher/Launcher.java | 9 ++- 10 files changed, 86 insertions(+), 55 deletions(-) create mode 100644 src/main/java/org/robolancers321/commands/MateAndShoot.java diff --git a/settings.gradle b/settings.gradle index d94f73c..e23ea1a 100644 --- a/settings.gradle +++ b/settings.gradle @@ -26,5 +26,5 @@ pluginManagement { } } -Properties props = System.getProperties(); -props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); +Properties props = System.getProperties() +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true") diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index ae2c130..200704f 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -1,10 +1,9 @@ /* (C) Robolancers 2024 */ package org.robolancers321; -import org.robolancers321.subsystems.intake.Intake; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import org.robolancers321.subsystems.intake.Intake; import org.robolancers321.subsystems.launcher.Launcher; public class RobotContainer { diff --git a/src/main/java/org/robolancers321/commands/MateAndShoot.java b/src/main/java/org/robolancers321/commands/MateAndShoot.java new file mode 100644 index 0000000..1ee6786 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/MateAndShoot.java @@ -0,0 +1,26 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import java.util.function.DoubleSupplier; +import org.robolancers321.subsystems.intake.Intake; +import org.robolancers321.subsystems.launcher.Launcher; + +public class MateAndShoot extends SequentialCommandGroup { + + private final Launcher launcher; + private final Intake intake; + + public MateAndShoot(DoubleSupplier distanceSupplier) { + launcher = Launcher.getInstance(); + intake = Intake.getInstance(); + + addRequirements(launcher, intake); + // Both mating commands have an end condition + addCommands( + new ParallelCommandGroup(launcher.pivot.goToMating(), intake.retractor.moveToMating()), + new ParallelCommandGroup(intake.sucker.out(), launcher.yeetSpeaker(distanceSupplier)), + intake.goAway(launcher::getBeamBreakState)); + } +} diff --git a/src/main/java/org/robolancers321/subsystems/intake/Intake.java b/src/main/java/org/robolancers321/subsystems/intake/Intake.java index 7f029ce..b036747 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Intake.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Intake.java @@ -1,39 +1,46 @@ +/* (C) Robolancers 2024 */ package org.robolancers321.subsystems.intake; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import java.util.function.BooleanSupplier; public class Intake extends SubsystemBase { - /* - * Singleton - */ + /* + * Singleton + */ - private static Intake instance = null; + private static Intake instance = null; - public static Intake getInstance(){ - if (instance == null) instance = new Intake(); + public static Intake getInstance() { + if (instance == null) instance = new Intake(); - return instance; - } + return instance; + } - /* - * Constants - */ + /* + * Constants + */ - // TODO: beam break port and any other resources/information that interops with retractor and sucker + // TODO: beam break port and any other resources/information that interops with retractor and + // sucker - /* - * Implementation - */ + /* + * Implementation + */ - public Retractor retractor; - public Sucker sucker; - // TODO: beam break + public Retractor retractor; + public Sucker sucker; - private Intake(){ - this.retractor = Retractor.getInstance(); - this.sucker = Sucker.getInstance(); - // TODO: beam break - } + private Intake() { + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + } - // TODO: any getters, setters, and interop commands + public Command goAway(BooleanSupplier beamBreakState) { + return new SequentialCommandGroup( + new WaitUntilCommand(beamBreakState), retractor.moveToRetracted()); + } } diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java index 76a5b91..914a8af 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -49,14 +49,14 @@ public static Retractor getInstance() { private static final double kErrorThreshold = 2.0; - public static enum RetractorPosition { + public enum RetractorPosition { kRetracted(0), kMating(0), kIntake(0); - private double angle; + private final double angle; - private RetractorPosition(double angle) { + RetractorPosition(double angle) { this.angle = angle; } @@ -69,9 +69,9 @@ public double getAngle() { * Implementation */ - private CANSparkMax motor; - private AbsoluteEncoder encoder; - private ProfiledPIDController feedbackController; + private final CANSparkMax motor; + private final AbsoluteEncoder encoder; + private final ProfiledPIDController feedbackController; private ArmFeedforward feedforwardController; private Retractor() { diff --git a/src/main/java/org/robolancers321/subsystems/intake/Sucker.java b/src/main/java/org/robolancers321/subsystems/intake/Sucker.java index ec6967b..e2810b4 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Sucker.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Sucker.java @@ -44,9 +44,9 @@ public static Sucker getInstance() { * Implementation */ - private CANSparkMax motor; - private RelativeEncoder encoder; - private SparkPIDController controller; + private final CANSparkMax motor; + private final RelativeEncoder encoder; + private final SparkPIDController controller; private Sucker() { this.motor = new CANSparkMax(kMotorPort, MotorType.kBrushless); @@ -122,7 +122,7 @@ public Command in() { return run(() -> this.useController(kInRPM)).finallyDo(() -> this.useController(0.0)); } - public Command out(double power) { + public Command out() { return run(() -> this.useController(kOutRPM)).finallyDo(() -> this.useController(0.0)); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java index 85b263a..c91c747 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java @@ -21,7 +21,7 @@ public AimCharacteristic(double angle, double rpm) { private AimCharacteristic lastAimCharacteristic; - private double lastDistance = 0; + private final double lastDistance = 0; private double kInterpolationCacheThreshold = 0.0; @@ -31,7 +31,7 @@ public AimCharacteristic(double angle, double rpm) { public static AimCharacteristic kAmpAimCharacteristic = new AimCharacteristic(0.0, 0.0); // TODO: tune - private static LinkedHashMap table = + private static final LinkedHashMap table = new LinkedHashMap<>() { { put(0.0, new AimCharacteristic(0.0, 0.0)); diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java index 078df9b..837b658 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java @@ -29,7 +29,7 @@ public static Flywheel getInstance() { private static final boolean kInvertTopMotor = false; private static final boolean kInvertBottomMotor = false; - private static double kRampUpRate = 0.5; + private static final double kRampUpRate = 0.5; private static final double kFF = 0.0; @@ -39,17 +39,17 @@ public static Flywheel getInstance() { * Implementation */ - private CANSparkMax topMotor; - private CANSparkMax bottomMotor; + private final CANSparkMax topMotor; + private final CANSparkMax bottomMotor; - private RelativeEncoder topEncoder; - private RelativeEncoder bottomEncoder; + private final RelativeEncoder topEncoder; + private final RelativeEncoder bottomEncoder; - private SparkPIDController topController; - private SparkPIDController bottomController; + private final SparkPIDController topController; + private final SparkPIDController bottomController; - private SlewRateLimiter topLimiter; - private SlewRateLimiter bottomLimiter; + private final SlewRateLimiter topLimiter; + private final SlewRateLimiter bottomLimiter; private double goalRPM = 0.0; diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index 3164f6e..3d58295 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -165,8 +165,8 @@ public Command cock(BooleanSupplier beamBreakStateSupplier) { */ return new SequentialCommandGroup( - new WaitUntilCommand(beamBreakStateSupplier), run(() -> this.setRPM(kReindexRPM)).until(beamBreakStateSupplier), + new WaitCommand(0.1), run(() -> this.setRPM(-kReindexRPM)) .until(() -> !beamBreakStateSupplier.getAsBoolean()), new WaitCommand(0.2)) diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java index 1fef901..1aab75b 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java @@ -10,9 +10,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import org.robolancers321.subsystems.launcher.AimTable.AimCharacteristic; - import java.util.function.DoubleSupplier; +import org.robolancers321.subsystems.launcher.AimTable.AimCharacteristic; public class Launcher extends SubsystemBase { /* @@ -74,7 +73,8 @@ public Command yeetAmp() { } public Command yeetSpeaker(DoubleSupplier distanceSupplier) { - AimCharacteristic aimCharacteristic = aimTable.getLastAimCharacteristic(distanceSupplier.getAsDouble()); + AimCharacteristic aimCharacteristic = + aimTable.getLastAimCharacteristic(distanceSupplier.getAsDouble()); return new SequentialCommandGroup( indexer.cock(this::getBeamBreakState), @@ -82,8 +82,7 @@ public Command yeetSpeaker(DoubleSupplier distanceSupplier) { flywheel.yeetNoteSpeaker(() -> aimCharacteristic.rpm), new SequentialCommandGroup( pivot.aimAtSpeaker(() -> aimCharacteristic.angle), - new WaitUntilCommand( - flywheel::isRevved), + new WaitUntilCommand(flywheel::isRevved), new WaitCommand(0.1), indexer.feed(this::getBeamBreakState), new WaitCommand(0.2)))); From 5382e2459ee9373eb83aff23c0855006859007e7 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Mon, 29 Jan 2024 22:31:41 -0500 Subject: [PATCH 48/57] fixed mate command, renamed some methods and commands --- .../org/robolancers321/RobotContainer.java | 2 - .../org/robolancers321/commands/Mate.java | 26 +++++++++++ .../robolancers321/commands/MateAndShoot.java | 26 ----------- .../subsystems/intake/Intake.java | 12 +----- .../subsystems/launcher/AimTable.java | 36 +++++++++------- .../subsystems/launcher/Indexer.java | 24 ++--------- .../subsystems/launcher/Launcher.java | 43 ++++++++++--------- .../subsystems/launcher/Pivot.java | 4 +- 8 files changed, 77 insertions(+), 96 deletions(-) create mode 100644 src/main/java/org/robolancers321/commands/Mate.java delete mode 100644 src/main/java/org/robolancers321/commands/MateAndShoot.java diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 200704f..ff79c78 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -8,12 +8,10 @@ public class RobotContainer { Intake intake; - Launcher launcher; public RobotContainer() { this.intake = Intake.getInstance(); - this.launcher = Launcher.getInstance(); configureBindings(); diff --git a/src/main/java/org/robolancers321/commands/Mate.java b/src/main/java/org/robolancers321/commands/Mate.java new file mode 100644 index 0000000..675993e --- /dev/null +++ b/src/main/java/org/robolancers321/commands/Mate.java @@ -0,0 +1,26 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import java.util.function.DoubleSupplier; +import org.robolancers321.subsystems.intake.Intake; +import org.robolancers321.subsystems.launcher.Launcher; + +public class Mate extends SequentialCommandGroup { + private final Intake intake; + private final Launcher launcher; + + public Mate(DoubleSupplier distanceSupplier) { + this.intake = Intake.getInstance(); + this.launcher = Launcher.getInstance(); + + addCommands( + new ParallelCommandGroup(this.intake.retractor.moveToMating(), this.launcher.pivot.moveToMating()), + new ParallelRaceGroup(this.intake.sucker.out(), this.launcher.acceptHandoff()) + ); + + addRequirements(this.intake, this.launcher); + } +} diff --git a/src/main/java/org/robolancers321/commands/MateAndShoot.java b/src/main/java/org/robolancers321/commands/MateAndShoot.java deleted file mode 100644 index 1ee6786..0000000 --- a/src/main/java/org/robolancers321/commands/MateAndShoot.java +++ /dev/null @@ -1,26 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.commands; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import java.util.function.DoubleSupplier; -import org.robolancers321.subsystems.intake.Intake; -import org.robolancers321.subsystems.launcher.Launcher; - -public class MateAndShoot extends SequentialCommandGroup { - - private final Launcher launcher; - private final Intake intake; - - public MateAndShoot(DoubleSupplier distanceSupplier) { - launcher = Launcher.getInstance(); - intake = Intake.getInstance(); - - addRequirements(launcher, intake); - // Both mating commands have an end condition - addCommands( - new ParallelCommandGroup(launcher.pivot.goToMating(), intake.retractor.moveToMating()), - new ParallelCommandGroup(intake.sucker.out(), launcher.yeetSpeaker(distanceSupplier)), - intake.goAway(launcher::getBeamBreakState)); - } -} diff --git a/src/main/java/org/robolancers321/subsystems/intake/Intake.java b/src/main/java/org/robolancers321/subsystems/intake/Intake.java index b036747..a64b1bf 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Intake.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Intake.java @@ -1,11 +1,7 @@ /* (C) Robolancers 2024 */ package org.robolancers321.subsystems.intake; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import java.util.function.BooleanSupplier; public class Intake extends SubsystemBase { /* @@ -24,8 +20,7 @@ public static Intake getInstance() { * Constants */ - // TODO: beam break port and any other resources/information that interops with retractor and - // sucker + // TODO: beam break /* * Implementation @@ -38,9 +33,4 @@ private Intake() { this.retractor = Retractor.getInstance(); this.sucker = Sucker.getInstance(); } - - public Command goAway(BooleanSupplier beamBreakState) { - return new SequentialCommandGroup( - new WaitUntilCommand(beamBreakState), retractor.moveToRetracted()); - } } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java index c91c747..42db7dc 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java @@ -19,16 +19,10 @@ public AimCharacteristic(double angle, double rpm) { } } - private AimCharacteristic lastAimCharacteristic; - - private final double lastDistance = 0; - - private double kInterpolationCacheThreshold = 0.0; - // TODO: tune - public static AimCharacteristic kRetractedAimCharacteristic = new AimCharacteristic(0.0, 0.0); - public static AimCharacteristic kMatingAimCharacteristic = new AimCharacteristic(0.0, 0.0); - public static AimCharacteristic kAmpAimCharacteristic = new AimCharacteristic(0.0, 0.0); + public static final AimCharacteristic kRetractedAimCharacteristic = new AimCharacteristic(0.0, 0.0); + public static final AimCharacteristic kMatingAimCharacteristic = new AimCharacteristic(0.0, 0.0); + public static final AimCharacteristic kAmpAimCharacteristic = new AimCharacteristic(0.0, 0.0); // TODO: tune private static final LinkedHashMap table = @@ -46,7 +40,9 @@ private static double interpolate( return lowKey + percent * (highValue - lowValue); } - public static AimCharacteristic getSpeakerAimCharacteristic(double distance) { + private static final double kInterpolationCacheThreshold = 0.0; + + private static AimCharacteristic calculateSpeakerAimCharacteristic(double distance) { List keys = table.keySet().stream().toList(); double lowerBound = keys.get(0); double upperBound = keys.get(keys.size() - 1); @@ -63,12 +59,22 @@ public static AimCharacteristic getSpeakerAimCharacteristic(double distance) { interpolate(lowKey, table.get(lowKey).rpm, highKey, table.get(highKey).rpm, distance)); } - public AimCharacteristic getLastAimCharacteristic(double distance) { - if (!epsilonEquals(lastDistance, distance, kInterpolationCacheThreshold)) { - lastAimCharacteristic = getSpeakerAimCharacteristic(lastDistance); - kInterpolationCacheThreshold = lastDistance; + private double lastDistance; + private AimCharacteristic lastAimCharacteristic; + + public AimTable(){ + this.lastDistance = 0.0; + this.lastAimCharacteristic = new AimCharacteristic(0.0, 0.0); + } + + public void updateSpeakerAimCharacteristic(double distance) { + if (!epsilonEquals(this.lastDistance, distance, kInterpolationCacheThreshold)) { + this.lastDistance = distance; + this.lastAimCharacteristic = calculateSpeakerAimCharacteristic(distance); } + } - return lastAimCharacteristic; + public AimCharacteristic getSpeakerAimCharacteristic(){ + return this.lastAimCharacteristic; } } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index 3d58295..22f5b7e 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -149,31 +149,15 @@ public Command acceptHandoff(BooleanSupplier beamBreakStateSupplier) { return run(() -> this.setRPM(kHandoffRPM)).until(beamBreakStateSupplier).finallyDo(this::off); } - // It's like cocking a gun, get your mind out of the gutter - public Command cock(BooleanSupplier beamBreakStateSupplier) { - /* - * TODO - * - * sequential - * while beam is broken, setRPM(kReindexRPM) - * while beam is not broken, setRPM(-kReindexRPM) - * finally do set rpm to 0 - * - * - * this should also have a timeout for safety - * - */ - + public Command shiftIntoPosition(BooleanSupplier beamBreakStateSupplier) { return new SequentialCommandGroup( run(() -> this.setRPM(kReindexRPM)).until(beamBreakStateSupplier), - new WaitCommand(0.1), - run(() -> this.setRPM(-kReindexRPM)) - .until(() -> !beamBreakStateSupplier.getAsBoolean()), - new WaitCommand(0.2)) + run(() -> this.setRPM(-kReindexRPM)).until(() -> !beamBreakStateSupplier.getAsBoolean()) + ) .finallyDo(this::off); } - public Command feed(BooleanSupplier beamBreakStateSupplier) { + public Command outtake(BooleanSupplier beamBreakStateSupplier) { return new ParallelRaceGroup( run(() -> this.setRPM(kOuttakeRPM)), new SequentialCommandGroup( diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java index 1aab75b..8deb50a 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -30,10 +31,9 @@ public static Launcher getInstance() { * Constants */ - private static final int kBeamBreakChannelPort = 1; + private static final int kBeamBreakPort = 0; private static final double kDebounceTime = 0.05; - // TODO: beam break(s) /* * Implementation @@ -43,48 +43,51 @@ public static Launcher getInstance() { public Indexer indexer; public Flywheel flywheel; - public AimTable aimTable; + private DigitalInput beamBreak; + private Debouncer beamBreakDebouncer; - public Debouncer beamBreakDebouncer = new Debouncer(kDebounceTime, Debouncer.DebounceType.kBoth); - - public DigitalInput beamBreak; - - // TODO: beam break(s) + private AimTable aimTable; private Launcher() { this.pivot = Pivot.getInstance(); this.indexer = Indexer.getInstance(); this.flywheel = Flywheel.getInstance(); + + this.beamBreak = new DigitalInput(kBeamBreakPort); + this.beamBreakDebouncer = new Debouncer(kDebounceTime, Debouncer.DebounceType.kBoth); + this.aimTable = new AimTable(); - this.beamBreak = new DigitalInput(kBeamBreakChannelPort); } public boolean getBeamBreakState() { return beamBreakDebouncer.calculate(beamBreak.get()); } + public Command acceptHandoff(){ + return this.indexer.acceptHandoff(this::getBeamBreakState); + } + public Command yeetAmp() { return new SequentialCommandGroup( pivot.aimAtAmp(), new ParallelCommandGroup( flywheel.yeetNoteAmp(), - indexer.feed(() -> !getBeamBreakState()), + indexer.outtake(this::getBeamBreakState), new WaitCommand(0.2))); } public Command yeetSpeaker(DoubleSupplier distanceSupplier) { - AimCharacteristic aimCharacteristic = - aimTable.getLastAimCharacteristic(distanceSupplier.getAsDouble()); - - return new SequentialCommandGroup( - indexer.cock(this::getBeamBreakState), + return new ParallelRaceGroup( + new RunCommand(() -> this.aimTable.updateSpeakerAimCharacteristic(distanceSupplier.getAsDouble())), + new SequentialCommandGroup( + indexer.shiftIntoPosition(this::getBeamBreakState), new ParallelRaceGroup( - flywheel.yeetNoteSpeaker(() -> aimCharacteristic.rpm), + flywheel.yeetNoteSpeaker(() -> this.aimTable.getSpeakerAimCharacteristic().rpm), new SequentialCommandGroup( - pivot.aimAtSpeaker(() -> aimCharacteristic.angle), + pivot.aimAtSpeaker(() -> this.aimTable.getSpeakerAimCharacteristic().angle), new WaitUntilCommand(flywheel::isRevved), - new WaitCommand(0.1), - indexer.feed(this::getBeamBreakState), - new WaitCommand(0.2)))); + indexer.outtake(this::getBeamBreakState), + new WaitCommand(0.2)))) + ); } } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 4e425a5..03ddde7 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -185,11 +185,11 @@ private Command moveToAngle(double angle) { return this.moveToAngle(() -> angle); } - public Command goToRetracted() { + public Command moveToRetracted() { return this.moveToAngle(AimTable.kRetractedAimCharacteristic.angle); } - public Command goToMating() { + public Command moveToMating() { return this.moveToAngle(AimTable.kMatingAimCharacteristic.angle); } From ead4df9b24f0a13b02db43aa1a3d2c99afd3e4f1 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Mon, 29 Jan 2024 22:32:30 -0500 Subject: [PATCH 49/57] spotless --- .../org/robolancers321/commands/Mate.java | 8 +++--- .../subsystems/launcher/AimTable.java | 9 ++++--- .../subsystems/launcher/Indexer.java | 4 +-- .../subsystems/launcher/Launcher.java | 25 +++++++++---------- 4 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/main/java/org/robolancers321/commands/Mate.java b/src/main/java/org/robolancers321/commands/Mate.java index 675993e..3e47cc5 100644 --- a/src/main/java/org/robolancers321/commands/Mate.java +++ b/src/main/java/org/robolancers321/commands/Mate.java @@ -15,11 +15,11 @@ public class Mate extends SequentialCommandGroup { public Mate(DoubleSupplier distanceSupplier) { this.intake = Intake.getInstance(); this.launcher = Launcher.getInstance(); - + addCommands( - new ParallelCommandGroup(this.intake.retractor.moveToMating(), this.launcher.pivot.moveToMating()), - new ParallelRaceGroup(this.intake.sucker.out(), this.launcher.acceptHandoff()) - ); + new ParallelCommandGroup( + this.intake.retractor.moveToMating(), this.launcher.pivot.moveToMating()), + new ParallelRaceGroup(this.intake.sucker.out(), this.launcher.acceptHandoff())); addRequirements(this.intake, this.launcher); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java index 42db7dc..8a7a961 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java @@ -20,7 +20,8 @@ public AimCharacteristic(double angle, double rpm) { } // TODO: tune - public static final AimCharacteristic kRetractedAimCharacteristic = new AimCharacteristic(0.0, 0.0); + public static final AimCharacteristic kRetractedAimCharacteristic = + new AimCharacteristic(0.0, 0.0); public static final AimCharacteristic kMatingAimCharacteristic = new AimCharacteristic(0.0, 0.0); public static final AimCharacteristic kAmpAimCharacteristic = new AimCharacteristic(0.0, 0.0); @@ -41,7 +42,7 @@ private static double interpolate( } private static final double kInterpolationCacheThreshold = 0.0; - + private static AimCharacteristic calculateSpeakerAimCharacteristic(double distance) { List keys = table.keySet().stream().toList(); double lowerBound = keys.get(0); @@ -62,7 +63,7 @@ private static AimCharacteristic calculateSpeakerAimCharacteristic(double distan private double lastDistance; private AimCharacteristic lastAimCharacteristic; - public AimTable(){ + public AimTable() { this.lastDistance = 0.0; this.lastAimCharacteristic = new AimCharacteristic(0.0, 0.0); } @@ -74,7 +75,7 @@ public void updateSpeakerAimCharacteristic(double distance) { } } - public AimCharacteristic getSpeakerAimCharacteristic(){ + public AimCharacteristic getSpeakerAimCharacteristic() { return this.lastAimCharacteristic; } } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index 22f5b7e..f66d0d7 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -152,8 +152,8 @@ public Command acceptHandoff(BooleanSupplier beamBreakStateSupplier) { public Command shiftIntoPosition(BooleanSupplier beamBreakStateSupplier) { return new SequentialCommandGroup( run(() -> this.setRPM(kReindexRPM)).until(beamBreakStateSupplier), - run(() -> this.setRPM(-kReindexRPM)).until(() -> !beamBreakStateSupplier.getAsBoolean()) - ) + run(() -> this.setRPM(-kReindexRPM)) + .until(() -> !beamBreakStateSupplier.getAsBoolean())) .finallyDo(this::off); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java index 8deb50a..7fc599e 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java @@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import java.util.function.DoubleSupplier; -import org.robolancers321.subsystems.launcher.AimTable.AimCharacteristic; public class Launcher extends SubsystemBase { /* @@ -63,7 +62,7 @@ public boolean getBeamBreakState() { return beamBreakDebouncer.calculate(beamBreak.get()); } - public Command acceptHandoff(){ + public Command acceptHandoff() { return this.indexer.acceptHandoff(this::getBeamBreakState); } @@ -78,16 +77,16 @@ public Command yeetAmp() { public Command yeetSpeaker(DoubleSupplier distanceSupplier) { return new ParallelRaceGroup( - new RunCommand(() -> this.aimTable.updateSpeakerAimCharacteristic(distanceSupplier.getAsDouble())), - new SequentialCommandGroup( - indexer.shiftIntoPosition(this::getBeamBreakState), - new ParallelRaceGroup( - flywheel.yeetNoteSpeaker(() -> this.aimTable.getSpeakerAimCharacteristic().rpm), - new SequentialCommandGroup( - pivot.aimAtSpeaker(() -> this.aimTable.getSpeakerAimCharacteristic().angle), - new WaitUntilCommand(flywheel::isRevved), - indexer.outtake(this::getBeamBreakState), - new WaitCommand(0.2)))) - ); + new RunCommand( + () -> this.aimTable.updateSpeakerAimCharacteristic(distanceSupplier.getAsDouble())), + new SequentialCommandGroup( + indexer.shiftIntoPosition(this::getBeamBreakState), + new ParallelRaceGroup( + flywheel.yeetNoteSpeaker(() -> this.aimTable.getSpeakerAimCharacteristic().rpm), + new SequentialCommandGroup( + pivot.aimAtSpeaker(() -> this.aimTable.getSpeakerAimCharacteristic().angle), + new WaitUntilCommand(flywheel::isRevved), + indexer.outtake(this::getBeamBreakState), + new WaitCommand(0.2))))); } } From 6aeed225a8700ef136297c409ea5c4e333ab20fe Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Fri, 2 Feb 2024 16:26:46 -0500 Subject: [PATCH 50/57] chore(aim-table): Wrote unit tests and fixed bugs --- build.gradle | 1 + .../subsystems/launcher/AimTable.java | 13 +- .../java/org/robolancers321/AimTableTest.java | 171 ++++++++++++++++++ 3 files changed, 182 insertions(+), 3 deletions(-) create mode 100644 src/test/java/org/robolancers321/AimTableTest.java diff --git a/build.gradle b/build.gradle index 90e315c..0bd6441 100644 --- a/build.gradle +++ b/build.gradle @@ -69,6 +69,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testImplementation 'org.junit.platform:junit-platform-launcher:1.8.2' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java index c91c747..ccf6003 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java @@ -47,12 +47,19 @@ private static double interpolate( } public static AimCharacteristic getSpeakerAimCharacteristic(double distance) { - List keys = table.keySet().stream().toList(); + List keys = table.keySet().stream().sorted().toList(); double lowerBound = keys.get(0); double upperBound = keys.get(keys.size() - 1); - if ((distance < lowerBound)) distance = lowerBound; - else if (distance > upperBound) distance = upperBound; + if ((distance < lowerBound)) { + AimTable.AimCharacteristic lowerValue = table.get(lowerBound); + + return new AimTable.AimCharacteristic(lowerValue.angle, lowerValue.rpm); + } else if (distance > upperBound) { + AimTable.AimCharacteristic upperValue = table.get(upperBound); + + return new AimTable.AimCharacteristic(upperValue.angle, upperValue.rpm); + } NavigableSet values = new TreeSet<>(keys); double lowKey = values.floor(distance); diff --git a/src/test/java/org/robolancers321/AimTableTest.java b/src/test/java/org/robolancers321/AimTableTest.java new file mode 100644 index 0000000..cd274fe --- /dev/null +++ b/src/test/java/org/robolancers321/AimTableTest.java @@ -0,0 +1,171 @@ +package org.robolancers321; + +import static java.lang.System.in; +import static org.junit.jupiter.api.Assertions.assertArrayEquals; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.robolancers321.util.MathUtils.epsilonEquals; + +import org.junit.jupiter.api.Test; +import org.robolancers321.subsystems.launcher.AimTable; + +import java.util.*; + + +public class AimTableTest { + + + private static final LinkedHashMap table = + new LinkedHashMap<>() { + { + put(0.0, new AimTable.AimCharacteristic(0, 0.0)); + put(1.0, new AimTable.AimCharacteristic(10, 1020)); + put(2.0, new AimTable.AimCharacteristic(20, 1500)); + put(4.0, new AimTable.AimCharacteristic(40, 2500)); + put(5.0, new AimTable.AimCharacteristic(54, 3300)); + put(6.0, new AimTable.AimCharacteristic(61, 4227)); + put(8.0, new AimTable.AimCharacteristic(83, 5599)); + } + }; + + private class DummyAimTable { + private AimTable.AimCharacteristic lastAimCharacteristic; + + private final double lastDistance = 0; + + private double kInterpolationCacheThreshold = 0.0; + + private static double interpolate( + double lowKey, double lowValue, double highKey, double highValue, double x) { + double percent = (x - lowKey) / (highKey - lowKey); + + return lowValue + percent * (highValue - lowValue); + } + + public static AimTable.AimCharacteristic getSpeakerAimCharacteristic(double distance) { + List keys = table.keySet().stream().sorted().toList(); + double lowerBound = keys.get(0); + double upperBound = keys.get(keys.size() - 1); + + if ((distance < lowerBound)) { + AimTable.AimCharacteristic lowerValue = table.get(lowerBound); + + return new AimTable.AimCharacteristic(lowerValue.angle, lowerValue.rpm); + } + else if (distance > upperBound) { + AimTable.AimCharacteristic upperValue = table.get(upperBound); + + return new AimTable.AimCharacteristic(upperValue.angle, upperValue.rpm); + } + + System.out.println("Distance " + distance); + + NavigableSet values = new TreeSet<>(keys); + double lowKey = values.floor(distance); + double highKey = values.ceiling(distance); + + return new AimTable.AimCharacteristic( + interpolate(lowKey, table.get(lowKey).angle, highKey, table.get(highKey).angle, distance), + interpolate(lowKey, table.get(lowKey).rpm, highKey, table.get(highKey).rpm, distance)); + } + + public AimTable.AimCharacteristic getLastAimCharacteristic(double distance) { + if (!epsilonEquals(lastDistance, distance, kInterpolationCacheThreshold)) { + lastAimCharacteristic = getSpeakerAimCharacteristic(lastDistance); + kInterpolationCacheThreshold = lastDistance; + } + + return lastAimCharacteristic; + } + } + + + private List getKeys() { + return table.keySet().stream().sorted().toList(); + } + private static double interpolate( + double lowKey, double lowValue, double highKey, double highValue, double x) { + double percent = (x - lowKey) / (highKey - lowKey); + + + return lowValue + percent * (highValue - lowValue); + } + + @Test + void testKeys() { + var keys = getKeys(); + + var expectedKeys = new Double[] { + 0.0, + 1.0, + 2.0, + 4.0, + 5.0, + 6.0, + 8.0, + }; + + assertArrayEquals(keys.toArray(new Double[0]), expectedKeys); + } + @Test + void testUpperBoundKey() { + var keys = getKeys(); + double upperBound = keys.get(keys.size() - 1); + + assertEquals(14.0, upperBound); + } + + @Test + void testLowerBoundKey(){ + var keys = getKeys(); + double lowerBound = keys.get(0); + + assertEquals(-1.0, lowerBound); + } + + @Test + void findAdjacentKeysTest() { + NavigableSet values = new TreeSet<>(getKeys()); + double distance = 3.0; + double lowKey = values.floor(distance); + double highKey = values.ceiling(distance); + + assertEquals(2.0, lowKey); + assertEquals(4.0, highKey); + } + + @Test + void interpolateTest() { + + double lowKey = 2.0; + double lowValue = 1500; + double highKey = 4.0; + double highValue = 2500; + double x = 3.0; + + assertEquals(2000, interpolate(lowKey, lowValue, highKey, highValue, x)); + } + + + private Double[] generateNumbers(int count, int range) { + Random rand = new Random(); + List nums = new ArrayList<>(); + for (int i = 0; i < count; i++) { + nums.add(rand.nextDouble(range + 1)); + } + return nums.toArray(new Double[0]); + } + @Test + void testAimTable() { + DummyAimTable aimTable = new DummyAimTable(); + Double[] nums = generateNumbers(20, 20); + + + for (Double num : nums) { + AimTable.AimCharacteristic characteristic = aimTable.getSpeakerAimCharacteristic(num); + System.out.println(num); + System.out.println(characteristic.angle + " " + characteristic.rpm); + } + } + + +} From 777d3992230dce1a00ff1907be18915903668cf2 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Wed, 14 Feb 2024 16:51:26 -0500 Subject: [PATCH 51/57] working changes for tuning --- .../org/robolancers321/RobotContainer.java | 6 +- .../subsystems/intake/Retractor.java | 5 +- .../subsystems/intake/Sucker.java | 3 +- .../subsystems/launcher/Flywheel.java | 124 ++++++------------ .../subsystems/launcher/Indexer.java | 16 ++- .../subsystems/launcher/Launcher.java | 9 +- .../subsystems/launcher/Pivot.java | 11 +- 7 files changed, 71 insertions(+), 103 deletions(-) diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index ff79c78..55898d5 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -7,13 +7,15 @@ import org.robolancers321.subsystems.launcher.Launcher; public class RobotContainer { - Intake intake; + // Intake intake; Launcher launcher; public RobotContainer() { - this.intake = Intake.getInstance(); + // this.intake = Intake.getInstance(); this.launcher = Launcher.getInstance(); + this.launcher.pivot.setDefaultCommand(this.launcher.pivot.tuneControllers()); + configureBindings(); } diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java index 914a8af..e6c8e78 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -31,9 +31,9 @@ public static Retractor getInstance() { * Constants */ - private static final int kMotorPort = 0; + private static final int kMotorPort = 13; - private static final boolean kInvertMotor = false; + private static final boolean kInvertMotor = true; private static final int kCurrentLimit = 20; private static final double kP = 0.000; @@ -84,6 +84,7 @@ private Retractor() { this.configureMotor(); this.configureEncoder(); this.configureController(); + this.motor.burnFlash(); } private void configureMotor() { diff --git a/src/main/java/org/robolancers321/subsystems/intake/Sucker.java b/src/main/java/org/robolancers321/subsystems/intake/Sucker.java index e2810b4..4cb7629 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Sucker.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Sucker.java @@ -27,7 +27,7 @@ public static Sucker getInstance() { * Constants */ - private static final int kMotorPort = 0; + private static final int kMotorPort = 14; private static final boolean kInvertMotor = false; private static final int kCurrentLimit = 20; @@ -56,6 +56,7 @@ private Sucker() { this.configureMotor(); this.configureEncoder(); this.configureController(); + this.motor.burnFlash(); } private void configureMotor() { diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java index 837b658..96d4df9 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java @@ -4,6 +4,8 @@ import static org.robolancers321.util.MathUtils.epsilonEquals; import com.revrobotics.*; +import com.revrobotics.CANSparkBase.ControlType; + import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -21,14 +23,11 @@ public static Flywheel getInstance() { /* Constants */ - private static final int kTopMotorPort = 0; - private static final int kBottomMotorPort = 0; + private static final int kMotorPort = 17; + private static final boolean kInvertMotor = false; private static final int kCurrentLimit = 40; - private static final boolean kInvertTopMotor = false; - private static final boolean kInvertBottomMotor = false; - private static final double kRampUpRate = 0.5; private static final double kFF = 0.0; @@ -39,106 +38,70 @@ public static Flywheel getInstance() { * Implementation */ - private final CANSparkMax topMotor; - private final CANSparkMax bottomMotor; - - private final RelativeEncoder topEncoder; - private final RelativeEncoder bottomEncoder; + private final CANSparkFlex motor; + private final RelativeEncoder encoder; + private final SparkPIDController controller; - private final SparkPIDController topController; - private final SparkPIDController bottomController; - - private final SlewRateLimiter topLimiter; - private final SlewRateLimiter bottomLimiter; + private final SlewRateLimiter limiter; private double goalRPM = 0.0; private Flywheel() { - this.topMotor = new CANSparkMax(kTopMotorPort, CANSparkLowLevel.MotorType.kBrushless); - this.bottomMotor = new CANSparkMax(kBottomMotorPort, CANSparkLowLevel.MotorType.kBrushless); - - this.topEncoder = this.topMotor.getEncoder(); - this.bottomEncoder = this.bottomMotor.getEncoder(); - - this.topController = this.topMotor.getPIDController(); - this.bottomController = this.bottomMotor.getPIDController(); + this.motor = new CANSparkFlex(kMotorPort, CANSparkLowLevel.MotorType.kBrushless); - this.topLimiter = new SlewRateLimiter(kRampUpRate); - this.bottomLimiter = new SlewRateLimiter(kRampUpRate); + this.encoder = this.motor.getEncoder(); - this.configureMotors(); - this.configureEncoders(); - this.configureControllers(); - } + this.controller = this.motor.getPIDController(); - private void configureMotors() { - this.topMotor.setInverted(kInvertTopMotor); - this.topMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); - this.topMotor.setSmartCurrentLimit(kCurrentLimit); - this.topMotor.enableVoltageCompensation(12); - this.topMotor.burnFlash(); + this.limiter = new SlewRateLimiter(kRampUpRate); - this.bottomMotor.setInverted(kInvertBottomMotor); - this.bottomMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); - this.bottomMotor.setSmartCurrentLimit(kCurrentLimit); - this.bottomMotor.enableVoltageCompensation(12); - this.bottomMotor.burnFlash(); + this.configureMotor(); + this.configureEncoder(); + this.configureController(); + this.motor.burnFlash(); } - private void configureEncoders() { - this.topEncoder.setVelocityConversionFactor(1.0); - this.bottomEncoder.setVelocityConversionFactor(1.0); + private void configureMotor() { + this.motor.setInverted(kInvertMotor); + this.motor.setIdleMode(CANSparkBase.IdleMode.kBrake); + this.motor.setSmartCurrentLimit(kCurrentLimit); + this.motor.enableVoltageCompensation(12); } - private void configureControllers() { - this.topController.setP(0.0); - this.topController.setI(0.0); - this.topController.setD(0.0); - this.topController.setFF(kFF); - - this.bottomController.setP(0.0); - this.bottomController.setI(0.0); - this.bottomController.setD(0.0); - this.bottomController.setFF(kFF); + private void configureEncoder() { + this.encoder.setVelocityConversionFactor(1.0); } - private double getTopRPM() { - // TODO: filter here? - return this.topEncoder.getVelocity(); + private void configureController() { + this.controller.setP(0.0); + this.controller.setI(0.0); + this.controller.setD(0.0); + this.controller.setFF(kFF); } - private double getBottomRPM() { + private double getRPM() { // TODO: filter here? - return this.bottomEncoder.getVelocity(); + return this.encoder.getVelocity(); } - private void useControllers() { - this.topController.setReference( - this.topLimiter.calculate(this.goalRPM), CANSparkBase.ControlType.kVelocity); - this.bottomController.setReference( - this.bottomLimiter.calculate(this.goalRPM), CANSparkBase.ControlType.kVelocity); - } - - private boolean isTopRevved() { - return epsilonEquals(this.getTopRPM(), this.goalRPM, kErrorTolerance); - } + private void useController() { + this.controller.setReference(this.goalRPM, ControlType.kVelocity); - private boolean isBottomRevved() { - return epsilonEquals(this.getBottomRPM(), this.goalRPM, kErrorTolerance); + // For tuning, no limiter + // this.controller.setReference( + // this.limiter.calculate(this.goalRPM), CANSparkBase.ControlType.kVelocity); } public boolean isRevved() { - return this.isTopRevved() && this.isBottomRevved(); + return epsilonEquals(this.getRPM(), this.goalRPM, kErrorTolerance); } private void dangerouslySetSpeed(double speed) { - this.topMotor.set(speed); - this.bottomMotor.set(speed); + this.motor.set(speed); } private void doSendables() { - SmartDashboard.putNumber("top flywheel rpm", this.getTopRPM()); - SmartDashboard.putNumber("bottom flywheel rpm", this.getBottomRPM()); + SmartDashboard.putNumber("flywheel rpm", this.getRPM()); } @Override @@ -155,12 +118,11 @@ private void initTuning() { private void tune() { double tunedFF = SmartDashboard.getNumber("shooter kff", kFF); - this.topController.setFF(tunedFF); - this.bottomController.setFF(tunedFF); + this.controller.setFF(tunedFF); this.goalRPM = SmartDashboard.getNumber("target shooter rpm", 0.0); - this.useControllers(); + this.useController(); } public Command dangerouslyYeet(double speed) { @@ -172,20 +134,20 @@ private Command off() { return runOnce( () -> { this.goalRPM = 0.0; - this.useControllers(); + this.useController(); }); } public Command yeetNoteAmp() { this.goalRPM = AimTable.kAmpAimCharacteristic.rpm; - return run(this::useControllers).finallyDo(this::off); + return run(this::useController).finallyDo(this::off); } public Command yeetNoteSpeaker(DoubleSupplier rpmSupplier) { return run(() -> { this.goalRPM = rpmSupplier.getAsDouble(); - this.useControllers(); + this.useController(); }) .finallyDo(this::off); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index f66d0d7..087d217 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -33,8 +33,8 @@ public static Indexer getInstance() { * Constants */ - private static final int kMotorPort = 0; - private static final int kBeamBreakPort = 0; + private static final int kMotorPort = 16; + // private static final int kBeamBreakPort = 0; // TODO private static final boolean kInvertMotor = false; private static final int kCurrentLimit = 20; @@ -52,22 +52,23 @@ public static Indexer getInstance() { * Implementation */ - private final CANSparkMax motor; + private final CANSparkFlex motor; private final SparkPIDController controller; private final RelativeEncoder encoder; - private final DigitalInput beamBreak; + // private final DigitalInput beamBreak; // TODO private Indexer() { - this.motor = new CANSparkMax(kMotorPort, kBrushless); + this.motor = new CANSparkFlex(kMotorPort, kBrushless); this.encoder = this.motor.getEncoder(); this.controller = this.motor.getPIDController(); - this.beamBreak = new DigitalInput(kBeamBreakPort); + // this.beamBreak = new DigitalInput(kBeamBreakPort); // TODO this.configureMotor(); this.configureEncoder(); this.configureController(); + this.motor.burnFlash(); } private void configureMotor() { @@ -93,7 +94,8 @@ public double getRPM() { } public boolean jawnDetected() { - return this.beamBreak.get(); + return true; + // return this.beamBreak.get(); // TODO } private void dangerouslySetSpeed(double speed) { diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java index 7fc599e..a37ce45 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java @@ -30,7 +30,7 @@ public static Launcher getInstance() { * Constants */ - private static final int kBeamBreakPort = 0; + // private static final int kBeamBreakPort = 0; // TODO private static final double kDebounceTime = 0.05; @@ -42,7 +42,7 @@ public static Launcher getInstance() { public Indexer indexer; public Flywheel flywheel; - private DigitalInput beamBreak; + // private DigitalInput beamBreak; // TODO private Debouncer beamBreakDebouncer; private AimTable aimTable; @@ -52,14 +52,15 @@ private Launcher() { this.indexer = Indexer.getInstance(); this.flywheel = Flywheel.getInstance(); - this.beamBreak = new DigitalInput(kBeamBreakPort); + // this.beamBreak = new DigitalInput(kBeamBreakPort); // TODO this.beamBreakDebouncer = new Debouncer(kDebounceTime, Debouncer.DebounceType.kBoth); this.aimTable = new AimTable(); } public boolean getBeamBreakState() { - return beamBreakDebouncer.calculate(beamBreak.get()); + return true; + // return beamBreakDebouncer.calculate(beamBreak.get()); // TODO } public Command acceptHandoff() { diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 03ddde7..ecf9140 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -30,17 +30,16 @@ public static Pivot getInstance() { * Constants */ - private static final int kMotorPort = 0; + private static final int kMotorPort = 15; private static final boolean kInvertMotor = false; - private static final boolean kInvertEncoder = false; private static final int kCurrentLimit = 40; private static final double kGearRatio = 360.0; private static final double kRotPerMinToDegPerSec = kGearRatio / 60.0; - private static final float kMinAngle = 0.0f; - private static final float kMaxAngle = 270.0f; + private static final float kMinAngle = -30.0f; + private static final float kMaxAngle = 90.0f; private static final double kS = 0.0; private static final double kG = 0.0; @@ -86,6 +85,7 @@ private Pivot() { this.configureMotor(); this.configureEncoder(); this.configureController(); + this.motor.burnFlash(); } private void configureMotor() { @@ -101,7 +101,6 @@ private void configureMotor() { } private void configureEncoder() { - this.absoluteEncoder.setInverted(kInvertEncoder); this.absoluteEncoder.setPositionConversionFactor(kGearRatio); this.absoluteEncoder.setVelocityConversionFactor(kRotPerMinToDegPerSec); } @@ -130,7 +129,7 @@ private boolean atGoal() { @Override protected void useOutput(double output, TrapezoidProfile.State setpoint) { double feedforwardOutput = - this.feedforwardController.calculate(setpoint.position, setpoint.velocity); + this.feedforwardController.calculate(setpoint.position * Math.PI / 180, setpoint.velocity * Math.PI / 180); double feedbackOutput = super.m_controller.calculate(this.getPositionDeg()); From 363353e93e01d04deba93de6ea6a68d470d34a7c Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Wed, 14 Feb 2024 20:44:49 -0500 Subject: [PATCH 52/57] initial constants for pivot and flywheel that will probably change, also changed some smart dashboard name spacing --- .../org/robolancers321/RobotContainer.java | 8 ++- .../org/robolancers321/commands/Mate.java | 9 +++- .../subsystems/intake/Intake.java | 6 +-- .../subsystems/intake/Retractor.java | 50 +++++++++++-------- .../subsystems/intake/Sucker.java | 35 ++++--------- .../subsystems/launcher/Flywheel.java | 24 +++++---- .../subsystems/launcher/Indexer.java | 27 ++++------ .../subsystems/launcher/Launcher.java | 45 +++++++++++++---- .../subsystems/launcher/Pivot.java | 41 +++++++++------ .../java/org/robolancers321/AimTableTest.java | 2 - 10 files changed, 138 insertions(+), 109 deletions(-) diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 55898d5..ff5c742 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -7,14 +7,18 @@ import org.robolancers321.subsystems.launcher.Launcher; public class RobotContainer { + // TODO // Intake intake; + Launcher launcher; public RobotContainer() { + // TODO // this.intake = Intake.getInstance(); - this.launcher = Launcher.getInstance(); - this.launcher.pivot.setDefaultCommand(this.launcher.pivot.tuneControllers()); + this.launcher = Launcher.getInstance(); + + this.launcher.setDefaultCommand(this.launcher.tuneMechanisms()); configureBindings(); } diff --git a/src/main/java/org/robolancers321/commands/Mate.java b/src/main/java/org/robolancers321/commands/Mate.java index 3e47cc5..964286b 100644 --- a/src/main/java/org/robolancers321/commands/Mate.java +++ b/src/main/java/org/robolancers321/commands/Mate.java @@ -9,10 +9,13 @@ import org.robolancers321.subsystems.launcher.Launcher; public class Mate extends SequentialCommandGroup { - private final Intake intake; - private final Launcher launcher; + private Intake intake; + private Launcher launcher; public Mate(DoubleSupplier distanceSupplier) { + // TODO + /* + this.intake = Intake.getInstance(); this.launcher = Launcher.getInstance(); @@ -22,5 +25,7 @@ public Mate(DoubleSupplier distanceSupplier) { new ParallelRaceGroup(this.intake.sucker.out(), this.launcher.acceptHandoff())); addRequirements(this.intake, this.launcher); + + */ } } diff --git a/src/main/java/org/robolancers321/subsystems/intake/Intake.java b/src/main/java/org/robolancers321/subsystems/intake/Intake.java index a64b1bf..f41e8ea 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Intake.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Intake.java @@ -20,14 +20,14 @@ public static Intake getInstance() { * Constants */ - // TODO: beam break + // TODO: implement beam break /* * Implementation */ - public Retractor retractor; - public Sucker sucker; + public final Retractor retractor; + public final Sucker sucker; private Intake() { this.retractor = Retractor.getInstance(); diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java index e6c8e78..09fd29d 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -56,8 +56,8 @@ public enum RetractorPosition { private final double angle; - RetractorPosition(double angle) { - this.angle = angle; + RetractorPosition(double angleDeg) { + this.angle = angleDeg; } public double getAngle() { @@ -72,7 +72,7 @@ public double getAngle() { private final CANSparkMax motor; private final AbsoluteEncoder encoder; private final ProfiledPIDController feedbackController; - private ArmFeedforward feedforwardController; + private ArmFeedforward feedforwardController; // TODO: make this final when not tuning private Retractor() { this.motor = new CANSparkMax(kMotorPort, MotorType.kBrushless); @@ -95,7 +95,6 @@ private void configureMotor() { } private void configureEncoder() { - this.encoder.setInverted(kInvertMotor); this.encoder.setPositionConversionFactor(360); // TODO: potentially a gear ratio on this } @@ -116,66 +115,73 @@ public boolean isAtGoal() { return this.feedbackController.atGoal(); } - private void setGoal(double goal) { - this.feedbackController.setGoal(goal); + private void setGoal(double goalDeg) { + this.feedbackController.setGoal(goalDeg * Math.PI / 180.0); } private void setGoal(RetractorPosition goal) { this.setGoal(goal.getAngle()); } - private void useController() { + private void useControllers() { State setpointState = this.feedbackController.getSetpoint(); double feedforwardOutput = this.feedforwardController.calculate(setpointState.position, setpointState.velocity); + + SmartDashboard.putNumber("retractor ff output", feedforwardOutput); + double feedbackOutput = this.feedbackController.calculate(this.getPosition()); + SmartDashboard.putNumber("retractor fb output", feedbackOutput); + double controllerOutput = feedforwardOutput + feedbackOutput; + SmartDashboard.putNumber("retractor controller output", controllerOutput); + this.motor.set(controllerOutput); } private void doSendables() { - SmartDashboard.putNumber("current position (deg)", this.getPosition()); SmartDashboard.putBoolean("retractor at goal", this.isAtGoal()); + SmartDashboard.putNumber("retractor position (deg)", this.getPosition()); } @Override public void periodic() { - useController(); + useControllers(); doSendables(); } private void initTuning() { - SmartDashboard.putNumber("retractor kP", SmartDashboard.getNumber("retractor kP", kP)); - SmartDashboard.putNumber("retractor kI", SmartDashboard.getNumber("retractor kI", kI)); - SmartDashboard.putNumber("retractor kD", SmartDashboard.getNumber("retractor kD", kD)); + SmartDashboard.putNumber("retractor kp", SmartDashboard.getNumber("retractor kp", kP)); + SmartDashboard.putNumber("retractor ki", SmartDashboard.getNumber("retractor ki", kI)); + SmartDashboard.putNumber("retractor kd", SmartDashboard.getNumber("retractor kd", kD)); - SmartDashboard.putNumber("retractor kS", SmartDashboard.getNumber("retractor kS", kS)); - SmartDashboard.putNumber("retractor kV", SmartDashboard.getNumber("retractor kV", kV)); - SmartDashboard.putNumber("retractor kG", SmartDashboard.getNumber("retractor kG", kG)); + SmartDashboard.putNumber("retractor ks", SmartDashboard.getNumber("retractor ks", kS)); + SmartDashboard.putNumber("retractor kv", SmartDashboard.getNumber("retractor kv", kV)); + SmartDashboard.putNumber("retractor kg", SmartDashboard.getNumber("retractor kg", kG)); - SmartDashboard.putNumber("target retractor position", this.getPosition()); + SmartDashboard.putNumber("retractor target position (deg)", this.getPosition()); } private void tune() { double tunedP = SmartDashboard.getNumber("retractor kp", kP); - double tunedI = SmartDashboard.getNumber("retractor kI", kI); - double tunedD = SmartDashboard.getNumber("retractor kD", kD); + double tunedI = SmartDashboard.getNumber("retractor ki", kI); + double tunedD = SmartDashboard.getNumber("retractor kd", kD); this.feedbackController.setP(tunedP); this.feedbackController.setI(tunedI); this.feedbackController.setD(tunedD); - double tunedS = SmartDashboard.getNumber("retractor kS", kS); - double tunedV = SmartDashboard.getNumber("retractor kV", kV); - double tunedG = SmartDashboard.getNumber("retractor kG", kG); + double tunedS = SmartDashboard.getNumber("retractor ks", kS); + double tunedV = SmartDashboard.getNumber("retractor kv", kV); + double tunedG = SmartDashboard.getNumber("retractor kg", kG); this.feedforwardController = new ArmFeedforward(tunedS, tunedG, tunedV); double targetRetractorPosition = - SmartDashboard.getNumber("target retractor position", this.getPosition()); + SmartDashboard.getNumber("retractor target position (deg)", this.getPosition()); this.setGoal(targetRetractorPosition); } diff --git a/src/main/java/org/robolancers321/subsystems/intake/Sucker.java b/src/main/java/org/robolancers321/subsystems/intake/Sucker.java index 4cb7629..a41c25d 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Sucker.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Sucker.java @@ -32,10 +32,7 @@ public static Sucker getInstance() { private static final boolean kInvertMotor = false; private static final int kCurrentLimit = 20; - private static final double kP = 0.0; - private static final double kI = 0.0; - private static final double kD = 0.0; - private static final double kFF = 0.01; + private static final double kFF = 0.00; private static final double kInRPM = -2000; private static final double kOutRPM = 1000; @@ -67,13 +64,13 @@ private void configureMotor() { } private void configureEncoder() { - this.encoder.setVelocityConversionFactor(1); + this.encoder.setVelocityConversionFactor(1.0); } private void configureController() { - this.controller.setP(kP); - this.controller.setI(kI); - this.controller.setD(kD); + this.controller.setP(0.0); + this.controller.setI(0.0); + this.controller.setD(0.0); this.controller.setFF(kFF); } @@ -86,7 +83,7 @@ private void useController(double desiredRPM) { } private void doSendables() { - SmartDashboard.putNumber("sucker velocity rpm", this.getVelocityRPM()); + SmartDashboard.putNumber("sucker rpm", this.getVelocityRPM()); } @Override @@ -95,26 +92,16 @@ public void periodic() { } private void initTuning() { - SmartDashboard.putNumber("sucker kP", SmartDashboard.getNumber("sucker kP", kP)); - SmartDashboard.putNumber("sucker kI", SmartDashboard.getNumber("sucker kI", kI)); - SmartDashboard.putNumber("sucker kD", SmartDashboard.getNumber("sucker kD", kD)); - SmartDashboard.putNumber("sucker kFF", SmartDashboard.getNumber("sucker kFF", kFF)); - - SmartDashboard.putNumber("target sucker rpm", 0.0); + SmartDashboard.putNumber("sucker kff", SmartDashboard.getNumber("sucker kff", kFF)); + SmartDashboard.putNumber("sucker target rpm", 0.0); } private void tune() { - double tunedP = SmartDashboard.getNumber("sucker kP", kP); - double tunedI = SmartDashboard.getNumber("sucker kI", kI); - double tunedD = SmartDashboard.getNumber("sucker kD", kD); - double tunedFF = SmartDashboard.getNumber("sucker kFF", kFF); - - this.controller.setP(tunedP); - this.controller.setI(tunedI); - this.controller.setD(tunedD); + double tunedFF = SmartDashboard.getNumber("sucker kff", kFF); + this.controller.setFF(tunedFF); - double targetRPM = SmartDashboard.getNumber("target sucker rpm", 0.0); + double targetRPM = SmartDashboard.getNumber("sucker target rpm", 0.0); this.useController(targetRPM); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java index 96d4df9..cf13600 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java @@ -28,11 +28,12 @@ public static Flywheel getInstance() { private static final boolean kInvertMotor = false; private static final int kCurrentLimit = 40; - private static final double kRampUpRate = 0.5; + // TODO + // private static final double kRampUpRate = 0.5; - private static final double kFF = 0.0; + private static final double kFF = 0.000152; - private final double kErrorTolerance = 0.0; + private static final double kErrorTolerance = 0.0; /* * Implementation @@ -42,7 +43,8 @@ public static Flywheel getInstance() { private final RelativeEncoder encoder; private final SparkPIDController controller; - private final SlewRateLimiter limiter; + // TODO + // private final SlewRateLimiter limiter; private double goalRPM = 0.0; @@ -53,7 +55,8 @@ private Flywheel() { this.controller = this.motor.getPIDController(); - this.limiter = new SlewRateLimiter(kRampUpRate); + // TODO + // this.limiter = new SlewRateLimiter(kRampUpRate); this.configureMotor(); this.configureEncoder(); @@ -87,7 +90,7 @@ private double getRPM() { private void useController() { this.controller.setReference(this.goalRPM, ControlType.kVelocity); - // For tuning, no limiter + // TODO // this.controller.setReference( // this.limiter.calculate(this.goalRPM), CANSparkBase.ControlType.kVelocity); } @@ -110,17 +113,16 @@ public void periodic() { } private void initTuning() { - SmartDashboard.putNumber("shooter kff", SmartDashboard.getNumber("shooter kff", kFF)); - - SmartDashboard.putNumber("target shooter rpm", 0.0); + SmartDashboard.putNumber("flywheel kff", SmartDashboard.getNumber("flywheel kff", kFF)); + SmartDashboard.putNumber("flywheel target rpm", 0.0); } private void tune() { - double tunedFF = SmartDashboard.getNumber("shooter kff", kFF); + double tunedFF = SmartDashboard.getNumber("flywheel kff", kFF); this.controller.setFF(tunedFF); - this.goalRPM = SmartDashboard.getNumber("target shooter rpm", 0.0); + this.goalRPM = SmartDashboard.getNumber("flywheel target rpm", 0.0); this.useController(); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index 087d217..8cf2bdc 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -39,9 +39,6 @@ public static Indexer getInstance() { private static final boolean kInvertMotor = false; private static final int kCurrentLimit = 20; - private static final double kP = 0; - private static final double kI = 0; - private static final double kD = 0; private static final double kFF = 0; private static final double kHandoffRPM = 1500; @@ -79,13 +76,13 @@ private void configureMotor() { } private void configureEncoder() { - this.encoder.setInverted(kInvertMotor); + this.encoder.setVelocityConversionFactor(1.0); } private void configureController() { - this.controller.setP(kP); - this.controller.setI(kI); - this.controller.setD(kD); + this.controller.setP(0.0); + this.controller.setI(0.0); + this.controller.setD(0.0); this.controller.setFF(kFF); } @@ -108,7 +105,7 @@ private void setRPM(double rpm) { private void doSendables() { SmartDashboard.putNumber("indexer rpm", this.getRPM()); - SmartDashboard.putBoolean("note detected", this.jawnDetected()); + SmartDashboard.putBoolean("indexer detected note", this.jawnDetected()); } @Override @@ -117,22 +114,18 @@ public void periodic() { } private void initTuning() { - SmartDashboard.putNumber("indexer kp", SmartDashboard.getNumber("indexer kp", kP)); - SmartDashboard.putNumber("indexer kp", SmartDashboard.getNumber("indexer ki", kI)); - SmartDashboard.putNumber("indexer kp", SmartDashboard.getNumber("indexer kd", kD)); SmartDashboard.putNumber("indexer kff", SmartDashboard.getNumber("indexer kff", kFF)); + SmartDashboard.putNumber("indexer target rpm", 0.0); } private void tune() { - double tunedP = SmartDashboard.getNumber("indexer kp", kP); - double tunedI = SmartDashboard.getNumber("indexer ki", kI); - double tunedD = SmartDashboard.getNumber("indexer kd", kD); double tunedFF = SmartDashboard.getNumber("indexer kff", kFF); - this.controller.setP(tunedP); - this.controller.setI(tunedI); - this.controller.setD(tunedD); this.controller.setFF(tunedFF); + + double targetRPM = SmartDashboard.getNumber("indexer target rpm", 0.0); + + this.setRPM(targetRPM); } public Command manualIndex(DoubleSupplier appliedSpeedSupplier) { diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java index a37ce45..e6086e8 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java @@ -30,34 +30,55 @@ public static Launcher getInstance() { * Constants */ - // private static final int kBeamBreakPort = 0; // TODO + // TODO + // private static final int kBeamBreakPort = 0; - private static final double kDebounceTime = 0.05; + // TODO + // private static final double kDebounceTime = 0.05; /* * Implementation */ - public Pivot pivot; - public Indexer indexer; - public Flywheel flywheel; + public final Pivot pivot; + public final Indexer indexer; + public final Flywheel flywheel; - // private DigitalInput beamBreak; // TODO - private Debouncer beamBreakDebouncer; + // TODO + // private final DigitalInput beamBreak; - private AimTable aimTable; + // TODO + // private final Debouncer beamBreakDebouncer; + + // TODO + // private final AimTable aimTable; private Launcher() { this.pivot = Pivot.getInstance(); this.indexer = Indexer.getInstance(); this.flywheel = Flywheel.getInstance(); - // this.beamBreak = new DigitalInput(kBeamBreakPort); // TODO - this.beamBreakDebouncer = new Debouncer(kDebounceTime, Debouncer.DebounceType.kBoth); + // TODO + // this.beamBreak = new DigitalInput(kBeamBreakPort); + + // TODO + // this.beamBreakDebouncer = new Debouncer(kDebounceTime, Debouncer.DebounceType.kBoth); + + // TODO + // this.aimTable = new AimTable(); + } - this.aimTable = new AimTable(); + public Command tuneMechanisms(){ + return new ParallelCommandGroup( + this.pivot.tuneControllers(), + this.indexer.tuneController(), + this.flywheel.tuneController() + ); } + // TODO + /* + public boolean getBeamBreakState() { return true; // return beamBreakDebouncer.calculate(beamBreak.get()); // TODO @@ -90,4 +111,6 @@ public Command yeetSpeaker(DoubleSupplier distanceSupplier) { indexer.outtake(this::getBeamBreakState), new WaitCommand(0.2))))); } + + */ } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index ecf9140..0397170 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -38,14 +38,14 @@ public static Pivot getInstance() { private static final double kGearRatio = 360.0; private static final double kRotPerMinToDegPerSec = kGearRatio / 60.0; - private static final float kMinAngle = -30.0f; - private static final float kMaxAngle = 90.0f; + private static final float kMinAngle = -5.0f; + private static final float kMaxAngle = 85.0f; private static final double kS = 0.0; private static final double kG = 0.0; private static final double kV = 0.0; - private static final double kP = 0.0; + private static final double kP = 0.11; private static final double kI = 0.0; private static final double kD = 0.0; private static final double kMaxVelocityDeg = 0.0; @@ -71,7 +71,7 @@ public enum PivotSetpoint { private final CANSparkMax motor; private final AbsoluteEncoder absoluteEncoder; - private ArmFeedforward feedforwardController; + private ArmFeedforward feedforwardController; // TODO: make this final when not tuning private Pivot() { super( @@ -86,6 +86,8 @@ private Pivot() { this.configureEncoder(); this.configureController(); this.motor.burnFlash(); + + this.m_enabled = true; } private void configureMotor() { @@ -107,6 +109,7 @@ private void configureEncoder() { private void configureController() { super.m_controller.setTolerance(kToleranceDeg); + super.m_controller.enableContinuousInput(0.0, 360.0); } @Override @@ -129,22 +132,30 @@ private boolean atGoal() { @Override protected void useOutput(double output, TrapezoidProfile.State setpoint) { double feedforwardOutput = - this.feedforwardController.calculate(setpoint.position * Math.PI / 180, setpoint.velocity * Math.PI / 180); + this.feedforwardController.calculate(setpoint.position, setpoint.velocity); + + SmartDashboard.putNumber("pivot ff output", feedforwardOutput); double feedbackOutput = super.m_controller.calculate(this.getPositionDeg()); + SmartDashboard.putNumber("pivot fb output", feedbackOutput); + double controllerOutput = feedforwardOutput + feedbackOutput; + SmartDashboard.putNumber("pivot controller output", controllerOutput); + this.motor.set(controllerOutput); } public void doSendables() { - SmartDashboard.putBoolean("Pivot At Goal", this.atGoal()); - SmartDashboard.putNumber("Pivot Position (deg)", this.getPositionDeg()); + SmartDashboard.putBoolean("pivot at goal", this.atGoal()); + SmartDashboard.putNumber("pivot position (deg)", this.getPositionDeg()); } @Override public void periodic() { + super.periodic(); + this.doSendables(); } @@ -157,7 +168,7 @@ private void initTuning() { SmartDashboard.putNumber("pivot kg", SmartDashboard.getNumber("pivot kg", kG)); SmartDashboard.putNumber("pivot kv", SmartDashboard.getNumber("pivot kv", kV)); - SmartDashboard.putNumber("target pivot position", this.getPositionDeg()); + SmartDashboard.putNumber("pivot target position (deg)", this.getPositionDeg()); } private void tune() { @@ -173,15 +184,15 @@ private void tune() { this.feedforwardController = new ArmFeedforward(tunedS, tunedG, tunedV); - this.setGoal(SmartDashboard.getNumber("target pivot position", this.getPositionDeg())); + this.setGoal(SmartDashboard.getNumber("pivot target position (deg)", this.getPositionDeg()) * Math.PI / 180.0); } - private Command moveToAngle(DoubleSupplier angleSupplier) { - return run(() -> this.setGoal(angleSupplier.getAsDouble())).until(this::atGoal); + private Command moveToAngle(DoubleSupplier angleDegSupplier) { + return run(() -> this.setGoal(angleDegSupplier.getAsDouble() * Math.PI / 180.0)).until(this::atGoal); } - private Command moveToAngle(double angle) { - return this.moveToAngle(() -> angle); + private Command moveToAngle(double angleDeg) { + return this.moveToAngle(() -> angleDeg); } public Command moveToRetracted() { @@ -196,8 +207,8 @@ public Command aimAtAmp() { return this.moveToAngle(AimTable.kAmpAimCharacteristic.angle); } - public Command aimAtSpeaker(DoubleSupplier angleSupplier) { - return this.moveToAngle(angleSupplier); + public Command aimAtSpeaker(DoubleSupplier angleDegSupplier) { + return this.moveToAngle(angleDegSupplier); } public Command tuneControllers() { diff --git a/src/test/java/org/robolancers321/AimTableTest.java b/src/test/java/org/robolancers321/AimTableTest.java index cd274fe..674a70d 100644 --- a/src/test/java/org/robolancers321/AimTableTest.java +++ b/src/test/java/org/robolancers321/AimTableTest.java @@ -12,8 +12,6 @@ public class AimTableTest { - - private static final LinkedHashMap table = new LinkedHashMap<>() { { From 6841d1ad3744f362aefa05cbeb1dc8d173f87d56 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Thu, 15 Feb 2024 09:45:37 -0500 Subject: [PATCH 53/57] retractor is tuned, potentially will change based on gearing --- .../org/robolancers321/RobotContainer.java | 2 +- .../subsystems/launcher/Launcher.java | 16 ++++--------- .../subsystems/launcher/Pivot.java | 23 ++++++++++++------- 3 files changed, 20 insertions(+), 21 deletions(-) diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index ff5c742..a034626 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -18,7 +18,7 @@ public RobotContainer() { this.launcher = Launcher.getInstance(); - this.launcher.setDefaultCommand(this.launcher.tuneMechanisms()); + this.launcher.pivot.setDefaultCommand(this.launcher.pivot.tuneControllers()); configureBindings(); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java index e6086e8..fe75066 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java @@ -41,8 +41,8 @@ public static Launcher getInstance() { */ public final Pivot pivot; - public final Indexer indexer; - public final Flywheel flywheel; + // public final Indexer indexer; + // public final Flywheel flywheel; // TODO // private final DigitalInput beamBreak; @@ -55,8 +55,8 @@ public static Launcher getInstance() { private Launcher() { this.pivot = Pivot.getInstance(); - this.indexer = Indexer.getInstance(); - this.flywheel = Flywheel.getInstance(); + // this.indexer = Indexer.getInstance(); + // this.flywheel = Flywheel.getInstance(); // TODO // this.beamBreak = new DigitalInput(kBeamBreakPort); @@ -68,14 +68,6 @@ private Launcher() { // this.aimTable = new AimTable(); } - public Command tuneMechanisms(){ - return new ParallelCommandGroup( - this.pivot.tuneControllers(), - this.indexer.tuneController(), - this.flywheel.tuneController() - ); - } - // TODO /* diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 0397170..501f7c4 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -5,6 +5,8 @@ import com.revrobotics.*; import com.revrobotics.SparkAbsoluteEncoder.Type; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -39,17 +41,17 @@ public static Pivot getInstance() { private static final double kRotPerMinToDegPerSec = kGearRatio / 60.0; private static final float kMinAngle = -5.0f; - private static final float kMaxAngle = 85.0f; + private static final float kMaxAngle = 90.0f; private static final double kS = 0.0; private static final double kG = 0.0; private static final double kV = 0.0; - private static final double kP = 0.11; + private static final double kP = 0.04; private static final double kI = 0.0; - private static final double kD = 0.0; - private static final double kMaxVelocityDeg = 0.0; - private static final double kMaxAccelerationDeg = 0.0; + private static final double kD = 0.02; + private static final double kMaxVelocityDeg = 140; + private static final double kMaxAccelerationDeg = 140; private static final double kToleranceDeg = 0.0; @@ -132,7 +134,12 @@ private boolean atGoal() { @Override protected void useOutput(double output, TrapezoidProfile.State setpoint) { double feedforwardOutput = - this.feedforwardController.calculate(setpoint.position, setpoint.velocity); + this.feedforwardController.calculate(setpoint.position * Math.PI / 180.0, setpoint.velocity * Math.PI / 180.0); + + SmartDashboard.putNumber("pivot position setpoint mp (deg)", setpoint.position); + SmartDashboard.putNumber("pivot velocity setpoint mp (deg)", setpoint.velocity); + + SmartDashboard.putNumber("pivot position setpoint error (deg)", setpoint.position - this.getPositionDeg()); SmartDashboard.putNumber("pivot ff output", feedforwardOutput); @@ -184,11 +191,11 @@ private void tune() { this.feedforwardController = new ArmFeedforward(tunedS, tunedG, tunedV); - this.setGoal(SmartDashboard.getNumber("pivot target position (deg)", this.getPositionDeg()) * Math.PI / 180.0); + this.setGoal(MathUtil.clamp(SmartDashboard.getNumber("pivot target position (deg)", this.getPositionDeg()), kMinAngle, kMaxAngle)); } private Command moveToAngle(DoubleSupplier angleDegSupplier) { - return run(() -> this.setGoal(angleDegSupplier.getAsDouble() * Math.PI / 180.0)).until(this::atGoal); + return run(() -> this.setGoal(MathUtil.clamp(angleDegSupplier.getAsDouble(), kMinAngle, kMaxAngle))).until(this::atGoal); } private Command moveToAngle(double angleDeg) { From 2fa30fe869cabecaae866b0db9e5afbf50fe7265 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Thu, 15 Feb 2024 19:48:20 -0500 Subject: [PATCH 54/57] pivot is tuned, maybe adjust motion profile speed and acceleration --- .../subsystems/launcher/Pivot.java | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 501f7c4..c90c242 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -41,7 +41,7 @@ public static Pivot getInstance() { private static final double kRotPerMinToDegPerSec = kGearRatio / 60.0; private static final float kMinAngle = -5.0f; - private static final float kMaxAngle = 90.0f; + private static final float kMaxAngle = 85.0f; private static final double kS = 0.0; private static final double kG = 0.0; @@ -50,10 +50,10 @@ public static Pivot getInstance() { private static final double kP = 0.04; private static final double kI = 0.0; private static final double kD = 0.02; - private static final double kMaxVelocityDeg = 140; - private static final double kMaxAccelerationDeg = 140; + private static final double kMaxVelocityDeg = 240; + private static final double kMaxAccelerationDeg = 360; - private static final double kToleranceDeg = 0.0; + private static final double kToleranceDeg = 0.5; public enum PivotSetpoint { kRetracted(0.0), @@ -73,7 +73,7 @@ public enum PivotSetpoint { private final CANSparkMax motor; private final AbsoluteEncoder absoluteEncoder; - private ArmFeedforward feedforwardController; // TODO: make this final when not tuning + private ArmFeedforward feedforwardController; private Pivot() { super( @@ -88,8 +88,6 @@ private Pivot() { this.configureEncoder(); this.configureController(); this.motor.burnFlash(); - - this.m_enabled = true; } private void configureMotor() { @@ -110,8 +108,11 @@ private void configureEncoder() { } private void configureController() { - super.m_controller.setTolerance(kToleranceDeg); super.m_controller.enableContinuousInput(0.0, 360.0); + super.m_controller.setTolerance(kToleranceDeg); + super.m_controller.setGoal(this.getPositionDeg()); + + this.m_enabled = true; } @Override @@ -139,8 +140,6 @@ protected void useOutput(double output, TrapezoidProfile.State setpoint) { SmartDashboard.putNumber("pivot position setpoint mp (deg)", setpoint.position); SmartDashboard.putNumber("pivot velocity setpoint mp (deg)", setpoint.velocity); - SmartDashboard.putNumber("pivot position setpoint error (deg)", setpoint.position - this.getPositionDeg()); - SmartDashboard.putNumber("pivot ff output", feedforwardOutput); double feedbackOutput = super.m_controller.calculate(this.getPositionDeg()); From 4ea99f4102de1821c9d44e4f4c22eb318f459a69 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Fri, 16 Feb 2024 09:40:41 -0500 Subject: [PATCH 55/57] made pivot and retractor use the same conventions, refactored some stuff, changes ready to be tested and tuned --- .../subsystems/intake/Intake.java | 8 +- .../subsystems/intake/Retractor.java | 144 ++++++++++-------- .../subsystems/launcher/AimTable.java | 16 +- .../subsystems/launcher/Flywheel.java | 16 +- .../subsystems/launcher/Launcher.java | 1 + .../subsystems/launcher/Pivot.java | 38 +++-- 6 files changed, 130 insertions(+), 93 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/intake/Intake.java b/src/main/java/org/robolancers321/subsystems/intake/Intake.java index f41e8ea..95c3835 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Intake.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Intake.java @@ -27,10 +27,14 @@ public static Intake getInstance() { */ public final Retractor retractor; - public final Sucker sucker; + + // TODO + // public final Sucker sucker; private Intake() { this.retractor = Retractor.getInstance(); - this.sucker = Sucker.getInstance(); + + // TODO + // this.sucker = Sucker.getInstance(); } } diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java index 09fd29d..4fe76ee 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -1,20 +1,23 @@ /* (C) Robolancers 2024 */ package org.robolancers321.subsystems.intake; +import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; + import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder.Type; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; -public class Retractor extends SubsystemBase { +public class Retractor extends ProfiledPIDSubsystem { /* * Singleton */ @@ -33,8 +36,14 @@ public static Retractor getInstance() { private static final int kMotorPort = 13; - private static final boolean kInvertMotor = true; - private static final int kCurrentLimit = 20; + private static final boolean kInvertMotor = false; + private static final int kCurrentLimit = 30; + + private static final double kGearRatio = 360.0; + private static final double kRotPerMinToDegPerSec = kGearRatio / 60.0; + + private static final float kMinAngle = 0.0f; + private static final float kMaxAngle = 180.0f; private static final double kP = 0.000; private static final double kI = 0.000; @@ -44,25 +53,21 @@ public static Retractor getInstance() { private static final double kG = 0.000; private static final double kV = 0.000; - private static final double kMaxVelocityDeg = 0.0; - private static final double kMaxAccelerationDeg = 0.0; + private static final double kMaxVelocityDeg = 30.0; + private static final double kMaxAccelerationDeg = 45.0; - private static final double kErrorThreshold = 2.0; + private static final double kToleranceDeg = 0.5; - public enum RetractorPosition { + private enum RetractorSetpoint { kRetracted(0), kMating(0), kIntake(0); - private final double angle; + public final double angle; - RetractorPosition(double angleDeg) { + private RetractorSetpoint(double angleDeg) { this.angle = angleDeg; } - - public double getAngle() { - return this.angle; - } } /* @@ -71,15 +76,16 @@ public double getAngle() { private final CANSparkMax motor; private final AbsoluteEncoder encoder; - private final ProfiledPIDController feedbackController; - private ArmFeedforward feedforwardController; // TODO: make this final when not tuning + private ArmFeedforward feedforwardController; private Retractor() { - this.motor = new CANSparkMax(kMotorPort, MotorType.kBrushless); - this.encoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); - this.feedbackController = + super( new ProfiledPIDController( - kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocityDeg, kMaxAccelerationDeg)); + kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocityDeg, kMaxAccelerationDeg))); + + this.motor = new CANSparkMax(kMotorPort, kBrushless); + this.encoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); + this.feedforwardController = new ArmFeedforward(kS, kG, kV); this.configureMotor(); this.configureEncoder(); @@ -89,49 +95,57 @@ private Retractor() { private void configureMotor() { this.motor.setInverted(kInvertMotor); - this.motor.setIdleMode(CANSparkMax.IdleMode.kBrake); + this.motor.setIdleMode(CANSparkBase.IdleMode.kBrake); this.motor.setSmartCurrentLimit(kCurrentLimit); this.motor.enableVoltageCompensation(12); + + this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, (float) kMaxAngle); + this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, (float) kMinAngle); + this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); + this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); } private void configureEncoder() { - this.encoder.setPositionConversionFactor(360); // TODO: potentially a gear ratio on this + this.encoder.setPositionConversionFactor(kGearRatio); + this.encoder.setVelocityConversionFactor(kRotPerMinToDegPerSec); } private void configureController() { - this.feedbackController.setP(kP); - this.feedbackController.setI(kI); - this.feedbackController.setD(kD); - this.feedbackController.setTolerance(kErrorThreshold); + super.m_controller.enableContinuousInput(0.0, 360.0); + super.m_controller.setTolerance(kToleranceDeg); + super.m_controller.setGoal(this.getPositionDeg()); - this.feedforwardController = new ArmFeedforward(kS, kG, kV); + this.m_enabled = true; } - - public double getPosition() { - return this.encoder.getPosition(); + + @Override + public double getMeasurement() { + return this.getPositionDeg(); } - public boolean isAtGoal() { - return this.feedbackController.atGoal(); + public double getPositionDeg() { + return this.encoder.getPosition(); } - private void setGoal(double goalDeg) { - this.feedbackController.setGoal(goalDeg * Math.PI / 180.0); + public double getVelocityDeg() { + return this.encoder.getVelocity(); } - private void setGoal(RetractorPosition goal) { - this.setGoal(goal.getAngle()); + private boolean atGoal() { + return super.m_controller.atGoal(); } - private void useControllers() { - State setpointState = this.feedbackController.getSetpoint(); - + @Override + protected void useOutput(double output, TrapezoidProfile.State setpoint) { double feedforwardOutput = - this.feedforwardController.calculate(setpointState.position, setpointState.velocity); + this.feedforwardController.calculate(setpoint.position * Math.PI / 180.0, setpoint.velocity * Math.PI / 180.0); + + SmartDashboard.putNumber("retractor position setpoint mp (deg)", setpoint.position); + SmartDashboard.putNumber("retractor velocity setpoint mp (deg)", setpoint.velocity); SmartDashboard.putNumber("retractor ff output", feedforwardOutput); - double feedbackOutput = this.feedbackController.calculate(this.getPosition()); + double feedbackOutput = super.m_controller.calculate(this.getPositionDeg()); SmartDashboard.putNumber("retractor fb output", feedbackOutput); @@ -143,14 +157,15 @@ private void useControllers() { } private void doSendables() { - SmartDashboard.putBoolean("retractor at goal", this.isAtGoal()); - SmartDashboard.putNumber("retractor position (deg)", this.getPosition()); + SmartDashboard.putBoolean("retractor at goal", this.atGoal()); + SmartDashboard.putNumber("retractor position (deg)", this.getPositionDeg()); } - + @Override public void periodic() { - useControllers(); - doSendables(); + super.periodic(); + + this.doSendables(); } private void initTuning() { @@ -162,7 +177,10 @@ private void initTuning() { SmartDashboard.putNumber("retractor kv", SmartDashboard.getNumber("retractor kv", kV)); SmartDashboard.putNumber("retractor kg", SmartDashboard.getNumber("retractor kg", kG)); - SmartDashboard.putNumber("retractor target position (deg)", this.getPosition()); + SmartDashboard.putNumber("retractor max vel (deg)", SmartDashboard.getNumber("retractor max vel (deg)", kMaxVelocityDeg)); + SmartDashboard.putNumber("retractor max acc (deg)", SmartDashboard.getNumber("retractor max acc (deg)", kMaxAccelerationDeg)); + + SmartDashboard.putNumber("retractor target position (deg)", this.getPositionDeg()); } private void tune() { @@ -170,9 +188,7 @@ private void tune() { double tunedI = SmartDashboard.getNumber("retractor ki", kI); double tunedD = SmartDashboard.getNumber("retractor kd", kD); - this.feedbackController.setP(tunedP); - this.feedbackController.setI(tunedI); - this.feedbackController.setD(tunedD); + super.m_controller.setPID(tunedP, tunedI, tunedD); double tunedS = SmartDashboard.getNumber("retractor ks", kS); double tunedV = SmartDashboard.getNumber("retractor kv", kV); @@ -180,28 +196,28 @@ private void tune() { this.feedforwardController = new ArmFeedforward(tunedS, tunedG, tunedV); - double targetRetractorPosition = - SmartDashboard.getNumber("retractor target position (deg)", this.getPosition()); + double tunedMaxVel = SmartDashboard.getNumber("retractor max vel (deg)", kMaxVelocityDeg); + double tunedMaxAcc = SmartDashboard.getNumber("retractor max acc (deg)", kMaxAccelerationDeg); - this.setGoal(targetRetractorPosition); - } - - private Command moveToPosition(RetractorPosition position) { - this.setGoal(position); + this.m_controller.setConstraints(new Constraints(tunedMaxVel, tunedMaxAcc)); - return new WaitUntilCommand(this::isAtGoal); + this.setGoal(MathUtil.clamp(SmartDashboard.getNumber("retractor target position (deg)", this.getPositionDeg()), kMinAngle, kMaxAngle)); + } + + private Command moveToAngle(double angleDeg) { + return run(() -> this.setGoal(MathUtil.clamp(angleDeg, kMinAngle, kMaxAngle))).until(this::atGoal); } public Command moveToRetracted() { - return this.moveToPosition(RetractorPosition.kRetracted); + return this.moveToAngle(RetractorSetpoint.kRetracted.angle); } public Command moveToMating() { - return this.moveToPosition(RetractorPosition.kMating); + return this.moveToAngle(RetractorSetpoint.kMating.angle); } public Command moveToIntake() { - return this.moveToPosition(RetractorPosition.kIntake); + return this.moveToAngle(RetractorSetpoint.kIntake.angle); } public Command tuneControllers() { diff --git a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java index e4dfa9d..cb11f6c 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java @@ -19,12 +19,6 @@ public AimCharacteristic(double angle, double rpm) { } } - // TODO: tune - public static final AimCharacteristic kRetractedAimCharacteristic = - new AimCharacteristic(0.0, 0.0); - public static final AimCharacteristic kMatingAimCharacteristic = new AimCharacteristic(0.0, 0.0); - public static final AimCharacteristic kAmpAimCharacteristic = new AimCharacteristic(0.0, 0.0); - // TODO: tune private static final LinkedHashMap table = new LinkedHashMap<>() { @@ -41,18 +35,20 @@ private static double interpolate( return lowKey + percent * (highValue - lowValue); } - private static final double kInterpolationCacheThreshold = 0.0; + private static final double kInterpolationCacheThreshold = 0.0; - private static AimCharacteristic calculateSpeakerAimCharacteristic(double distance) { + private static AimCharacteristic calculateSpeakerAimCharacteristic(double distance) { List keys = table.keySet().stream().sorted().toList(); double lowerBound = keys.get(0); double upperBound = keys.get(keys.size() - 1); - if ((distance < lowerBound)) { + if (distance < lowerBound){ AimTable.AimCharacteristic lowerValue = table.get(lowerBound); return new AimTable.AimCharacteristic(lowerValue.angle, lowerValue.rpm); - } else if (distance > upperBound) { + } + + if (distance > upperBound) { AimTable.AimCharacteristic upperValue = table.get(upperBound); return new AimTable.AimCharacteristic(upperValue.angle, upperValue.rpm); diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java index cf13600..744df0e 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java @@ -33,7 +33,17 @@ public static Flywheel getInstance() { private static final double kFF = 0.000152; - private static final double kErrorTolerance = 0.0; + private static final double kToleranceRPM = 40.0; + + private enum FlywheelSetpoint { + kAmp(0.0); + + public final double rpm; + + private FlywheelSetpoint(double rpm){ + this.rpm = rpm; + } + } /* * Implementation @@ -96,7 +106,7 @@ private void useController() { } public boolean isRevved() { - return epsilonEquals(this.getRPM(), this.goalRPM, kErrorTolerance); + return epsilonEquals(this.getRPM(), this.goalRPM, kToleranceRPM); } private void dangerouslySetSpeed(double speed) { @@ -141,7 +151,7 @@ private Command off() { } public Command yeetNoteAmp() { - this.goalRPM = AimTable.kAmpAimCharacteristic.rpm; + this.goalRPM = FlywheelSetpoint.kAmp.rpm; return run(this::useController).finallyDo(this::off); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java index fe75066..dce5f6d 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java @@ -80,6 +80,7 @@ public Command acceptHandoff() { return this.indexer.acceptHandoff(this::getBeamBreakState); } + // TODO: race? public Command yeetAmp() { return new SequentialCommandGroup( pivot.aimAtAmp(), diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index c90c242..3a72480 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; @@ -43,26 +44,27 @@ public static Pivot getInstance() { private static final float kMinAngle = -5.0f; private static final float kMaxAngle = 85.0f; + private static final double kP = 0.04; + private static final double kI = 0.0; + private static final double kD = 0.02; + private static final double kS = 0.0; private static final double kG = 0.0; private static final double kV = 0.0; - private static final double kP = 0.04; - private static final double kI = 0.0; - private static final double kD = 0.02; private static final double kMaxVelocityDeg = 240; private static final double kMaxAccelerationDeg = 360; private static final double kToleranceDeg = 0.5; - public enum PivotSetpoint { + private enum PivotSetpoint { kRetracted(0.0), kMating(0.0), kAmp(0.0); public final double angle; - PivotSetpoint(double angle) { + private PivotSetpoint(double angle) { this.angle = angle; } } @@ -72,7 +74,7 @@ public enum PivotSetpoint { */ private final CANSparkMax motor; - private final AbsoluteEncoder absoluteEncoder; + private final AbsoluteEncoder encoder; private ArmFeedforward feedforwardController; private Pivot() { @@ -81,7 +83,7 @@ private Pivot() { kP, kI, kD, new TrapezoidProfile.Constraints(kMaxVelocityDeg, kMaxAccelerationDeg))); this.motor = new CANSparkMax(kMotorPort, kBrushless); - this.absoluteEncoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); + this.encoder = this.motor.getAbsoluteEncoder(Type.kDutyCycle); this.feedforwardController = new ArmFeedforward(kS, kG, kV); this.configureMotor(); @@ -103,8 +105,8 @@ private void configureMotor() { } private void configureEncoder() { - this.absoluteEncoder.setPositionConversionFactor(kGearRatio); - this.absoluteEncoder.setVelocityConversionFactor(kRotPerMinToDegPerSec); + this.encoder.setPositionConversionFactor(kGearRatio); + this.encoder.setVelocityConversionFactor(kRotPerMinToDegPerSec); } private void configureController() { @@ -121,11 +123,11 @@ public double getMeasurement() { } public double getPositionDeg() { - return this.absoluteEncoder.getPosition(); + return this.encoder.getPosition(); } public double getVelocityDeg() { - return this.absoluteEncoder.getVelocity(); + return this.encoder.getVelocity(); } private boolean atGoal() { @@ -174,6 +176,9 @@ private void initTuning() { SmartDashboard.putNumber("pivot kg", SmartDashboard.getNumber("pivot kg", kG)); SmartDashboard.putNumber("pivot kv", SmartDashboard.getNumber("pivot kv", kV)); + SmartDashboard.putNumber("pivot max vel (deg)", SmartDashboard.getNumber("pivot max vel (deg)", kMaxVelocityDeg)); + SmartDashboard.putNumber("pivot max acc (deg)", SmartDashboard.getNumber("pivot max acc (deg)", kMaxAccelerationDeg)); + SmartDashboard.putNumber("pivot target position (deg)", this.getPositionDeg()); } @@ -190,6 +195,11 @@ private void tune() { this.feedforwardController = new ArmFeedforward(tunedS, tunedG, tunedV); + double tunedMaxVel = SmartDashboard.getNumber("pivot max vel (deg)", kMaxVelocityDeg); + double tunedMaxAcc = SmartDashboard.getNumber("pivot max acc (deg)", kMaxAccelerationDeg); + + this.m_controller.setConstraints(new Constraints(tunedMaxVel, tunedMaxAcc)); + this.setGoal(MathUtil.clamp(SmartDashboard.getNumber("pivot target position (deg)", this.getPositionDeg()), kMinAngle, kMaxAngle)); } @@ -202,15 +212,15 @@ private Command moveToAngle(double angleDeg) { } public Command moveToRetracted() { - return this.moveToAngle(AimTable.kRetractedAimCharacteristic.angle); + return this.moveToAngle(PivotSetpoint.kRetracted.angle); } public Command moveToMating() { - return this.moveToAngle(AimTable.kMatingAimCharacteristic.angle); + return this.moveToAngle(PivotSetpoint.kMating.angle); } public Command aimAtAmp() { - return this.moveToAngle(AimTable.kAmpAimCharacteristic.angle); + return this.moveToAngle(PivotSetpoint.kAmp.angle); } public Command aimAtSpeaker(DoubleSupplier angleDegSupplier) { From 9027f513fd50130877a28ac9b20ffb3c4089a5ca Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Fri, 16 Feb 2024 15:35:37 -0500 Subject: [PATCH 56/57] config stuff --- .../org/robolancers321/subsystems/intake/Retractor.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java index 4fe76ee..81725ec 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -36,21 +36,21 @@ public static Retractor getInstance() { private static final int kMotorPort = 13; - private static final boolean kInvertMotor = false; + private static final boolean kInvertMotor = true; private static final int kCurrentLimit = 30; private static final double kGearRatio = 360.0; private static final double kRotPerMinToDegPerSec = kGearRatio / 60.0; - private static final float kMinAngle = 0.0f; - private static final float kMaxAngle = 180.0f; + private static final float kMinAngle = -15.0f; + private static final float kMaxAngle = 168.0f; private static final double kP = 0.000; private static final double kI = 0.000; private static final double kD = 0.000; private static final double kS = 0.000; - private static final double kG = 0.000; + private static final double kG = 0.0441; private static final double kV = 0.000; private static final double kMaxVelocityDeg = 30.0; From f96c9ea612753934d1420faca79eed22a1b7f00a Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Fri, 16 Feb 2024 18:04:12 -0500 Subject: [PATCH 57/57] both arms work and velocity control is ready to be tuned --- .../org/robolancers321/RobotContainer.java | 10 ++--- .../subsystems/intake/Retractor.java | 44 ++++++++++++------- .../subsystems/launcher/Launcher.java | 4 +- .../subsystems/launcher/Pivot.java | 30 ++++++++----- 4 files changed, 52 insertions(+), 36 deletions(-) diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index a034626..3bb9cd1 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -7,18 +7,16 @@ import org.robolancers321.subsystems.launcher.Launcher; public class RobotContainer { - // TODO - // Intake intake; - + Intake intake; Launcher launcher; public RobotContainer() { - // TODO - // this.intake = Intake.getInstance(); - + this.intake = Intake.getInstance(); this.launcher = Launcher.getInstance(); + this.intake.retractor.setDefaultCommand(this.intake.retractor.tuneControllers()); this.launcher.pivot.setDefaultCommand(this.launcher.pivot.tuneControllers()); + this.launcher.flywheel.setDefaultCommand(this.launcher.flywheel.tuneController()); configureBindings(); } diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java index 81725ec..7c1f086 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -5,6 +5,7 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkBase.SoftLimitDirection; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder.Type; @@ -37,26 +38,27 @@ public static Retractor getInstance() { private static final int kMotorPort = 13; private static final boolean kInvertMotor = true; - private static final int kCurrentLimit = 30; + private static final boolean kInvertEncoder = true; + + private static final int kCurrentLimit = 40; private static final double kGearRatio = 360.0; - private static final double kRotPerMinToDegPerSec = kGearRatio / 60.0; - private static final float kMinAngle = -15.0f; - private static final float kMaxAngle = 168.0f; + private static final float kMinAngle = -20.0f; + private static final float kMaxAngle = 160.0f; - private static final double kP = 0.000; + private static final double kP = 0.0065; private static final double kI = 0.000; - private static final double kD = 0.000; + private static final double kD = 0.0001; private static final double kS = 0.000; - private static final double kG = 0.0441; + private static final double kG = 0.0155; private static final double kV = 0.000; - private static final double kMaxVelocityDeg = 30.0; - private static final double kMaxAccelerationDeg = 45.0; + private static final double kMaxVelocityDeg = 90.0; + private static final double kMaxAccelerationDeg = 60.0; - private static final double kToleranceDeg = 0.5; + private static final double kToleranceDeg = 2.5; private enum RetractorSetpoint { kRetracted(0), @@ -99,19 +101,26 @@ private void configureMotor() { this.motor.setSmartCurrentLimit(kCurrentLimit); this.motor.enableVoltageCompensation(12); - this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, (float) kMaxAngle); - this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, (float) kMinAngle); - this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); - this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); + // this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, (float) kMaxAngle); + // this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, (float) kMinAngle); + // this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); + // this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); + + this.motor.enableSoftLimit(SoftLimitDirection.kForward, false); + this.motor.enableSoftLimit(SoftLimitDirection.kReverse, false); } private void configureEncoder() { + // this.motor.getEncoder().setPositionConversionFactor(kGearRatio); + // this.motor.getEncoder().setPosition(this.getPositionDeg()); + + this.encoder.setInverted(kInvertEncoder); this.encoder.setPositionConversionFactor(kGearRatio); - this.encoder.setVelocityConversionFactor(kRotPerMinToDegPerSec); + this.encoder.setVelocityConversionFactor(kGearRatio); } private void configureController() { - super.m_controller.enableContinuousInput(0.0, 360.0); + super.m_controller.enableContinuousInput(-180.0, 180.0); super.m_controller.setTolerance(kToleranceDeg); super.m_controller.setGoal(this.getPositionDeg()); @@ -124,7 +133,7 @@ public double getMeasurement() { } public double getPositionDeg() { - return this.encoder.getPosition(); + return this.encoder.getPosition() > 180 ? this.encoder.getPosition() - 360.0 : this.encoder.getPosition(); } public double getVelocityDeg() { @@ -159,6 +168,7 @@ protected void useOutput(double output, TrapezoidProfile.State setpoint) { private void doSendables() { SmartDashboard.putBoolean("retractor at goal", this.atGoal()); SmartDashboard.putNumber("retractor position (deg)", this.getPositionDeg()); + SmartDashboard.putNumber("retractor velocity (deg)", this.getVelocityDeg()); } @Override diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java index dce5f6d..6d4f18a 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java @@ -42,7 +42,7 @@ public static Launcher getInstance() { public final Pivot pivot; // public final Indexer indexer; - // public final Flywheel flywheel; + public final Flywheel flywheel; // TODO // private final DigitalInput beamBreak; @@ -56,7 +56,7 @@ public static Launcher getInstance() { private Launcher() { this.pivot = Pivot.getInstance(); // this.indexer = Indexer.getInstance(); - // this.flywheel = Flywheel.getInstance(); + this.flywheel = Flywheel.getInstance(); // TODO // this.beamBreak = new DigitalInput(kBeamBreakPort); diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 3a72480..6094587 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -4,6 +4,7 @@ import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; import com.revrobotics.*; +import com.revrobotics.CANSparkBase.SoftLimitDirection; import com.revrobotics.SparkAbsoluteEncoder.Type; import edu.wpi.first.math.MathUtil; @@ -36,13 +37,13 @@ public static Pivot getInstance() { private static final int kMotorPort = 15; private static final boolean kInvertMotor = false; + private static final boolean kInvertEncoder = false; private static final int kCurrentLimit = 40; private static final double kGearRatio = 360.0; - private static final double kRotPerMinToDegPerSec = kGearRatio / 60.0; - private static final float kMinAngle = -5.0f; - private static final float kMaxAngle = 85.0f; + private static final float kMinAngle = 0.0f; + private static final float kMaxAngle = 90.0f; private static final double kP = 0.04; private static final double kI = 0.0; @@ -55,7 +56,7 @@ public static Pivot getInstance() { private static final double kMaxVelocityDeg = 240; private static final double kMaxAccelerationDeg = 360; - private static final double kToleranceDeg = 0.5; + private static final double kToleranceDeg = 2.5; private enum PivotSetpoint { kRetracted(0.0), @@ -98,15 +99,22 @@ private void configureMotor() { this.motor.setSmartCurrentLimit(kCurrentLimit); this.motor.enableVoltageCompensation(12); - this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, (float) kMaxAngle); - this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, (float) kMinAngle); - this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); - this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); + // this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, (float) kMaxAngle); + // this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, (float) kMinAngle); + // this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); + // this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); + + this.motor.enableSoftLimit(SoftLimitDirection.kForward, false); + this.motor.enableSoftLimit(SoftLimitDirection.kReverse, false); } private void configureEncoder() { + // this.motor.getEncoder().setPositionConversionFactor(kGearRatio); + // this.motor.getEncoder().setPosition(this.getPositionDeg()); + + this.encoder.setInverted(kInvertEncoder); this.encoder.setPositionConversionFactor(kGearRatio); - this.encoder.setVelocityConversionFactor(kRotPerMinToDegPerSec); + this.encoder.setVelocityConversionFactor(kGearRatio); } private void configureController() { @@ -121,9 +129,9 @@ private void configureController() { public double getMeasurement() { return this.getPositionDeg(); } - + public double getPositionDeg() { - return this.encoder.getPosition(); + return this.encoder.getPosition() > 180 ? this.encoder.getPosition() - 360.0 : this.encoder.getPosition(); } public double getVelocityDeg() {