From f244384724854b733e0b584397f3ca3d63cd5175 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 25 Jan 2026 12:50:14 -0500 Subject: [PATCH 01/11] add flywheel and hood IO --- src/main/java/frc/robot/Robot.java | 24 +++++------ .../robot/subsystems/launcher/FlywheelIO.java | 19 ++++++++ .../frc/robot/subsystems/launcher/HoodIO.java | 21 +++++++++ .../Turret.java => launcher/Launcher.java} | 18 ++++---- .../launcher/LauncherConstants.java | 43 +++++++++++++++++++ .../robot/subsystems/launcher/TurretIO.java | 21 +++++++++ .../{turret => launcher}/TurretIOSim.java | 18 ++++---- .../{turret => launcher}/TurretIOSpark.java | 18 ++++---- .../subsystems/turret/TurretConstants.java | 40 ----------------- .../frc/robot/subsystems/turret/TurretIO.java | 21 --------- 10 files changed, 143 insertions(+), 100 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java create mode 100644 src/main/java/frc/robot/subsystems/launcher/HoodIO.java rename src/main/java/frc/robot/subsystems/{turret/Turret.java => launcher/Launcher.java} (91%) create mode 100644 src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java create mode 100644 src/main/java/frc/robot/subsystems/launcher/TurretIO.java rename src/main/java/frc/robot/subsystems/{turret => launcher}/TurretIOSim.java (74%) rename src/main/java/frc/robot/subsystems/{turret => launcher}/TurretIOSpark.java (86%) delete mode 100644 src/main/java/frc/robot/subsystems/turret/TurretConstants.java delete mode 100644 src/main/java/frc/robot/subsystems/turret/TurretIO.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9f191ef..1358cd0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -34,10 +34,10 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; -import frc.robot.subsystems.turret.Turret; -import frc.robot.subsystems.turret.TurretIO; -import frc.robot.subsystems.turret.TurretIOSim; -import frc.robot.subsystems.turret.TurretIOSpark; +import frc.robot.subsystems.launcher.Launcher; +import frc.robot.subsystems.launcher.TurretIO; +import frc.robot.subsystems.launcher.TurretIOSim; +import frc.robot.subsystems.launcher.TurretIOSpark; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhotonVision; @@ -66,7 +66,7 @@ public class Robot extends LoggedRobot { // Subsystems private Drive drive; private Vision vision; - private Turret turret; + private Launcher launcher; public Robot() { // Record metadata @@ -110,8 +110,8 @@ public Robot() { new VisionIOPhotonVision(cameraFrontLeftName, robotToFrontLeftCamera), new VisionIOPhotonVision(cameraBackRightName, robotToBackRightCamera), new VisionIOPhotonVision(cameraBackLeftName, robotToBackLeftCamera)); - turret = - new Turret(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSpark()); + launcher = + new Launcher(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSpark()); break; case SIM: // Running a physics simulator @@ -138,8 +138,8 @@ public Robot() { cameraBackRightName, robotToBackRightCamera, drive::getPose), new VisionIOPhotonVisionSim( cameraBackLeftName, robotToBackLeftCamera, drive::getPose)); - turret = - new Turret(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSim()); + launcher = + new Launcher(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSim()); break; case REPLAY: // Replaying a log @@ -166,8 +166,8 @@ public Robot() { new VisionIO() {}, new VisionIO() {}, new VisionIO() {}); - turret = - new Turret(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIO() {}); + launcher = + new Launcher(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIO() {}); break; } @@ -183,7 +183,7 @@ public Robot() { SmartDashboard.putData("Field", field); Field.plotRegions(); - turret.setDefaultCommand(Commands.run(turret::aimAtHub, turret)); + launcher.setDefaultCommand(Commands.run(launcher::aimAtHub, launcher)); } /** This function is called periodically during all modes. */ diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java new file mode 100644 index 0000000..9040566 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.launcher; + +import org.littletonrobotics.junction.AutoLog; + +public interface FlywheelIO { + @AutoLog + public static class FlywheelIOInputs { + public boolean connected = false; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double currentAmps = 0.0; + } + + public default void updateInputs(FlywheelIOInputs inputs) {} + + public default void setOpenLoop(double output) {} + + public default void setVelocity(double velocityRadPerSec, double feedforwardVolts) {} +} diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIO.java b/src/main/java/frc/robot/subsystems/launcher/HoodIO.java new file mode 100644 index 0000000..c06fa3c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIO.java @@ -0,0 +1,21 @@ +package frc.robot.subsystems.launcher; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface HoodIO { + @AutoLog + public static class HoodIOInputs { + public boolean connected = false; + public Rotation2d position = Rotation2d.kZero; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double currentAmps = 0.0; + } + + public default void updateInputs(HoodIOInputs inputs) {} + + public default void setOpenLoop(double output) {} + + public default void setPosition(Rotation2d rotation, double feedforwardVolts) {} +} diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java similarity index 91% rename from src/main/java/frc/robot/subsystems/turret/Turret.java rename to src/main/java/frc/robot/subsystems/launcher/Launcher.java index db144d6..fcbe172 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -1,7 +1,7 @@ -package frc.robot.subsystems.turret; +package frc.robot.subsystems.launcher; import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.turret.TurretConstants.*; +import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,7 +17,7 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class Turret extends SubsystemBase { +public class Launcher extends SubsystemBase { private final TurretIO io; private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); private final Supplier chassisPoseSupplier; @@ -29,7 +29,7 @@ public class Turret extends SubsystemBase { private Translation2d dynamicTargetToTurretBase = new Translation2d(); private Pose2d target = new Pose2d(); - public Turret( + public Launcher( Supplier chassisPoseSupplier, Supplier chassisSpeedsSupplier, TurretIO io) { @@ -43,11 +43,11 @@ public Turret( public void periodic() { io.updateInputs(inputs); Logger.processInputs("Turret", inputs); - turnDisconnectedAlert.set(!inputs.turnConnected); + turnDisconnectedAlert.set(!inputs.connected); } public void stop() { - io.setTurnOpenLoop(0.0); + io.setOpenLoop(0.0); } public void aimAtHub() { @@ -100,7 +100,7 @@ public void aimAtHub() { + apparentAngularVelocityRadPerSec) / turnMaxAngularVelocity.in(RadiansPerSecond); - io.setTurnPosition(turretOrientationSetpoint, feedforwardVolts); + io.setPosition(turretOrientationSetpoint, feedforwardVolts); } @AutoLogOutput(key = "Turret/Pose") @@ -108,7 +108,7 @@ public Pose2d getTurretPose() { return chassisPoseSupplier .get() .plus(chassisToTurretBase) - .plus(new Transform2d(0, 0, inputs.turnPosition)); + .plus(new Transform2d(0, 0, inputs.position)); } @AutoLogOutput(key = "Turret/Target") @@ -118,7 +118,7 @@ public Pose2d getTargetPose() { @AutoLogOutput(key = "Turret/IsOnTarget") public boolean isOnTarget() { - return inputs.turnPosition.minus(turretOrientationSetpoint).getMeasure().abs(Radians) + return inputs.position.minus(turretOrientationSetpoint).getMeasure().abs(Radians) * dynamicTargetToTurretBase.getNorm() < (hubWidth.in(Meters) / 2.0); } diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java new file mode 100644 index 0000000..aa1b89e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.launcher; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import frc.robot.Constants.MotorConstants.NEO550Constants; + +public final class LauncherConstants { + + public static final class TurretConstants { + // Geometry + public static final Transform2d chassisToTurretBase = + new Transform2d(Inches.of(0), Inches.of(10), Rotation2d.kZero); + public static final Rotation2d rotationOffset = new Rotation2d(0.44); + public static final LinearVelocity fuelVelocityRadial = MetersPerSecond.of(5); + + // Absolute encoder + public static final double turnEncoderPositionFactor = 2 * Math.PI; // Radians + public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // Rad/sec + + // Position controller + public static final double turnPIDMinInput = 0.0; + public static final double turnPIDMaxInput = 2 * Math.PI; + public static final double turnKp = 0.35; + public static final Distance hubWidth = Inches.of(41.73); + + // Motor controller + public static final int port = 12; + public static final double turnMotorReduction = 5.0; + public static final AngularVelocity turnMaxAngularVelocity = + NEO550Constants.kFreeSpeed.div(turnMotorReduction); + + // Simulation + public static final DCMotor turnGearbox = DCMotor.getNeo550(1); + public static final double turnKpSim = 0.8; + public static final double turnKdSim = 0.05; + } +} diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java new file mode 100644 index 0000000..30b2c5d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java @@ -0,0 +1,21 @@ +package frc.robot.subsystems.launcher; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface TurretIO { + @AutoLog + public static class TurretIOInputs { + public boolean connected = false; + public Rotation2d position = Rotation2d.kZero; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double currentAmps = 0.0; + } + + public default void updateInputs(TurretIOInputs inputs) {} + + public default void setOpenLoop(double output) {} + + public default void setPosition(Rotation2d rotation, double feedforwardVolts) {} +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOSim.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java similarity index 74% rename from src/main/java/frc/robot/subsystems/turret/TurretIOSim.java rename to src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java index 384544c..a9fa8ba 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java @@ -1,6 +1,6 @@ -package frc.robot.subsystems.turret; +package frc.robot.subsystems.launcher; -import static frc.robot.subsystems.turret.TurretConstants.*; +import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; @@ -41,21 +41,21 @@ public void updateInputs(TurretIOInputs inputs) { turnSim.update(0.02); // Update turn inputs - inputs.turnConnected = true; - inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); - inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); - inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + inputs.connected = true; + inputs.position = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.velocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = turnAppliedVolts; + inputs.currentAmps = Math.abs(turnSim.getCurrentDrawAmps()); } @Override - public void setTurnOpenLoop(double output) { + public void setOpenLoop(double output) { turnClosedLoop = false; turnAppliedVolts = output; } @Override - public void setTurnPosition(Rotation2d rotation, double feedforwardVolts) { + public void setPosition(Rotation2d rotation, double feedforwardVolts) { turnClosedLoop = true; this.feedforwardVolts = feedforwardVolts; turnController.setSetpoint(rotation.getRadians()); diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java similarity index 86% rename from src/main/java/frc/robot/subsystems/turret/TurretIOSpark.java rename to src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index 9428767..682e3fa 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -1,6 +1,6 @@ -package frc.robot.subsystems.turret; +package frc.robot.subsystems.launcher; -import static frc.robot.subsystems.turret.TurretConstants.*; +import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; import static frc.robot.util.SparkUtil.*; import com.revrobotics.AbsoluteEncoder; @@ -81,23 +81,23 @@ public void updateInputs(TurretIOInputs inputs) { ifOk( turnSpark, turnEncoder::getPosition, - (value) -> inputs.turnPosition = new Rotation2d(value).minus(rotationOffset)); - ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value); + (value) -> inputs.position = new Rotation2d(value).minus(rotationOffset)); + ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); ifOk( turnSpark, new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage}, - (values) -> inputs.turnAppliedVolts = values[0] * values[1]); - ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); - inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault); + (values) -> inputs.appliedVolts = values[0] * values[1]); + ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.currentAmps = value); + inputs.connected = turnConnectedDebounce.calculate(!sparkStickyFault); } @Override - public void setTurnOpenLoop(double output) { + public void setOpenLoop(double output) { turnSpark.setVoltage(output); } @Override - public void setTurnPosition(Rotation2d rotation, double feedforwardVolts) { + public void setPosition(Rotation2d rotation, double feedforwardVolts) { double setpoint = MathUtil.inputModulus( rotation.plus(rotationOffset).getRadians(), turnPIDMinInput, turnPIDMaxInput); diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java deleted file mode 100644 index 202d3be..0000000 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.robot.subsystems.turret; - -import static edu.wpi.first.units.Units.*; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; -import frc.robot.Constants.MotorConstants.NEO550Constants; - -public class TurretConstants { - // Geometry - public static final Transform2d chassisToTurretBase = - new Transform2d(Inches.of(0), Inches.of(10), Rotation2d.kZero); - public static final Rotation2d rotationOffset = new Rotation2d(0.44); - public static final LinearVelocity fuelVelocityRadial = MetersPerSecond.of(5); - - // Absolute encoder - public static final double turnEncoderPositionFactor = 2 * Math.PI; // Radians - public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // Rad/sec - - // Position controller - public static final double turnPIDMinInput = 0.0; - public static final double turnPIDMaxInput = 2 * Math.PI; - public static final double turnKp = 0.35; - public static final Distance hubWidth = Inches.of(41.73); - - // Motor controller - public static final int port = 12; - public static final double turnMotorReduction = 5.0; - public static final AngularVelocity turnMaxAngularVelocity = - NEO550Constants.kFreeSpeed.div(turnMotorReduction); - - // Simulation - public static final DCMotor turnGearbox = DCMotor.getNeo550(1); - public static final double turnKpSim = 0.8; - public static final double turnKdSim = 0.05; -} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java deleted file mode 100644 index 2f0a1ed..0000000 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.robot.subsystems.turret; - -import edu.wpi.first.math.geometry.Rotation2d; -import org.littletonrobotics.junction.AutoLog; - -public interface TurretIO { - @AutoLog - public static class TurretIOInputs { - public boolean turnConnected = false; - public Rotation2d turnPosition = Rotation2d.kZero; - public double turnVelocityRadPerSec = 0.0; - public double turnAppliedVolts = 0.0; - public double turnCurrentAmps = 0.0; - } - - public default void updateInputs(TurretIOInputs inputs) {} - - public default void setTurnOpenLoop(double output) {} - - public default void setTurnPosition(Rotation2d rotation, double feedforwardVolts) {} -} From 27e56d6e19a826f9ddc3d60b1d7ac62d3adba818 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 25 Jan 2026 12:59:06 -0500 Subject: [PATCH 02/11] initialize flywheel, hood --- src/main/java/frc/robot/Robot.java | 8 +-- .../robot/subsystems/launcher/Launcher.java | 50 ++++++++++++++----- 2 files changed, 42 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1358cd0..e45a4b3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -34,6 +34,8 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.launcher.FlywheelIO; +import frc.robot.subsystems.launcher.HoodIO; import frc.robot.subsystems.launcher.Launcher; import frc.robot.subsystems.launcher.TurretIO; import frc.robot.subsystems.launcher.TurretIOSim; @@ -111,7 +113,7 @@ public Robot() { new VisionIOPhotonVision(cameraBackRightName, robotToBackRightCamera), new VisionIOPhotonVision(cameraBackLeftName, robotToBackLeftCamera)); launcher = - new Launcher(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSpark()); + new Launcher(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSpark(), new FlywheelIO() {}, new HoodIO() {}); break; case SIM: // Running a physics simulator @@ -139,7 +141,7 @@ public Robot() { new VisionIOPhotonVisionSim( cameraBackLeftName, robotToBackLeftCamera, drive::getPose)); launcher = - new Launcher(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSim()); + new Launcher(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSim(), new FlywheelIO() {}, new HoodIO() {}); break; case REPLAY: // Replaying a log @@ -167,7 +169,7 @@ public Robot() { new VisionIO() {}, new VisionIO() {}); launcher = - new Launcher(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIO() {}); + new Launcher(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIO() {}, new FlywheelIO() {}, new HoodIO() {}); break; } diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index fcbe172..4d6b9aa 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -18,12 +18,20 @@ import org.littletonrobotics.junction.Logger; public class Launcher extends SubsystemBase { - private final TurretIO io; - private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); + private final TurretIO turretIO; + private final FlywheelIO flywheelIO; + private final HoodIO hoodIO; + + private final TurretIOInputsAutoLogged turretInputs = new TurretIOInputsAutoLogged(); + private final FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); + private final HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); + private final Supplier chassisPoseSupplier; private final Supplier chassisSpeedsSupplier; - private final Alert turnDisconnectedAlert; + private final Alert turretDisconnectedAlert; + private final Alert flywheelDisconnectedAlert; + private final Alert hoodDisconnectedAlert; private Rotation2d turretOrientationSetpoint = Rotation2d.kZero; private Translation2d dynamicTargetToTurretBase = new Translation2d(); @@ -32,22 +40,38 @@ public class Launcher extends SubsystemBase { public Launcher( Supplier chassisPoseSupplier, Supplier chassisSpeedsSupplier, - TurretIO io) { - this.io = io; + TurretIO turretIO, FlywheelIO flywheelIO, HoodIO hoodIO) { + this.turretIO = turretIO; + this.flywheelIO = flywheelIO; + this.hoodIO = hoodIO; + this.chassisPoseSupplier = chassisPoseSupplier; this.chassisSpeedsSupplier = chassisSpeedsSupplier; - turnDisconnectedAlert = new Alert("Disconnected turret turn motor", AlertType.kError); + + turretDisconnectedAlert = new Alert("Disconnected turret motor", AlertType.kError); + flywheelDisconnectedAlert = new Alert("Disconnected flywheel motor", AlertType.kError); + hoodDisconnectedAlert = new Alert("Disconnected hood motor", AlertType.kError); } @Override public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Turret", inputs); - turnDisconnectedAlert.set(!inputs.connected); + turretIO.updateInputs(turretInputs); + flywheelIO.updateInputs(flywheelInputs); + hoodIO.updateInputs(hoodInputs); + + Logger.processInputs("Turret", turretInputs); + Logger.processInputs("Flyweel", flywheelInputs); + Logger.processInputs("Hood", hoodInputs); + + turretDisconnectedAlert.set(!turretInputs.connected); + flywheelDisconnectedAlert.set(!flywheelInputs.connected); + hoodDisconnectedAlert.set(!hoodInputs.connected); } public void stop() { - io.setOpenLoop(0.0); + turretIO.setOpenLoop(0.0); + flywheelIO.setOpenLoop(0.0); + hoodIO.setOpenLoop(0.0); } public void aimAtHub() { @@ -100,7 +124,7 @@ public void aimAtHub() { + apparentAngularVelocityRadPerSec) / turnMaxAngularVelocity.in(RadiansPerSecond); - io.setPosition(turretOrientationSetpoint, feedforwardVolts); + turretIO.setPosition(turretOrientationSetpoint, feedforwardVolts); } @AutoLogOutput(key = "Turret/Pose") @@ -108,7 +132,7 @@ public Pose2d getTurretPose() { return chassisPoseSupplier .get() .plus(chassisToTurretBase) - .plus(new Transform2d(0, 0, inputs.position)); + .plus(new Transform2d(0, 0, turretInputs.position)); } @AutoLogOutput(key = "Turret/Target") @@ -118,7 +142,7 @@ public Pose2d getTargetPose() { @AutoLogOutput(key = "Turret/IsOnTarget") public boolean isOnTarget() { - return inputs.position.minus(turretOrientationSetpoint).getMeasure().abs(Radians) + return turretInputs.position.minus(turretOrientationSetpoint).getMeasure().abs(Radians) * dynamicTargetToTurretBase.getNorm() < (hubWidth.in(Meters) / 2.0); } From 1d321874e554594c02f37f760872629f64831947 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 25 Jan 2026 13:18:22 -0500 Subject: [PATCH 03/11] use Pose3d for hubs and turretBase --- src/main/java/frc/game/Field.java | 18 ++++++++++-------- src/main/java/frc/game/GameState.java | 9 ++++----- src/main/java/frc/robot/Robot.java | 4 ++-- .../robot/subsystems/launcher/Launcher.java | 2 +- .../subsystems/launcher/LauncherConstants.java | 7 ++++--- 5 files changed, 21 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/game/Field.java b/src/main/java/frc/game/Field.java index 1e74583..19dccc7 100644 --- a/src/main/java/frc/game/Field.java +++ b/src/main/java/frc/game/Field.java @@ -3,8 +3,10 @@ import static edu.wpi.first.units.Units.*; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Distance; import frc.robot.Robot; @@ -17,6 +19,7 @@ public class Field { private static final Distance hub_x_centerPos = Inches.of(181.56); private static final Distance hub_x_len = Inches.of(47); private static final Distance hub_y_len = Inches.of(47); + private static final Distance hub_z_len = Inches.of(72); private static final Distance trench_y_len = Inches.of(49.86); private static final Distance bump_y_len = Inches.of(73); private static final Distance depot_y_centerPos = centerField_y_pos.plus(Inches.of(75.93)); @@ -36,12 +39,11 @@ public class Field { (centerField_x_pos.minus(hub_x_centerPos.plus(hub_x_len.div(2)))).times(2); private static final Pose2d fieldCenter = new Pose2d(new Translation2d(centerField_x_pos, centerField_y_pos), Rotation2d.kZero); - private static final Pose2d blueHubCenter = - new Pose2d(new Translation2d(hub_x_centerPos, centerField_y_pos), Rotation2d.kZero); - private static final Pose2d redHubCenter = - new Pose2d( - new Translation2d(centerField_x_pos.times(2).minus(hub_x_centerPos), centerField_y_pos), - Rotation2d.kZero); + public static final Pose3d blueHubCenter = new Pose3d( + hub_x_centerPos, centerField_y_pos, hub_z_len, Rotation3d.kZero); + public static final Pose3d redHubCenter = + new Pose3d(centerField_x_pos.times(2).minus(hub_x_centerPos), centerField_y_pos, hub_z_len, + Rotation3d.kZero); enum Region { BlueZone( @@ -52,8 +54,8 @@ enum Region { new Rectangle2d( new Translation2d(field_x_len.minus(allianceZone_x_len), Inches.of(0)), new Translation2d(field_x_len, field_y_len))), - BlueHub(new Rectangle2d(blueHubCenter, hub_x_len, hub_y_len)), - RedHub(new Rectangle2d(redHubCenter, hub_x_len, hub_y_len)), + BlueHub(new Rectangle2d(blueHubCenter.toPose2d(), hub_x_len, hub_y_len)), + RedHub(new Rectangle2d(redHubCenter.toPose2d(), hub_x_len, hub_y_len)), RedLeftTrench( new Rectangle2d( new Translation2d( diff --git a/src/main/java/frc/game/GameState.java b/src/main/java/frc/game/GameState.java index 8bb39e7..5dc7135 100644 --- a/src/main/java/frc/game/GameState.java +++ b/src/main/java/frc/game/GameState.java @@ -1,9 +1,8 @@ package frc.game; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc.game.Field.Region; import frc.robot.Robot; import java.util.List; import java.util.Optional; @@ -122,10 +121,10 @@ public static void logValues() { Logger.recordOutput("GameState/IsMyHubActive", isMyHubActive()); } - public static Translation2d getMyHubLocation() { + public static Pose3d getMyHubPose() { if (Robot.getAlliance() == Alliance.Red) { - return Region.RedHub.getCenter().getTranslation(); + return Field.redHubCenter; } - return Region.BlueHub.getCenter().getTranslation(); + return Field.blueHubCenter; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e45a4b3..4fe85b7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -323,7 +323,7 @@ public void bindZorroDriver(int port) { () -> -zorroDriver.getRightYAxis(), () -> -zorroDriver.getRightXAxis(), () -> - GameState.getMyHubLocation().minus(drive.getPose().getTranslation()).getAngle(), + GameState.getMyHubPose().toPose2d().getTranslation().minus(drive.getPose().getTranslation()).getAngle(), allianceSelector::fieldRotated)); // Switch to X pattern when button D is pressed @@ -366,7 +366,7 @@ public void bindXboxDriver(int port) { () -> -xboxDriver.getLeftY(), () -> -xboxDriver.getLeftX(), () -> - GameState.getMyHubLocation().minus(drive.getPose().getTranslation()).getAngle(), + GameState.getMyHubPose().toPose2d().getTranslation().minus(drive.getPose().getTranslation()).getAngle(), allianceSelector::fieldRotated)); // Point in the direction of the commanded translation while Y button is held diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index 4d6b9aa..f197ef1 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -76,7 +76,7 @@ public void stop() { public void aimAtHub() { // Get vector from static target to turret - var staticTarget = GameState.getMyHubLocation(); + var staticTarget = GameState.getMyHubPose(); var turretBase = chassisPoseSupplier.get().plus(chassisToTurretBase); var staticTargetToTurretBase = turretBase.getTranslation().minus(staticTarget); diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index aa1b89e..8de4bf2 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -3,7 +3,8 @@ import static edu.wpi.first.units.Units.*; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; @@ -14,8 +15,8 @@ public final class LauncherConstants { public static final class TurretConstants { // Geometry - public static final Transform2d chassisToTurretBase = - new Transform2d(Inches.of(0), Inches.of(10), Rotation2d.kZero); + public static final Transform3d chassisToTurretBase = + new Transform3d(Inches.of(0), Inches.of(10), Inches.of(22), Rotation3d.kZero); public static final Rotation2d rotationOffset = new Rotation2d(0.44); public static final LinearVelocity fuelVelocityRadial = MetersPerSecond.of(5); From 3c4e9248ff1e95e289edaedf11c48096858c6f1e Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 25 Jan 2026 17:10:10 -0500 Subject: [PATCH 04/11] update trajectory calculation --- src/main/java/frc/game/Field.java | 9 +- src/main/java/frc/robot/Robot.java | 37 +++- .../robot/subsystems/launcher/Launcher.java | 191 ++++++++++-------- .../launcher/LauncherConstants.java | 3 + 4 files changed, 146 insertions(+), 94 deletions(-) diff --git a/src/main/java/frc/game/Field.java b/src/main/java/frc/game/Field.java index 19dccc7..51d62f1 100644 --- a/src/main/java/frc/game/Field.java +++ b/src/main/java/frc/game/Field.java @@ -39,10 +39,13 @@ public class Field { (centerField_x_pos.minus(hub_x_centerPos.plus(hub_x_len.div(2)))).times(2); private static final Pose2d fieldCenter = new Pose2d(new Translation2d(centerField_x_pos, centerField_y_pos), Rotation2d.kZero); - public static final Pose3d blueHubCenter = new Pose3d( - hub_x_centerPos, centerField_y_pos, hub_z_len, Rotation3d.kZero); + public static final Pose3d blueHubCenter = + new Pose3d(hub_x_centerPos, centerField_y_pos, hub_z_len, Rotation3d.kZero); public static final Pose3d redHubCenter = - new Pose3d(centerField_x_pos.times(2).minus(hub_x_centerPos), centerField_y_pos, hub_z_len, + new Pose3d( + centerField_x_pos.times(2).minus(hub_x_centerPos), + centerField_y_pos, + hub_z_len, Rotation3d.kZero); enum Region { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4fe85b7..ec231b1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -113,7 +113,12 @@ public Robot() { new VisionIOPhotonVision(cameraBackRightName, robotToBackRightCamera), new VisionIOPhotonVision(cameraBackLeftName, robotToBackLeftCamera)); launcher = - new Launcher(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSpark(), new FlywheelIO() {}, new HoodIO() {}); + new Launcher( + drive::getPose, + drive::getRobotRelativeChassisSpeeds, + new TurretIOSpark(), + new FlywheelIO() {}, + new HoodIO() {}); break; case SIM: // Running a physics simulator @@ -141,7 +146,12 @@ public Robot() { new VisionIOPhotonVisionSim( cameraBackLeftName, robotToBackLeftCamera, drive::getPose)); launcher = - new Launcher(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSim(), new FlywheelIO() {}, new HoodIO() {}); + new Launcher( + drive::getPose, + drive::getRobotRelativeChassisSpeeds, + new TurretIOSim(), + new FlywheelIO() {}, + new HoodIO() {}); break; case REPLAY: // Replaying a log @@ -169,7 +179,12 @@ public Robot() { new VisionIO() {}, new VisionIO() {}); launcher = - new Launcher(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIO() {}, new FlywheelIO() {}, new HoodIO() {}); + new Launcher( + drive::getPose, + drive::getRobotRelativeChassisSpeeds, + new TurretIO() {}, + new FlywheelIO() {}, + new HoodIO() {}); break; } @@ -185,7 +200,7 @@ public Robot() { SmartDashboard.putData("Field", field); Field.plotRegions(); - launcher.setDefaultCommand(Commands.run(launcher::aimAtHub, launcher)); + launcher.setDefaultCommand(Commands.run(launcher::aimAtHub, launcher).withName("Aim at hub")); } /** This function is called periodically during all modes. */ @@ -203,6 +218,8 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); SmartDashboard.putData(CommandScheduler.getInstance()); SmartDashboard.putData(drive); + SmartDashboard.putData(vision); + SmartDashboard.putData(launcher); GameState.logValues(); @@ -323,7 +340,11 @@ public void bindZorroDriver(int port) { () -> -zorroDriver.getRightYAxis(), () -> -zorroDriver.getRightXAxis(), () -> - GameState.getMyHubPose().toPose2d().getTranslation().minus(drive.getPose().getTranslation()).getAngle(), + GameState.getMyHubPose() + .toPose2d() + .getTranslation() + .minus(drive.getPose().getTranslation()) + .getAngle(), allianceSelector::fieldRotated)); // Switch to X pattern when button D is pressed @@ -366,7 +387,11 @@ public void bindXboxDriver(int port) { () -> -xboxDriver.getLeftY(), () -> -xboxDriver.getLeftX(), () -> - GameState.getMyHubPose().toPose2d().getTranslation().minus(drive.getPose().getTranslation()).getAngle(), + GameState.getMyHubPose() + .toPose2d() + .getTranslation() + .minus(drive.getPose().getTranslation()) + .getAngle(), allianceSelector::fieldRotated)); // Point in the direction of the commanded translation while Y button is held diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index f197ef1..6bb6fb5 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -1,18 +1,19 @@ package frc.robot.subsystems.launcher; -import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; +import static frc.robot.subsystems.launcher.LauncherConstants.gravity; +import static frc.robot.subsystems.launcher.LauncherConstants.impactAngle; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.game.GameState; -import frc.robot.Constants.RobotConstants; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -25,7 +26,7 @@ public class Launcher extends SubsystemBase { private final TurretIOInputsAutoLogged turretInputs = new TurretIOInputsAutoLogged(); private final FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); private final HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); - + private final Supplier chassisPoseSupplier; private final Supplier chassisSpeedsSupplier; @@ -33,14 +34,17 @@ public class Launcher extends SubsystemBase { private final Alert flywheelDisconnectedAlert; private final Alert hoodDisconnectedAlert; - private Rotation2d turretOrientationSetpoint = Rotation2d.kZero; - private Translation2d dynamicTargetToTurretBase = new Translation2d(); - private Pose2d target = new Pose2d(); + // private Rotation2d turretOrientationSetpoint = Rotation2d.kZero; + private Translation3d vectorTurretBaseToHub = new Translation3d(); + private Pose3d turretBasePose = new Pose3d(); + private Translation3d initialVelocities = new Translation3d(); public Launcher( Supplier chassisPoseSupplier, Supplier chassisSpeedsSupplier, - TurretIO turretIO, FlywheelIO flywheelIO, HoodIO hoodIO) { + TurretIO turretIO, + FlywheelIO flywheelIO, + HoodIO hoodIO) { this.turretIO = turretIO; this.flywheelIO = flywheelIO; this.hoodIO = hoodIO; @@ -66,6 +70,11 @@ public void periodic() { turretDisconnectedAlert.set(!turretInputs.connected); flywheelDisconnectedAlert.set(!flywheelInputs.connected); hoodDisconnectedAlert.set(!hoodInputs.connected); + + // Get vector from static target to turret + var hubPose = GameState.getMyHubPose(); + turretBasePose = new Pose3d(chassisPoseSupplier.get()).plus(chassisToTurretBase); + vectorTurretBaseToHub = hubPose.getTranslation().minus(turretBasePose.getTranslation()); } public void stop() { @@ -75,87 +84,99 @@ public void stop() { } public void aimAtHub() { - // Get vector from static target to turret - var staticTarget = GameState.getMyHubPose(); - var turretBase = chassisPoseSupplier.get().plus(chassisToTurretBase); - var staticTargetToTurretBase = turretBase.getTranslation().minus(staticTarget); - - // Calculate travel time of free projectile at constant horizontal velocity - var radialDistance = Meters.of(staticTargetToTurretBase.getNorm()); - var fuelTravelTime = radialDistance.div(fuelVelocityRadial); - - // Get turret velocity (m/s) relative to hub - var robotRelative = chassisSpeedsSupplier.get(); - var fieldRelative = - ChassisSpeeds.fromRobotRelativeSpeeds(robotRelative, turretBase.getRotation()); - Translation2d turretBaseSpeeds = getTurretBaseSpeeds(fieldRelative); - - // Create unit vectors - var radialUnit = staticTargetToTurretBase.div(staticTargetToTurretBase.getNorm()); - var tangentialUnit = radialUnit.rotateBy(Rotation2d.kCCW_90deg); - - // Compute dot products - var radialMagnitude = turretBaseSpeeds.dot(radialUnit); - var tangentialMagnitude = turretBaseSpeeds.dot(tangentialUnit); - - // Create radial and tangential components of turret velocity (m/s) relative to hub - var radialVelocity = radialUnit.times(radialMagnitude); - var tangentialVelocity = tangentialUnit.times(tangentialMagnitude); - - // Calculate distance to lag the target - var lag = tangentialVelocity.times(fuelTravelTime.in(Seconds)); - var dynamicTarget = staticTarget.minus(lag); - target = new Pose2d(dynamicTarget, Rotation2d.kZero); - - // Get vector from dynamic target to turret - dynamicTargetToTurretBase = turretBase.getTranslation().minus(dynamicTarget); - - // Get angular velocity omega, where V = omega x R - var apparentAngularVelocityRadPerSec = - turretBaseSpeeds.cross(dynamicTargetToTurretBase) - / dynamicTargetToTurretBase.getSquaredNorm(); - - turretOrientationSetpoint = - dynamicTargetToTurretBase.unaryMinus().getAngle().minus(turretBase.getRotation()); - - double feedforwardVolts = - RobotConstants.kNominalVoltage - * -(2 * robotRelative.omegaRadiansPerSecond // TODO: Why is this factor 2 needed? - + apparentAngularVelocityRadPerSec) - / turnMaxAngularVelocity.in(RadiansPerSecond); - - turretIO.setPosition(turretOrientationSetpoint, feedforwardVolts); + updateIntialVelocities(vectorTurretBaseToHub); + flywheelIO.setVelocity(initialVelocities.getNorm(), 0); + + // // Calculate travel time of free projectile at constant horizontal velocity + // var radialDistance = Meters.of(vectorHubToTurretBase.getNorm()); + // var fuelTravelTime = radialDistance.div(fuelVelocityRadial); + + // // Get turret velocity (m/s) relative to hub + // var robotRelative = chassisSpeedsSupplier.get(); + // var fieldRelative = + // ChassisSpeeds.fromRobotRelativeSpeeds(robotRelative, turretBasePose.getRotation()); + // Translation2d turretBaseSpeeds = getTurretBaseSpeeds(fieldRelative); + + // // Create unit vectors + // var radialUnit = vectorHubToTurretBase.div(vectorHubToTurretBase.getNorm()); + // var tangentialUnit = radialUnit.rotateBy(Rotation2d.kCCW_90deg); + + // // Compute dot products + // var radialMagnitude = turretBaseSpeeds.dot(radialUnit); + // var tangentialMagnitude = turretBaseSpeeds.dot(tangentialUnit); + + // // Create radial and tangential components of turret velocity (m/s) relative to hub + // var radialVelocity = radialUnit.times(radialMagnitude); + // var tangentialVelocity = tangentialUnit.times(tangentialMagnitude); + + // // Calculate distance to lag the target + // var lag = tangentialVelocity.times(fuelTravelTime.in(Seconds)); + // var dynamicTarget = hubPose.minus(lag); + // target = new Pose2d(dynamicTarget, Rotation2d.kZero); + + // // Get vector from dynamic target to turret + // dynamicTargetToTurretBase = turretBasePose.getTranslation().minus(dynamicTarget); + + // // Get angular velocity omega, where V = omega x R + // var apparentAngularVelocityRadPerSec = + // turretBaseSpeeds.cross(dynamicTargetToTurretBase) + // / dynamicTargetToTurretBase.getSquaredNorm(); + + // turretOrientationSetpoint = + // dynamicTargetToTurretBase.unaryMinus().getAngle().minus(turretBasePose.getRotation()); + + // double feedforwardVolts = + // RobotConstants.kNominalVoltage + // * -(2 * robotRelative.omegaRadiansPerSecond // TODO: Why is this factor 2 needed? + // + apparentAngularVelocityRadPerSec) + // / turnMaxAngularVelocity.in(RadiansPerSecond); + + // turretIO.setPosition(turretOrientationSetpoint, feedforwardVolts); + turretIO.setPosition(turretBasePose.toPose2d().getRotation().unaryMinus(), 0); } - @AutoLogOutput(key = "Turret/Pose") + @AutoLogOutput(key = "Launcher/TurretPose") public Pose2d getTurretPose() { - return chassisPoseSupplier - .get() - .plus(chassisToTurretBase) - .plus(new Transform2d(0, 0, turretInputs.position)); - } - - @AutoLogOutput(key = "Turret/Target") - public Pose2d getTargetPose() { - return target; - } - - @AutoLogOutput(key = "Turret/IsOnTarget") - public boolean isOnTarget() { - return turretInputs.position.minus(turretOrientationSetpoint).getMeasure().abs(Radians) - * dynamicTargetToTurretBase.getNorm() - < (hubWidth.in(Meters) / 2.0); + return turretBasePose.toPose2d().plus(new Transform2d(0, 0, turretInputs.position)); } - private Translation2d getTurretBaseSpeeds(ChassisSpeeds chassisSpeeds) { - // Extract translation from the transform - Translation2d r = chassisToTurretBase.getTranslation(); - - double vx = chassisSpeeds.vxMetersPerSecond; - double vy = chassisSpeeds.vyMetersPerSecond; - double omega = chassisSpeeds.omegaRadiansPerSecond; - - // Rigid-body velocity equation - return new Translation2d(vx - omega * r.getY(), vy + omega * r.getX()); + // @AutoLogOutput(key = "Turret/IsOnTarget") + // public boolean isOnTarget() { + // return turretInputs.position.minus(turretOrientationSetpoint).getMeasure().abs(Radians) + // * dynamicTargetToTurretBase.getNorm() + // < (hubWidth.in(Meters) / 2.0); + // } + + // private Translation2d getTurretBaseSpeeds(ChassisSpeeds chassisSpeeds) { + // // Extract translation from the transform + // Translation2d r = chassisToTurretBase.getTranslation(); + + // double vx = chassisSpeeds.vxMetersPerSecond; + // double vy = chassisSpeeds.vyMetersPerSecond; + // double omega = chassisSpeeds.omegaRadiansPerSecond; + + // // Rigid-body velocity equation + // return new Translation2d(vx - omega * r.getY(), vy + omega * r.getX()); + // } + + private void updateIntialVelocities(Translation3d distances) { + double dr = Math.hypot(distances.getX(), distances.getY()); + double dz = distances.getZ(); + + double v_0r = dr * Math.sqrt(gravity / (2 * (dz + dr * impactAngle.getTan()))); + double v_0z = (gravity * dr) / v_0r - v_0r * impactAngle.getTan(); + + double v_0x = v_0r * distances.toTranslation2d().getAngle().getCos(); + double v_0y = v_0r * distances.toTranslation2d().getAngle().getSin(); + + initialVelocities = new Translation3d(v_0x, v_0y, v_0z); + Logger.recordOutput("Launcher/InitialVelocities", initialVelocities); + Logger.recordOutput("Launcher/InitialSpeedMetersPerSecond", initialVelocities.getNorm()); + Logger.recordOutput( + "Launcher/VerticalLaunchAngleDegrees", + new Translation2d(v_0r, v_0z).getAngle().getDegrees()); + Logger.recordOutput( + "Launcher/HorizontalLaunchAngleDegrees", + initialVelocities.toTranslation2d().getAngle().getDegrees()); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 8de4bf2..7ded89b 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -13,6 +13,9 @@ public final class LauncherConstants { + public static final Rotation2d impactAngle = Rotation2d.fromDegrees(60); + public static final double gravity = 9.81; + public static final class TurretConstants { // Geometry public static final Transform3d chassisToTurretBase = From ff4b815683fabf008b8477abffdfe2841e74e6ce Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 25 Jan 2026 17:18:28 -0500 Subject: [PATCH 05/11] plot nominal travel time --- src/main/java/frc/robot/subsystems/launcher/Launcher.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index 6bb6fb5..d019277 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -178,5 +178,6 @@ private void updateIntialVelocities(Translation3d distances) { Logger.recordOutput( "Launcher/HorizontalLaunchAngleDegrees", initialVelocities.toTranslation2d().getAngle().getDegrees()); + Logger.recordOutput("Launcher/NominalTravelTime", dr / v_0r); } } From 84902ca5aa4daa15c4b8459995ca8be835d0fab2 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 25 Jan 2026 17:45:29 -0500 Subject: [PATCH 06/11] calculate residual velocities after subtracting chassis base speed --- .../robot/subsystems/launcher/Launcher.java | 41 ++++++++++--------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index d019277..eb789ba 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -87,15 +88,16 @@ public void aimAtHub() { updateIntialVelocities(vectorTurretBaseToHub); flywheelIO.setVelocity(initialVelocities.getNorm(), 0); - // // Calculate travel time of free projectile at constant horizontal velocity - // var radialDistance = Meters.of(vectorHubToTurretBase.getNorm()); - // var fuelTravelTime = radialDistance.div(fuelVelocityRadial); + // Get turret linear velocities (m/s) + var robotRelative = chassisSpeedsSupplier.get(); + var fieldRelative = + ChassisSpeeds.fromRobotRelativeSpeeds( + robotRelative, turretBasePose.toPose2d().getRotation()); + Translation3d turretBaseSpeeds = getTurretBaseSpeeds(fieldRelative); - // // Get turret velocity (m/s) relative to hub - // var robotRelative = chassisSpeedsSupplier.get(); - // var fieldRelative = - // ChassisSpeeds.fromRobotRelativeSpeeds(robotRelative, turretBasePose.getRotation()); - // Translation2d turretBaseSpeeds = getTurretBaseSpeeds(fieldRelative); + var residualVelocities = initialVelocities.minus(turretBaseSpeeds); + Rotation2d turretSetpoint = + new Rotation2d(residualVelocities.getX(), residualVelocities.getY()); // // Create unit vectors // var radialUnit = vectorHubToTurretBase.div(vectorHubToTurretBase.getNorm()); @@ -132,7 +134,7 @@ public void aimAtHub() { // / turnMaxAngularVelocity.in(RadiansPerSecond); // turretIO.setPosition(turretOrientationSetpoint, feedforwardVolts); - turretIO.setPosition(turretBasePose.toPose2d().getRotation().unaryMinus(), 0); + turretIO.setPosition(turretSetpoint.minus(turretBasePose.toPose2d().getRotation()), 0); } @AutoLogOutput(key = "Launcher/TurretPose") @@ -147,17 +149,16 @@ public Pose2d getTurretPose() { // < (hubWidth.in(Meters) / 2.0); // } - // private Translation2d getTurretBaseSpeeds(ChassisSpeeds chassisSpeeds) { - // // Extract translation from the transform - // Translation2d r = chassisToTurretBase.getTranslation(); - - // double vx = chassisSpeeds.vxMetersPerSecond; - // double vy = chassisSpeeds.vyMetersPerSecond; - // double omega = chassisSpeeds.omegaRadiansPerSecond; - - // // Rigid-body velocity equation - // return new Translation2d(vx - omega * r.getY(), vy + omega * r.getX()); - // } + private Translation3d getTurretBaseSpeeds(ChassisSpeeds chassisSpeeds) { + double vx = chassisSpeeds.vxMetersPerSecond; + double vy = chassisSpeeds.vyMetersPerSecond; + double omega = chassisSpeeds.omegaRadiansPerSecond; + var baseSpeeds = + new Translation3d( + vx - omega * chassisToTurretBase.getY(), vy + omega * chassisToTurretBase.getX(), 0); + Logger.recordOutput("Launcher/BaseSpeeds", baseSpeeds); + return baseSpeeds; + } private void updateIntialVelocities(Translation3d distances) { double dr = Math.hypot(distances.getX(), distances.getY()); From 37439ed7eda8a762925c493c3dbbf4b6480a3c38 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Mon, 26 Jan 2026 08:00:42 -0500 Subject: [PATCH 07/11] log ball max height and whether it clears ceiling --- .../java/frc/robot/subsystems/launcher/Launcher.java | 9 +++++++++ .../frc/robot/subsystems/launcher/LauncherConstants.java | 4 +++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index eb789ba..e7d9283 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -1,6 +1,9 @@ package frc.robot.subsystems.launcher; +import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; +import static frc.robot.subsystems.launcher.LauncherConstants.ballRadius; +import static frc.robot.subsystems.launcher.LauncherConstants.ceilingHeight; import static frc.robot.subsystems.launcher.LauncherConstants.gravity; import static frc.robot.subsystems.launcher.LauncherConstants.impactAngle; @@ -180,5 +183,11 @@ private void updateIntialVelocities(Translation3d distances) { "Launcher/HorizontalLaunchAngleDegrees", initialVelocities.toTranslation2d().getAngle().getDegrees()); Logger.recordOutput("Launcher/NominalTravelTime", dr / v_0r); + + var max_height = turretBasePose.getZ() + v_0z * v_0z / (2 * gravity); + Logger.recordOutput("Launcher/NominalMaxHeight", max_height); + + boolean clearsCeiling = Meters.of(max_height).plus(ballRadius).lt(ceilingHeight); + Logger.recordOutput("Launcher/ClearsCeiling", clearsCeiling); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 7ded89b..2f11dce 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -13,7 +13,9 @@ public final class LauncherConstants { - public static final Rotation2d impactAngle = Rotation2d.fromDegrees(60); + public static final Rotation2d impactAngle = Rotation2d.fromDegrees(50); + public static final Distance ballRadius = Inches.of(3); + public static final Distance ceilingHeight = Feet.of(11).plus(Inches.of(2)); public static final double gravity = 9.81; public static final class TurretConstants { From df4ac26678214b59c4915b48105cd8fef50e4d95 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Mon, 26 Jan 2026 09:04:19 -0500 Subject: [PATCH 08/11] visualize fuel in flight --- .../robot/subsystems/launcher/Launcher.java | 59 ++++++++++++++++++- 1 file changed, 58 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index e7d9283..b0da697 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -18,6 +18,8 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.game.GameState; +import frc.robot.Robot; +import java.util.ArrayList; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -38,11 +40,15 @@ public class Launcher extends SubsystemBase { private final Alert flywheelDisconnectedAlert; private final Alert hoodDisconnectedAlert; - // private Rotation2d turretOrientationSetpoint = Rotation2d.kZero; private Translation3d vectorTurretBaseToHub = new Translation3d(); private Pose3d turretBasePose = new Pose3d(); private Translation3d initialVelocities = new Translation3d(); + // Ball simulation + private final ArrayList balls = new ArrayList<>(); + private static final double kBallSpawnPeriod = 0.1; // seconds + private double ballSpawnTimer = 0.0; + public Launcher( Supplier chassisPoseSupplier, Supplier chassisSpeedsSupplier, @@ -79,6 +85,8 @@ public void periodic() { var hubPose = GameState.getMyHubPose(); turretBasePose = new Pose3d(chassisPoseSupplier.get()).plus(chassisToTurretBase); vectorTurretBaseToHub = hubPose.getTranslation().minus(turretBasePose.getTranslation()); + + updateBalls(); } public void stop() { @@ -89,6 +97,19 @@ public void stop() { public void aimAtHub() { updateIntialVelocities(vectorTurretBaseToHub); + + ballSpawnTimer += Robot.defaultPeriodSecs; + if (ballSpawnTimer >= kBallSpawnPeriod) { + ballSpawnTimer = 0.0; + + balls.add( + new BallInFlight( + new Translation3d( + turretBasePose.getX(), turretBasePose.getY(), turretBasePose.getZ()), + new Translation3d( + initialVelocities.getX(), initialVelocities.getY(), initialVelocities.getZ()))); + } + flywheelIO.setVelocity(initialVelocities.getNorm(), 0); // Get turret linear velocities (m/s) @@ -190,4 +211,40 @@ private void updateIntialVelocities(Translation3d distances) { boolean clearsCeiling = Meters.of(max_height).plus(ballRadius).lt(ceilingHeight); Logger.recordOutput("Launcher/ClearsCeiling", clearsCeiling); } + + private static class BallInFlight { + Translation3d position; + Translation3d velocity; + + BallInFlight(Translation3d position, Translation3d velocity) { + this.position = position; + this.velocity = velocity; + } + } + + private void updateBalls() { + double dt = Robot.defaultPeriodSecs; + double hubZ = GameState.getMyHubPose().getZ(); + + balls.removeIf( + ball -> { + // Integrate velocity + ball.velocity = ball.velocity.plus(new Translation3d(0, 0, -gravity * dt)); + + // Integrate position + ball.position = ball.position.plus(ball.velocity.times(dt)); + + // Remove when below target height and falling + return ball.position.getZ() < hubZ && ball.velocity.getZ() < 0; + }); + } + + @AutoLogOutput(key = "Launcher/BallTrajectory") + public Translation3d[] getBallTrajectory() { + Translation3d[] t = new Translation3d[balls.size()]; + for (int i = 0; i < balls.size(); i++) { + t[i] = balls.get(i).position; + } + return t; + } } From 1a830a10c4b6781035d4ee4485b58ee4932dc364 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Mon, 26 Jan 2026 09:46:58 -0500 Subject: [PATCH 09/11] add basic turret feedforward --- .../robot/subsystems/launcher/FlywheelIO.java | 2 +- .../frc/robot/subsystems/launcher/HoodIO.java | 3 ++- .../robot/subsystems/launcher/Launcher.java | 21 ++++++++++--------- .../robot/subsystems/launcher/TurretIO.java | 3 ++- .../subsystems/launcher/TurretIOSim.java | 10 +++++++-- .../subsystems/launcher/TurretIOSpark.java | 8 ++++++- 6 files changed, 31 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java index 9040566..ab87d0f 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java @@ -15,5 +15,5 @@ public default void updateInputs(FlywheelIOInputs inputs) {} public default void setOpenLoop(double output) {} - public default void setVelocity(double velocityRadPerSec, double feedforwardVolts) {} + public default void setVelocity(double velocityRadPerSec) {} } diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIO.java b/src/main/java/frc/robot/subsystems/launcher/HoodIO.java index c06fa3c..79688ec 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIO.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.launcher; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.AngularVelocity; import org.littletonrobotics.junction.AutoLog; public interface HoodIO { @@ -17,5 +18,5 @@ public default void updateInputs(HoodIOInputs inputs) {} public default void setOpenLoop(double output) {} - public default void setPosition(Rotation2d rotation, double feedforwardVolts) {} + public default void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) {} } diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index b0da697..2ea1b79 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -42,7 +42,7 @@ public class Launcher extends SubsystemBase { private Translation3d vectorTurretBaseToHub = new Translation3d(); private Pose3d turretBasePose = new Pose3d(); - private Translation3d initialVelocities = new Translation3d(); + private Translation3d v0_nominal = new Translation3d(); // Ball simulation private final ArrayList balls = new ArrayList<>(); @@ -106,11 +106,10 @@ public void aimAtHub() { new BallInFlight( new Translation3d( turretBasePose.getX(), turretBasePose.getY(), turretBasePose.getZ()), - new Translation3d( - initialVelocities.getX(), initialVelocities.getY(), initialVelocities.getZ()))); + new Translation3d(v0_nominal.getX(), v0_nominal.getY(), v0_nominal.getZ()))); } - flywheelIO.setVelocity(initialVelocities.getNorm(), 0); + flywheelIO.setVelocity(v0_nominal.getNorm()); // Get turret linear velocities (m/s) var robotRelative = chassisSpeedsSupplier.get(); @@ -119,7 +118,7 @@ public void aimAtHub() { robotRelative, turretBasePose.toPose2d().getRotation()); Translation3d turretBaseSpeeds = getTurretBaseSpeeds(fieldRelative); - var residualVelocities = initialVelocities.minus(turretBaseSpeeds); + var residualVelocities = v0_nominal.minus(turretBaseSpeeds); Rotation2d turretSetpoint = new Rotation2d(residualVelocities.getX(), residualVelocities.getY()); @@ -158,7 +157,9 @@ public void aimAtHub() { // / turnMaxAngularVelocity.in(RadiansPerSecond); // turretIO.setPosition(turretOrientationSetpoint, feedforwardVolts); - turretIO.setPosition(turretSetpoint.minus(turretBasePose.toPose2d().getRotation()), 0); + turretIO.setPosition( + turretSetpoint.minus(turretBasePose.toPose2d().getRotation()), + RadiansPerSecond.of(robotRelative.omegaRadiansPerSecond).unaryMinus()); } @AutoLogOutput(key = "Launcher/TurretPose") @@ -194,15 +195,15 @@ private void updateIntialVelocities(Translation3d distances) { double v_0x = v_0r * distances.toTranslation2d().getAngle().getCos(); double v_0y = v_0r * distances.toTranslation2d().getAngle().getSin(); - initialVelocities = new Translation3d(v_0x, v_0y, v_0z); - Logger.recordOutput("Launcher/InitialVelocities", initialVelocities); - Logger.recordOutput("Launcher/InitialSpeedMetersPerSecond", initialVelocities.getNorm()); + v0_nominal = new Translation3d(v_0x, v_0y, v_0z); + Logger.recordOutput("Launcher/InitialVelocities", v0_nominal); + Logger.recordOutput("Launcher/InitialSpeedMetersPerSecond", v0_nominal.getNorm()); Logger.recordOutput( "Launcher/VerticalLaunchAngleDegrees", new Translation2d(v_0r, v_0z).getAngle().getDegrees()); Logger.recordOutput( "Launcher/HorizontalLaunchAngleDegrees", - initialVelocities.toTranslation2d().getAngle().getDegrees()); + v0_nominal.toTranslation2d().getAngle().getDegrees()); Logger.recordOutput("Launcher/NominalTravelTime", dr / v_0r); var max_height = turretBasePose.getZ() + v_0z * v_0z / (2 * gravity); diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java index 30b2c5d..32dfe86 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.launcher; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.AngularVelocity; import org.littletonrobotics.junction.AutoLog; public interface TurretIO { @@ -17,5 +18,5 @@ public default void updateInputs(TurretIOInputs inputs) {} public default void setOpenLoop(double output) {} - public default void setPosition(Rotation2d rotation, double feedforwardVolts) {} + public default void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) {} } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java index a9fa8ba..62c0605 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java @@ -1,12 +1,15 @@ package frc.robot.subsystems.launcher; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants.RobotConstants; public class TurretIOSim implements TurretIO { private final DCMotorSim turnSim; @@ -55,9 +58,12 @@ public void setOpenLoop(double output) { } @Override - public void setPosition(Rotation2d rotation, double feedforwardVolts) { + public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { turnClosedLoop = true; - this.feedforwardVolts = feedforwardVolts; + this.feedforwardVolts = + RobotConstants.kNominalVoltage + * angularVelocity.in(RadiansPerSecond) + / turnMaxAngularVelocity.in(RadiansPerSecond); turnController.setSetpoint(rotation.getRadians()); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index 682e3fa..8d6a748 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.launcher; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; import static frc.robot.util.SparkUtil.*; @@ -18,6 +19,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.Constants.MotorConstants.NEO550Constants; import frc.robot.Constants.RobotConstants; import java.util.function.DoubleSupplier; @@ -97,10 +99,14 @@ public void setOpenLoop(double output) { } @Override - public void setPosition(Rotation2d rotation, double feedforwardVolts) { + public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = MathUtil.inputModulus( rotation.plus(rotationOffset).getRadians(), turnPIDMinInput, turnPIDMaxInput); + double feedforwardVolts = + RobotConstants.kNominalVoltage + * angularVelocity.in(RadiansPerSecond) + / turnMaxAngularVelocity.in(RadiansPerSecond); turnController.setSetpoint( setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); } From 72c6cbfc25a9ca381f0f291cc9f5608fea403e42 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Mon, 26 Jan 2026 10:02:18 -0500 Subject: [PATCH 10/11] plot 2nd set of ballistic trajectories --- .../robot/subsystems/launcher/Launcher.java | 117 +++++++----------- .../launcher/LauncherConstants.java | 3 +- 2 files changed, 48 insertions(+), 72 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index 2ea1b79..ae1a0a3 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -1,11 +1,8 @@ package frc.robot.subsystems.launcher; import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.launcher.LauncherConstants.*; import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; -import static frc.robot.subsystems.launcher.LauncherConstants.ballRadius; -import static frc.robot.subsystems.launcher.LauncherConstants.ceilingHeight; -import static frc.robot.subsystems.launcher.LauncherConstants.gravity; -import static frc.robot.subsystems.launcher.LauncherConstants.impactAngle; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -44,10 +41,10 @@ public class Launcher extends SubsystemBase { private Pose3d turretBasePose = new Pose3d(); private Translation3d v0_nominal = new Translation3d(); - // Ball simulation - private final ArrayList balls = new ArrayList<>(); - private static final double kBallSpawnPeriod = 0.1; // seconds - private double ballSpawnTimer = 0.0; + // Fuel ballistics simulation + private final ArrayList fuelNominal = new ArrayList<>(); + private final ArrayList fuelActual = new ArrayList<>(); + private double fuelSpawnTimer = 0.0; public Launcher( Supplier chassisPoseSupplier, @@ -86,7 +83,11 @@ public void periodic() { turretBasePose = new Pose3d(chassisPoseSupplier.get()).plus(chassisToTurretBase); vectorTurretBaseToHub = hubPose.getTranslation().minus(turretBasePose.getTranslation()); - updateBalls(); + updateBallisticsSim(fuelNominal); + updateBallisticsSim(fuelActual); + + Logger.recordOutput("Launcher/FuelNominal", getBallTrajectory(fuelNominal)); + Logger.recordOutput("Launcher/FuelActual", getBallTrajectory(fuelActual)); } public void stop() { @@ -97,18 +98,6 @@ public void stop() { public void aimAtHub() { updateIntialVelocities(vectorTurretBaseToHub); - - ballSpawnTimer += Robot.defaultPeriodSecs; - if (ballSpawnTimer >= kBallSpawnPeriod) { - ballSpawnTimer = 0.0; - - balls.add( - new BallInFlight( - new Translation3d( - turretBasePose.getX(), turretBasePose.getY(), turretBasePose.getZ()), - new Translation3d(v0_nominal.getX(), v0_nominal.getY(), v0_nominal.getZ()))); - } - flywheelIO.setVelocity(v0_nominal.getNorm()); // Get turret linear velocities (m/s) @@ -118,48 +107,31 @@ public void aimAtHub() { robotRelative, turretBasePose.toPose2d().getRotation()); Translation3d turretBaseSpeeds = getTurretBaseSpeeds(fieldRelative); + // Point turret to align velocities var residualVelocities = v0_nominal.minus(turretBaseSpeeds); Rotation2d turretSetpoint = new Rotation2d(residualVelocities.getX(), residualVelocities.getY()); - - // // Create unit vectors - // var radialUnit = vectorHubToTurretBase.div(vectorHubToTurretBase.getNorm()); - // var tangentialUnit = radialUnit.rotateBy(Rotation2d.kCCW_90deg); - - // // Compute dot products - // var radialMagnitude = turretBaseSpeeds.dot(radialUnit); - // var tangentialMagnitude = turretBaseSpeeds.dot(tangentialUnit); - - // // Create radial and tangential components of turret velocity (m/s) relative to hub - // var radialVelocity = radialUnit.times(radialMagnitude); - // var tangentialVelocity = tangentialUnit.times(tangentialMagnitude); - - // // Calculate distance to lag the target - // var lag = tangentialVelocity.times(fuelTravelTime.in(Seconds)); - // var dynamicTarget = hubPose.minus(lag); - // target = new Pose2d(dynamicTarget, Rotation2d.kZero); - - // // Get vector from dynamic target to turret - // dynamicTargetToTurretBase = turretBasePose.getTranslation().minus(dynamicTarget); - - // // Get angular velocity omega, where V = omega x R - // var apparentAngularVelocityRadPerSec = - // turretBaseSpeeds.cross(dynamicTargetToTurretBase) - // / dynamicTargetToTurretBase.getSquaredNorm(); - - // turretOrientationSetpoint = - // dynamicTargetToTurretBase.unaryMinus().getAngle().minus(turretBasePose.getRotation()); - - // double feedforwardVolts = - // RobotConstants.kNominalVoltage - // * -(2 * robotRelative.omegaRadiansPerSecond // TODO: Why is this factor 2 needed? - // + apparentAngularVelocityRadPerSec) - // / turnMaxAngularVelocity.in(RadiansPerSecond); - - // turretIO.setPosition(turretOrientationSetpoint, feedforwardVolts); turretIO.setPosition( turretSetpoint.minus(turretBasePose.toPose2d().getRotation()), RadiansPerSecond.of(robotRelative.omegaRadiansPerSecond).unaryMinus()); + + // Spawn simulated fuel + fuelSpawnTimer += Robot.defaultPeriodSecs; + if (fuelSpawnTimer >= fuelSpawnPeriod) { + fuelSpawnTimer = 0.0; + + fuelNominal.add( + new BallisticObject( + new Translation3d( + turretBasePose.getX(), turretBasePose.getY(), turretBasePose.getZ()), + new Translation3d(v0_nominal.getX(), v0_nominal.getY(), v0_nominal.getZ()))); + + fuelActual.add( + new BallisticObject( + new Translation3d( + turretBasePose.getX(), turretBasePose.getY(), turretBasePose.getZ()), + new Translation3d(v0_nominal.getX(), v0_nominal.getY(), v0_nominal.getZ()))); + } } @AutoLogOutput(key = "Launcher/TurretPose") @@ -209,42 +181,45 @@ private void updateIntialVelocities(Translation3d distances) { var max_height = turretBasePose.getZ() + v_0z * v_0z / (2 * gravity); Logger.recordOutput("Launcher/NominalMaxHeight", max_height); - boolean clearsCeiling = Meters.of(max_height).plus(ballRadius).lt(ceilingHeight); + boolean clearsCeiling = Meters.of(max_height).plus(fuelRadius).lt(ceilingHeight); Logger.recordOutput("Launcher/ClearsCeiling", clearsCeiling); } - private static class BallInFlight { + private static class BallisticObject { Translation3d position; Translation3d velocity; - BallInFlight(Translation3d position, Translation3d velocity) { + BallisticObject(Translation3d position, Translation3d velocity) { this.position = position; this.velocity = velocity; } + + public Translation3d getPosition() { + return position; + } } - private void updateBalls() { + private void updateBallisticsSim(ArrayList traj) { double dt = Robot.defaultPeriodSecs; double hubZ = GameState.getMyHubPose().getZ(); - balls.removeIf( - ball -> { + traj.removeIf( + o -> { // Integrate velocity - ball.velocity = ball.velocity.plus(new Translation3d(0, 0, -gravity * dt)); + o.velocity = o.velocity.plus(new Translation3d(0, 0, -gravity * dt)); // Integrate position - ball.position = ball.position.plus(ball.velocity.times(dt)); + o.position = o.position.plus(o.velocity.times(dt)); // Remove when below target height and falling - return ball.position.getZ() < hubZ && ball.velocity.getZ() < 0; + return o.position.getZ() < hubZ && o.velocity.getZ() < 0; }); } - @AutoLogOutput(key = "Launcher/BallTrajectory") - public Translation3d[] getBallTrajectory() { - Translation3d[] t = new Translation3d[balls.size()]; - for (int i = 0; i < balls.size(); i++) { - t[i] = balls.get(i).position; + public Translation3d[] getBallTrajectory(ArrayList traj) { + Translation3d[] t = new Translation3d[traj.size()]; + for (int i = 0; i < traj.size(); i++) { + t[i] = traj.get(i).position; } return t; } diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 2f11dce..b8f6ee2 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -14,9 +14,10 @@ public final class LauncherConstants { public static final Rotation2d impactAngle = Rotation2d.fromDegrees(50); - public static final Distance ballRadius = Inches.of(3); + public static final Distance fuelRadius = Inches.of(3); public static final Distance ceilingHeight = Feet.of(11).plus(Inches.of(2)); public static final double gravity = 9.81; + public static final double fuelSpawnPeriod = 0.1; // seconds public static final class TurretConstants { // Geometry From 3796455cd95d1a8bd5c229940c1e938d992c56d5 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Mon, 26 Jan 2026 11:04:17 -0500 Subject: [PATCH 11/11] simulate actual ball trajectories --- .../robot/subsystems/launcher/FlywheelIO.java | 3 +- .../robot/subsystems/launcher/Launcher.java | 65 +++++++++++++------ .../launcher/LauncherConstants.java | 4 ++ 3 files changed, 52 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java index ab87d0f..50ce781 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.launcher; +import edu.wpi.first.units.measure.AngularVelocity; import org.littletonrobotics.junction.AutoLog; public interface FlywheelIO { @@ -15,5 +16,5 @@ public default void updateInputs(FlywheelIOInputs inputs) {} public default void setOpenLoop(double output) {} - public default void setVelocity(double velocityRadPerSec) {} + public default void setVelocity(AngularVelocity angularVelocity) {} } diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index ae1a0a3..5afca94 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.launcher.LauncherConstants.*; +import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*; import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; import edu.wpi.first.math.geometry.Pose2d; @@ -11,6 +12,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -86,8 +88,8 @@ public void periodic() { updateBallisticsSim(fuelNominal); updateBallisticsSim(fuelActual); - Logger.recordOutput("Launcher/FuelNominal", getBallTrajectory(fuelNominal)); - Logger.recordOutput("Launcher/FuelActual", getBallTrajectory(fuelActual)); + Logger.recordOutput("Launcher/Nominal/FuelTrajectory", getBallTrajectory(fuelNominal)); + Logger.recordOutput("Launcher/Actual/FuelTrajectory", getBallTrajectory(fuelActual)); } public void stop() { @@ -97,23 +99,52 @@ public void stop() { } public void aimAtHub() { + // Set flywheel speed assuming a motionless robot updateIntialVelocities(vectorTurretBaseToHub); - flywheelIO.setVelocity(v0_nominal.getNorm()); + AngularVelocity flywheelSetpoint = + RadiansPerSecond.of(v0_nominal.getNorm() / wheelRadius.in(Meters)); + flywheelIO.setVelocity(flywheelSetpoint); - // Get turret linear velocities (m/s) + // Get translation velocities (m/s) of the turret caused by motion of the chassis var robotRelative = chassisSpeedsSupplier.get(); var fieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds( robotRelative, turretBasePose.toPose2d().getRotation()); Translation3d turretBaseSpeeds = getTurretBaseSpeeds(fieldRelative); - // Point turret to align velocities + // Point turret and hood to align velocity vectors var residualVelocities = v0_nominal.minus(turretBaseSpeeds); Rotation2d turretSetpoint = new Rotation2d(residualVelocities.getX(), residualVelocities.getY()); turretIO.setPosition( turretSetpoint.minus(turretBasePose.toPose2d().getRotation()), - RadiansPerSecond.of(robotRelative.omegaRadiansPerSecond).unaryMinus()); + RadiansPerSecond.of(robotRelative.omegaRadiansPerSecond) + .unaryMinus() + .times(2)); // TODO: Why is this factor 2 needed? + Rotation2d hoodSetpoint = + new Rotation2d( + Math.hypot(residualVelocities.getX(), residualVelocities.getY()), + residualVelocities.getZ()); + hoodIO.setPosition(hoodSetpoint, RadiansPerSecond.of(0)); + + // Get actuals + double flywheelSpeedMetersPerSec = + flywheelSetpoint.in(RadiansPerSecond) + * wheelRadius.in( + Meters); // TODO: Replace with actual speed when flywheel simulation is added + Rotation2d hoodPosition = + hoodSetpoint; // TODO: Replace with actual position when hood simulation is added + Rotation2d turretPosition = turretInputs.position.plus(turretBasePose.toPose2d().getRotation()); + + // Build actual velocities + Translation3d v0_actual = + new Translation3d( + hoodPosition.getCos() * turretPosition.getCos(), + hoodPosition.getCos() * turretPosition.getSin(), + hoodPosition.getSin()) + .times(flywheelSpeedMetersPerSec) + .plus(turretBaseSpeeds); + Logger.recordOutput("Launcher/Actual/InitialVelocities", v0_actual); // Spawn simulated fuel fuelSpawnTimer += Robot.defaultPeriodSecs; @@ -130,11 +161,11 @@ public void aimAtHub() { new BallisticObject( new Translation3d( turretBasePose.getX(), turretBasePose.getY(), turretBasePose.getZ()), - new Translation3d(v0_nominal.getX(), v0_nominal.getY(), v0_nominal.getZ()))); + new Translation3d(v0_actual.getX(), v0_actual.getY(), v0_actual.getZ()))); } } - @AutoLogOutput(key = "Launcher/TurretPose") + @AutoLogOutput(key = "Launcher/Actual/TurretPose") public Pose2d getTurretPose() { return turretBasePose.toPose2d().plus(new Transform2d(0, 0, turretInputs.position)); } @@ -168,21 +199,21 @@ private void updateIntialVelocities(Translation3d distances) { double v_0y = v_0r * distances.toTranslation2d().getAngle().getSin(); v0_nominal = new Translation3d(v_0x, v_0y, v_0z); - Logger.recordOutput("Launcher/InitialVelocities", v0_nominal); - Logger.recordOutput("Launcher/InitialSpeedMetersPerSecond", v0_nominal.getNorm()); + Logger.recordOutput("Launcher/Nominal/InitialVelocities", v0_nominal); + Logger.recordOutput("Launcher/Nominal/InitialSpeedMetersPerSecond", v0_nominal.getNorm()); Logger.recordOutput( - "Launcher/VerticalLaunchAngleDegrees", + "Launcher/Nominal/VerticalLaunchAngleDegrees", new Translation2d(v_0r, v_0z).getAngle().getDegrees()); Logger.recordOutput( - "Launcher/HorizontalLaunchAngleDegrees", + "Launcher/Nominal/HorizontalLaunchAngleDegrees", v0_nominal.toTranslation2d().getAngle().getDegrees()); - Logger.recordOutput("Launcher/NominalTravelTime", dr / v_0r); + Logger.recordOutput("Launcher/Nominal/TravelTime", dr / v_0r); var max_height = turretBasePose.getZ() + v_0z * v_0z / (2 * gravity); - Logger.recordOutput("Launcher/NominalMaxHeight", max_height); + Logger.recordOutput("Launcher/Nominal/MaxHeight", max_height); boolean clearsCeiling = Meters.of(max_height).plus(fuelRadius).lt(ceilingHeight); - Logger.recordOutput("Launcher/ClearsCeiling", clearsCeiling); + Logger.recordOutput("Launcher/Nominal/ClearsCeiling", clearsCeiling); } private static class BallisticObject { @@ -193,10 +224,6 @@ private static class BallisticObject { this.position = position; this.velocity = velocity; } - - public Translation3d getPosition() { - return position; - } } private void updateBallisticsSim(ArrayList traj) { diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index b8f6ee2..dd79a57 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -47,4 +47,8 @@ public static final class TurretConstants { public static final double turnKpSim = 0.8; public static final double turnKdSim = 0.05; } + + public static final class FlywheelConstants { + public static final Distance wheelRadius = Inches.of(1.5); + } }