diff --git a/src/main/java/frc/game/Field.java b/src/main/java/frc/game/Field.java index 1e74583..51d62f1 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,14 @@ 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 +57,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 9f191ef..ec231b1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -34,10 +34,12 @@ 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.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; +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 +68,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 +112,13 @@ 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(), + new FlywheelIO() {}, + new HoodIO() {}); break; case SIM: // Running a physics simulator @@ -138,8 +145,13 @@ 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(), + new FlywheelIO() {}, + new HoodIO() {}); break; case REPLAY: // Replaying a log @@ -166,8 +178,13 @@ 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() {}, + new FlywheelIO() {}, + new HoodIO() {}); break; } @@ -183,7 +200,7 @@ public Robot() { SmartDashboard.putData("Field", field); Field.plotRegions(); - turret.setDefaultCommand(Commands.run(turret::aimAtHub, turret)); + launcher.setDefaultCommand(Commands.run(launcher::aimAtHub, launcher).withName("Aim at hub")); } /** This function is called periodically during all modes. */ @@ -201,6 +218,8 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); SmartDashboard.putData(CommandScheduler.getInstance()); SmartDashboard.putData(drive); + SmartDashboard.putData(vision); + SmartDashboard.putData(launcher); GameState.logValues(); @@ -321,7 +340,11 @@ 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 @@ -364,7 +387,11 @@ 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/FlywheelIO.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java new file mode 100644 index 0000000..50ce781 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.launcher; + +import edu.wpi.first.units.measure.AngularVelocity; +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(AngularVelocity angularVelocity) {} +} 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..79688ec --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIO.java @@ -0,0 +1,22 @@ +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 { + @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, AngularVelocity angularVelocity) {} +} diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java new file mode 100644 index 0000000..5afca94 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -0,0 +1,253 @@ +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.FlywheelConstants.*; +import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; + +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; +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; +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; + +public class Launcher extends SubsystemBase { + 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 turretDisconnectedAlert; + private final Alert flywheelDisconnectedAlert; + private final Alert hoodDisconnectedAlert; + + private Translation3d vectorTurretBaseToHub = new Translation3d(); + private Pose3d turretBasePose = new Pose3d(); + private Translation3d v0_nominal = new Translation3d(); + + // Fuel ballistics simulation + private final ArrayList fuelNominal = new ArrayList<>(); + private final ArrayList fuelActual = new ArrayList<>(); + private double fuelSpawnTimer = 0.0; + + public Launcher( + Supplier chassisPoseSupplier, + Supplier chassisSpeedsSupplier, + TurretIO turretIO, + FlywheelIO flywheelIO, + HoodIO hoodIO) { + this.turretIO = turretIO; + this.flywheelIO = flywheelIO; + this.hoodIO = hoodIO; + + this.chassisPoseSupplier = chassisPoseSupplier; + this.chassisSpeedsSupplier = chassisSpeedsSupplier; + + 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() { + 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); + + // Get vector from static target to turret + var hubPose = GameState.getMyHubPose(); + turretBasePose = new Pose3d(chassisPoseSupplier.get()).plus(chassisToTurretBase); + vectorTurretBaseToHub = hubPose.getTranslation().minus(turretBasePose.getTranslation()); + + updateBallisticsSim(fuelNominal); + updateBallisticsSim(fuelActual); + + Logger.recordOutput("Launcher/Nominal/FuelTrajectory", getBallTrajectory(fuelNominal)); + Logger.recordOutput("Launcher/Actual/FuelTrajectory", getBallTrajectory(fuelActual)); + } + + public void stop() { + turretIO.setOpenLoop(0.0); + flywheelIO.setOpenLoop(0.0); + hoodIO.setOpenLoop(0.0); + } + + public void aimAtHub() { + // Set flywheel speed assuming a motionless robot + updateIntialVelocities(vectorTurretBaseToHub); + AngularVelocity flywheelSetpoint = + RadiansPerSecond.of(v0_nominal.getNorm() / wheelRadius.in(Meters)); + flywheelIO.setVelocity(flywheelSetpoint); + + // 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 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() + .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; + 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_actual.getX(), v0_actual.getY(), v0_actual.getZ()))); + } + } + + @AutoLogOutput(key = "Launcher/Actual/TurretPose") + public Pose2d getTurretPose() { + return turretBasePose.toPose2d().plus(new Transform2d(0, 0, turretInputs.position)); + } + + // @AutoLogOutput(key = "Turret/IsOnTarget") + // public boolean isOnTarget() { + // return turretInputs.position.minus(turretOrientationSetpoint).getMeasure().abs(Radians) + // * dynamicTargetToTurretBase.getNorm() + // < (hubWidth.in(Meters) / 2.0); + // } + + 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()); + 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(); + + v0_nominal = new Translation3d(v_0x, v_0y, v_0z); + Logger.recordOutput("Launcher/Nominal/InitialVelocities", v0_nominal); + Logger.recordOutput("Launcher/Nominal/InitialSpeedMetersPerSecond", v0_nominal.getNorm()); + Logger.recordOutput( + "Launcher/Nominal/VerticalLaunchAngleDegrees", + new Translation2d(v_0r, v_0z).getAngle().getDegrees()); + Logger.recordOutput( + "Launcher/Nominal/HorizontalLaunchAngleDegrees", + v0_nominal.toTranslation2d().getAngle().getDegrees()); + Logger.recordOutput("Launcher/Nominal/TravelTime", dr / v_0r); + + var max_height = turretBasePose.getZ() + v_0z * v_0z / (2 * gravity); + Logger.recordOutput("Launcher/Nominal/MaxHeight", max_height); + + boolean clearsCeiling = Meters.of(max_height).plus(fuelRadius).lt(ceilingHeight); + Logger.recordOutput("Launcher/Nominal/ClearsCeiling", clearsCeiling); + } + + private static class BallisticObject { + Translation3d position; + Translation3d velocity; + + BallisticObject(Translation3d position, Translation3d velocity) { + this.position = position; + this.velocity = velocity; + } + } + + private void updateBallisticsSim(ArrayList traj) { + double dt = Robot.defaultPeriodSecs; + double hubZ = GameState.getMyHubPose().getZ(); + + traj.removeIf( + o -> { + // Integrate velocity + o.velocity = o.velocity.plus(new Translation3d(0, 0, -gravity * dt)); + + // Integrate position + o.position = o.position.plus(o.velocity.times(dt)); + + // Remove when below target height and falling + return o.position.getZ() < hubZ && o.velocity.getZ() < 0; + }); + } + + 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 new file mode 100644 index 0000000..dd79a57 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -0,0 +1,54 @@ +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.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; +import edu.wpi.first.units.measure.LinearVelocity; +import frc.robot.Constants.MotorConstants.NEO550Constants; + +public final class LauncherConstants { + + public static final Rotation2d impactAngle = Rotation2d.fromDegrees(50); + 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 + 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); + + // 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; + } + + public static final class FlywheelConstants { + public static final Distance wheelRadius = Inches.of(1.5); + } +} 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..32dfe86 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java @@ -0,0 +1,22 @@ +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 { + @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, AngularVelocity angularVelocity) {} +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOSim.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java similarity index 62% rename from src/main/java/frc/robot/subsystems/turret/TurretIOSim.java rename to src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java index 384544c..62c0605 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java @@ -1,12 +1,15 @@ -package frc.robot.subsystems.turret; +package frc.robot.subsystems.launcher; -import static frc.robot.subsystems.turret.TurretConstants.*; +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; @@ -41,23 +44,26 @@ 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, 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/turret/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java similarity index 80% rename from src/main/java/frc/robot/subsystems/turret/TurretIOSpark.java rename to src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index 9428767..8d6a748 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -1,6 +1,7 @@ -package frc.robot.subsystems.turret; +package frc.robot.subsystems.launcher; -import static frc.robot.subsystems.turret.TurretConstants.*; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; import static frc.robot.util.SparkUtil.*; import com.revrobotics.AbsoluteEncoder; @@ -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; @@ -81,26 +83,30 @@ 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, 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); } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java deleted file mode 100644 index db144d6..0000000 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ /dev/null @@ -1,137 +0,0 @@ -package frc.robot.subsystems.turret; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.turret.TurretConstants.*; - -import edu.wpi.first.math.geometry.Pose2d; -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.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; - -public class Turret extends SubsystemBase { - private final TurretIO io; - private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); - private final Supplier chassisPoseSupplier; - private final Supplier chassisSpeedsSupplier; - - private final Alert turnDisconnectedAlert; - - private Rotation2d turretOrientationSetpoint = Rotation2d.kZero; - private Translation2d dynamicTargetToTurretBase = new Translation2d(); - private Pose2d target = new Pose2d(); - - public Turret( - Supplier chassisPoseSupplier, - Supplier chassisSpeedsSupplier, - TurretIO io) { - this.io = io; - this.chassisPoseSupplier = chassisPoseSupplier; - this.chassisSpeedsSupplier = chassisSpeedsSupplier; - turnDisconnectedAlert = new Alert("Disconnected turret turn motor", AlertType.kError); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Turret", inputs); - turnDisconnectedAlert.set(!inputs.turnConnected); - } - - public void stop() { - io.setTurnOpenLoop(0.0); - } - - public void aimAtHub() { - // Get vector from static target to turret - var staticTarget = GameState.getMyHubLocation(); - 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); - - io.setTurnPosition(turretOrientationSetpoint, feedforwardVolts); - } - - @AutoLogOutput(key = "Turret/Pose") - public Pose2d getTurretPose() { - return chassisPoseSupplier - .get() - .plus(chassisToTurretBase) - .plus(new Transform2d(0, 0, inputs.turnPosition)); - } - - @AutoLogOutput(key = "Turret/Target") - public Pose2d getTargetPose() { - return target; - } - - @AutoLogOutput(key = "Turret/IsOnTarget") - public boolean isOnTarget() { - return inputs.turnPosition.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()); - } -} 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) {} -}