diff --git a/build.gradle b/build.gradle index c4ef2db..0bd6441 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) @@ -68,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' } @@ -104,14 +106,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 { @@ -156,12 +160,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/gradlew b/gradlew old mode 100644 new mode 100755 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/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/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 index c52be06..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 = 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 DIRTY = 1; + 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-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 55b784c..3bb9cd1 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -3,9 +3,21 @@ 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 { + Intake intake; + Launcher launcher; + public RobotContainer() { + 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/commands/Mate.java b/src/main/java/org/robolancers321/commands/Mate.java new file mode 100644 index 0000000..964286b --- /dev/null +++ b/src/main/java/org/robolancers321/commands/Mate.java @@ -0,0 +1,31 @@ +/* (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 Intake intake; + private Launcher launcher; + + public Mate(DoubleSupplier distanceSupplier) { + // TODO + /* + + 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/subsystems/intake/Intake.java b/src/main/java/org/robolancers321/subsystems/intake/Intake.java new file mode 100644 index 0000000..95c3835 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/intake/Intake.java @@ -0,0 +1,40 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase { + /* + * Singleton + */ + + private static Intake instance = null; + + public static Intake getInstance() { + if (instance == null) instance = new Intake(); + + return instance; + } + + /* + * Constants + */ + + // TODO: implement beam break + + /* + * Implementation + */ + + public final Retractor retractor; + + // TODO + // public final Sucker sucker; + + private Intake() { + this.retractor = Retractor.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 new file mode 100644 index 0000000..7c1f086 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -0,0 +1,238 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.intake; + +import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkBase.SoftLimitDirection; +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.Constraints; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; + +public class Retractor extends ProfiledPIDSubsystem { + /* + * Singleton + */ + + private static Retractor instance = null; + + public static Retractor getInstance() { + if (instance == null) instance = new Retractor(); + + return instance; + } + + /* + * Constants + */ + + private static final int kMotorPort = 13; + + private static final boolean kInvertMotor = true; + private static final boolean kInvertEncoder = true; + + private static final int kCurrentLimit = 40; + + private static final double kGearRatio = 360.0; + + private static final float kMinAngle = -20.0f; + private static final float kMaxAngle = 160.0f; + + private static final double kP = 0.0065; + private static final double kI = 0.000; + private static final double kD = 0.0001; + + private static final double kS = 0.000; + private static final double kG = 0.0155; + private static final double kV = 0.000; + + private static final double kMaxVelocityDeg = 90.0; + private static final double kMaxAccelerationDeg = 60.0; + + private static final double kToleranceDeg = 2.5; + + private enum RetractorSetpoint { + kRetracted(0), + kMating(0), + kIntake(0); + + public final double angle; + + private RetractorSetpoint(double angleDeg) { + this.angle = angleDeg; + } + } + + /* + * Implementation + */ + + private final CANSparkMax motor; + private final AbsoluteEncoder encoder; + private ArmFeedforward feedforwardController; + + private Retractor() { + super( + new ProfiledPIDController( + 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(); + this.configureController(); + this.motor.burnFlash(); + } + + 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); + + 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(kGearRatio); + } + + private void configureController() { + super.m_controller.enableContinuousInput(-180.0, 180.0); + super.m_controller.setTolerance(kToleranceDeg); + super.m_controller.setGoal(this.getPositionDeg()); + + this.m_enabled = true; + } + + @Override + public double getMeasurement() { + return this.getPositionDeg(); + } + + public double getPositionDeg() { + return this.encoder.getPosition() > 180 ? this.encoder.getPosition() - 360.0 : this.encoder.getPosition(); + } + + public double getVelocityDeg() { + return this.encoder.getVelocity(); + } + + private boolean atGoal() { + return super.m_controller.atGoal(); + } + + @Override + protected void useOutput(double output, TrapezoidProfile.State setpoint) { + double feedforwardOutput = + 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 = super.m_controller.calculate(this.getPositionDeg()); + + SmartDashboard.putNumber("retractor fb output", feedbackOutput); + + double controllerOutput = feedforwardOutput + feedbackOutput; + + SmartDashboard.putNumber("retractor controller output", controllerOutput); + + this.motor.set(controllerOutput); + } + + 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 + public void periodic() { + super.periodic(); + + this.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 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 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() { + double tunedP = SmartDashboard.getNumber("retractor kp", kP); + double tunedI = SmartDashboard.getNumber("retractor ki", kI); + double tunedD = SmartDashboard.getNumber("retractor kd", kD); + + super.m_controller.setPID(tunedP, tunedI, 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 tunedMaxVel = SmartDashboard.getNumber("retractor max vel (deg)", kMaxVelocityDeg); + double tunedMaxAcc = SmartDashboard.getNumber("retractor max acc (deg)", kMaxAccelerationDeg); + + this.m_controller.setConstraints(new Constraints(tunedMaxVel, tunedMaxAcc)); + + 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.moveToAngle(RetractorSetpoint.kRetracted.angle); + } + + public Command moveToMating() { + return this.moveToAngle(RetractorSetpoint.kMating.angle); + } + + public Command moveToIntake() { + return this.moveToAngle(RetractorSetpoint.kIntake.angle); + } + + public Command tuneControllers() { + this.initTuning(); + + return run(this::tune); + } +} diff --git a/src/main/java/org/robolancers321/subsystems/intake/Sucker.java b/src/main/java/org/robolancers321/subsystems/intake/Sucker.java new file mode 100644 index 0000000..a41c25d --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/intake/Sucker.java @@ -0,0 +1,122 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.intake; + +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +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 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 = 14; + + private static final boolean kInvertMotor = false; + private static final int kCurrentLimit = 20; + + private static final double kFF = 0.00; + + private static final double kInRPM = -2000; + private static final double kOutRPM = 1000; + + /* + * Implementation + */ + + private final CANSparkMax motor; + private final RelativeEncoder encoder; + private final 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(); + this.motor.burnFlash(); + } + + private void configureMotor() { + this.motor.setInverted(kInvertMotor); + this.motor.setIdleMode(CANSparkMax.IdleMode.kBrake); + this.motor.setSmartCurrentLimit(kCurrentLimit); + this.motor.enableVoltageCompensation(12); + } + + private void configureEncoder() { + this.encoder.setVelocityConversionFactor(1.0); + } + + private void configureController() { + this.controller.setP(0.0); + this.controller.setI(0.0); + this.controller.setD(0.0); + this.controller.setFF(kFF); + } + + public double getVelocityRPM() { + return this.encoder.getVelocity(); + } + + private void useController(double desiredRPM) { + this.controller.setReference(desiredRPM, ControlType.kVelocity); + } + + private void doSendables() { + SmartDashboard.putNumber("sucker rpm", this.getVelocityRPM()); + } + + @Override + public void periodic() { + this.doSendables(); + } + + private void initTuning() { + SmartDashboard.putNumber("sucker kff", SmartDashboard.getNumber("sucker kff", kFF)); + SmartDashboard.putNumber("sucker target rpm", 0.0); + } + + private void tune() { + double tunedFF = SmartDashboard.getNumber("sucker kff", kFF); + + this.controller.setFF(tunedFF); + + double targetRPM = SmartDashboard.getNumber("sucker target rpm", 0.0); + + this.useController(targetRPM); + } + + public Command in() { + return run(() -> this.useController(kInRPM)).finallyDo(() -> this.useController(0.0)); + } + + public Command out() { + return run(() -> this.useController(kOutRPM)).finallyDo(() -> this.useController(0.0)); + } + + public Command tuneController() { + initTuning(); + + return run(this::tune); + } +} 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..cb11f6c --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java @@ -0,0 +1,84 @@ +/* (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; + public final double rpm; + + public AimCharacteristic(double angle, double rpm) { + this.angle = angle; + this.rpm = rpm; + } + } + + // TODO: tune + private static final 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) { + double percent = (x - lowKey) / (highKey - lowKey); + + return lowKey + percent * (highValue - lowValue); + } + + private static final double kInterpolationCacheThreshold = 0.0; + + 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){ + AimTable.AimCharacteristic lowerValue = table.get(lowerBound); + + return new AimTable.AimCharacteristic(lowerValue.angle, lowerValue.rpm); + } + + 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); + 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)); + } + + 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); + } + } + + public AimCharacteristic getSpeakerAimCharacteristic() { + return this.lastAimCharacteristic; + } +} 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..744df0e --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java @@ -0,0 +1,172 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.launcher; + +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; +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 kMotorPort = 17; + + private static final boolean kInvertMotor = false; + private static final int kCurrentLimit = 40; + + // TODO + // private static final double kRampUpRate = 0.5; + + private static final double kFF = 0.000152; + + 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 + */ + + private final CANSparkFlex motor; + private final RelativeEncoder encoder; + private final SparkPIDController controller; + + // TODO + // private final SlewRateLimiter limiter; + + private double goalRPM = 0.0; + + private Flywheel() { + this.motor = new CANSparkFlex(kMotorPort, CANSparkLowLevel.MotorType.kBrushless); + + this.encoder = this.motor.getEncoder(); + + this.controller = this.motor.getPIDController(); + + // TODO + // this.limiter = new SlewRateLimiter(kRampUpRate); + + this.configureMotor(); + this.configureEncoder(); + this.configureController(); + this.motor.burnFlash(); + } + + private void configureMotor() { + this.motor.setInverted(kInvertMotor); + this.motor.setIdleMode(CANSparkBase.IdleMode.kBrake); + this.motor.setSmartCurrentLimit(kCurrentLimit); + this.motor.enableVoltageCompensation(12); + } + + private void configureEncoder() { + this.encoder.setVelocityConversionFactor(1.0); + } + + private void configureController() { + this.controller.setP(0.0); + this.controller.setI(0.0); + this.controller.setD(0.0); + this.controller.setFF(kFF); + } + + private double getRPM() { + // TODO: filter here? + return this.encoder.getVelocity(); + } + + private void useController() { + this.controller.setReference(this.goalRPM, ControlType.kVelocity); + + // TODO + // this.controller.setReference( + // this.limiter.calculate(this.goalRPM), CANSparkBase.ControlType.kVelocity); + } + + public boolean isRevved() { + return epsilonEquals(this.getRPM(), this.goalRPM, kToleranceRPM); + } + + private void dangerouslySetSpeed(double speed) { + this.motor.set(speed); + } + + private void doSendables() { + SmartDashboard.putNumber("flywheel rpm", this.getRPM()); + } + + @Override + public void periodic() { + this.doSendables(); + } + + private void initTuning() { + SmartDashboard.putNumber("flywheel kff", SmartDashboard.getNumber("flywheel kff", kFF)); + SmartDashboard.putNumber("flywheel target rpm", 0.0); + } + + private void tune() { + double tunedFF = SmartDashboard.getNumber("flywheel kff", kFF); + + this.controller.setFF(tunedFF); + + this.goalRPM = SmartDashboard.getNumber("flywheel target rpm", 0.0); + + this.useController(); + } + + 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.useController(); + }); + } + + public Command yeetNoteAmp() { + this.goalRPM = FlywheelSetpoint.kAmp.rpm; + + return run(this::useController).finallyDo(this::off); + } + + public Command yeetNoteSpeaker(DoubleSupplier rpmSupplier) { + return run(() -> { + this.goalRPM = rpmSupplier.getAsDouble(); + this.useController(); + }) + .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 new file mode 100644 index 0000000..8cf2bdc --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -0,0 +1,172 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.launcher; + +import static com.revrobotics.CANSparkLowLevel.MotorType.kBrushless; + +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 { + /* + * Singleton + */ + + private static Indexer instance = null; + + public static Indexer getInstance() { + if (instance == null) instance = new Indexer(); + + return instance; + } + + /* + * Constants + */ + + 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; + + private static final double kFF = 0; + + private static final double kHandoffRPM = 1500; + private static final double kReindexRPM = 500; + private static final double kOuttakeRPM = 5000; + + /* + * Implementation + */ + + private final CANSparkFlex motor; + private final SparkPIDController controller; + private final RelativeEncoder encoder; + + // private final DigitalInput beamBreak; // TODO + + private Indexer() { + this.motor = new CANSparkFlex(kMotorPort, kBrushless); + this.encoder = this.motor.getEncoder(); + this.controller = this.motor.getPIDController(); + + // this.beamBreak = new DigitalInput(kBeamBreakPort); // TODO + + this.configureMotor(); + this.configureEncoder(); + this.configureController(); + this.motor.burnFlash(); + } + + private void configureMotor() { + this.motor.setInverted(kInvertMotor); + this.motor.setIdleMode(CANSparkBase.IdleMode.kBrake); + this.motor.setSmartCurrentLimit(kCurrentLimit); + this.motor.enableVoltageCompensation(12); + } + + private void configureEncoder() { + this.encoder.setVelocityConversionFactor(1.0); + } + + private void configureController() { + this.controller.setP(0.0); + this.controller.setI(0.0); + this.controller.setD(0.0); + this.controller.setFF(kFF); + } + + public double getRPM() { + return this.encoder.getVelocity(); + } + + public boolean jawnDetected() { + return true; + // return this.beamBreak.get(); // TODO + } + + private void dangerouslySetSpeed(double speed) { + this.motor.set(speed); + } + + private void setRPM(double rpm) { + this.controller.setReference(rpm, ControlType.kVelocity); + } + + private void doSendables() { + SmartDashboard.putNumber("indexer rpm", this.getRPM()); + SmartDashboard.putBoolean("indexer detected note", this.jawnDetected()); + } + + @Override + public void periodic() { + this.doSendables(); + } + + private void initTuning() { + SmartDashboard.putNumber("indexer kff", SmartDashboard.getNumber("indexer kff", kFF)); + SmartDashboard.putNumber("indexer target rpm", 0.0); + } + + private void tune() { + double tunedFF = SmartDashboard.getNumber("indexer kff", kFF); + + this.controller.setFF(tunedFF); + + double targetRPM = SmartDashboard.getNumber("indexer target rpm", 0.0); + + this.setRPM(targetRPM); + } + + 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(BooleanSupplier beamBreakStateSupplier) { + return run(() -> this.setRPM(kHandoffRPM)).until(beamBreakStateSupplier).finallyDo(this::off); + } + + public Command shiftIntoPosition(BooleanSupplier beamBreakStateSupplier) { + return new SequentialCommandGroup( + run(() -> this.setRPM(kReindexRPM)).until(beamBreakStateSupplier), + run(() -> this.setRPM(-kReindexRPM)) + .until(() -> !beamBreakStateSupplier.getAsBoolean())) + .finallyDo(this::off); + } + + public Command outtake(BooleanSupplier beamBreakStateSupplier) { + return new ParallelRaceGroup( + run(() -> this.setRPM(kOuttakeRPM)), + new SequentialCommandGroup( + new WaitUntilCommand(beamBreakStateSupplier), + new WaitUntilCommand(() -> !beamBreakStateSupplier.getAsBoolean())), + new WaitCommand( + 0.4) // 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..6d4f18a --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java @@ -0,0 +1,109 @@ +/* (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; +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; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import java.util.function.DoubleSupplier; + +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 + // private static final int kBeamBreakPort = 0; + + // TODO + // private static final double kDebounceTime = 0.05; + + /* + * Implementation + */ + + public final Pivot pivot; + // public final Indexer indexer; + public final Flywheel flywheel; + + // TODO + // private final DigitalInput beamBreak; + + // TODO + // private final Debouncer beamBreakDebouncer; + + // TODO + // private final AimTable aimTable; + + private Launcher() { + this.pivot = Pivot.getInstance(); + // this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + + // TODO + // this.beamBreak = new DigitalInput(kBeamBreakPort); + + // TODO + // this.beamBreakDebouncer = new Debouncer(kDebounceTime, Debouncer.DebounceType.kBoth); + + // TODO + // this.aimTable = new AimTable(); + } + + // TODO + /* + + public boolean getBeamBreakState() { + return true; + // return beamBreakDebouncer.calculate(beamBreak.get()); // TODO + } + + public Command acceptHandoff() { + return this.indexer.acceptHandoff(this::getBeamBreakState); + } + + // TODO: race? + public Command yeetAmp() { + return new SequentialCommandGroup( + pivot.aimAtAmp(), + new ParallelCommandGroup( + flywheel.yeetNoteAmp(), + indexer.outtake(this::getBeamBreakState), + new WaitCommand(0.2))); + } + + 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))))); + } + + */ +} 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..6094587 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -0,0 +1,243 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.launcher; + +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; +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; +import java.util.function.DoubleSupplier; + +public class Pivot extends ProfiledPIDSubsystem { + /* + * Singleton + */ + + private static Pivot instance = null; + + public static Pivot getInstance() { + if (instance == null) instance = new Pivot(); + + return instance; + } + + /* + * Constants + */ + + 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 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; + 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 kMaxVelocityDeg = 240; + private static final double kMaxAccelerationDeg = 360; + + private static final double kToleranceDeg = 2.5; + + private enum PivotSetpoint { + kRetracted(0.0), + kMating(0.0), + kAmp(0.0); + + public final double angle; + + private PivotSetpoint(double angle) { + this.angle = angle; + } + } + + /* + * Implementation + */ + + private final CANSparkMax motor; + private final AbsoluteEncoder encoder; + private ArmFeedforward feedforwardController; + + private Pivot() { + super( + new ProfiledPIDController( + 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(); + this.configureController(); + this.motor.burnFlash(); + } + + 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); + + 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(kGearRatio); + } + + private void configureController() { + super.m_controller.enableContinuousInput(0.0, 360.0); + super.m_controller.setTolerance(kToleranceDeg); + super.m_controller.setGoal(this.getPositionDeg()); + + this.m_enabled = true; + } + + @Override + public double getMeasurement() { + return this.getPositionDeg(); + } + + public double getPositionDeg() { + return this.encoder.getPosition() > 180 ? this.encoder.getPosition() - 360.0 : this.encoder.getPosition(); + } + + public double getVelocityDeg() { + return this.encoder.getVelocity(); + } + + private boolean atGoal() { + return super.m_controller.atGoal(); + } + + @Override + protected void useOutput(double output, TrapezoidProfile.State setpoint) { + double feedforwardOutput = + 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 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()); + } + + @Override + public void periodic() { + super.periodic(); + + this.doSendables(); + } + + 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 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()); + } + + 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); + + 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)); + } + + private Command moveToAngle(DoubleSupplier angleDegSupplier) { + return run(() -> this.setGoal(MathUtil.clamp(angleDegSupplier.getAsDouble(), kMinAngle, kMaxAngle))).until(this::atGoal); + } + + private Command moveToAngle(double angleDeg) { + return this.moveToAngle(() -> angleDeg); + } + + public Command moveToRetracted() { + return this.moveToAngle(PivotSetpoint.kRetracted.angle); + } + + public Command moveToMating() { + return this.moveToAngle(PivotSetpoint.kMating.angle); + } + + public Command aimAtAmp() { + return this.moveToAngle(PivotSetpoint.kAmp.angle); + } + + public Command aimAtSpeaker(DoubleSupplier angleDegSupplier) { + return this.moveToAngle(angleDegSupplier); + } + + public Command tuneControllers() { + this.initTuning(); + + return run(this::tune); + } +} 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..921dd1b --- /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; + } +} diff --git a/src/test/java/org/robolancers321/AimTableTest.java b/src/test/java/org/robolancers321/AimTableTest.java new file mode 100644 index 0000000..674a70d --- /dev/null +++ b/src/test/java/org/robolancers321/AimTableTest.java @@ -0,0 +1,169 @@ +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); + } + } + + +} 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