diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ec231b1..6ac1eff 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -35,7 +35,9 @@ import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.launcher.FlywheelIO; +import frc.robot.subsystems.launcher.FlywheelIOSim; import frc.robot.subsystems.launcher.HoodIO; +import frc.robot.subsystems.launcher.HoodIOSim; import frc.robot.subsystems.launcher.Launcher; import frc.robot.subsystems.launcher.TurretIO; import frc.robot.subsystems.launcher.TurretIOSim; @@ -150,8 +152,8 @@ public Robot() { drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSim(), - new FlywheelIO() {}, - new HoodIO() {}); + new FlywheelIOSim(), + new HoodIOSim()); break; case REPLAY: // Replaying a log @@ -200,7 +202,9 @@ public Robot() { SmartDashboard.putData("Field", field); Field.plotRegions(); - launcher.setDefaultCommand(Commands.run(launcher::aimAtHub, launcher).withName("Aim at hub")); + launcher.setDefaultCommand( + Commands.run(() -> launcher.aim(GameState.getMyHubPose().getTranslation()), launcher) + .withName("Aim at hub")); } /** This function is called periodically during all modes. */ diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java new file mode 100644 index 0000000..1f60aa5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java @@ -0,0 +1,66 @@ +package frc.robot.subsystems.launcher; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +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; +import frc.robot.Robot; + +public class FlywheelIOSim implements FlywheelIO { + private final DCMotorSim flywheelSim; + + private boolean closedLoop = false; + private PIDController velocityController = new PIDController(kPSim, 0.0, 0.0); + private double appliedVolts = 0.0; + private double feedforwardVolts = 0.0; + + public FlywheelIOSim() { + flywheelSim = + new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + // Run closed-loop control + if (closedLoop) { + appliedVolts = + velocityController.calculate(flywheelSim.getAngularVelocityRadPerSec()) + + feedforwardVolts; + } else { + velocityController.reset(); + } + + // Update simulation state + flywheelSim.setInputVoltage( + MathUtil.clamp( + appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); + flywheelSim.update(Robot.defaultPeriodSecs); + + // Update turn inputs + inputs.connected = true; + inputs.velocityRadPerSec = flywheelSim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = appliedVolts; + inputs.currentAmps = Math.abs(flywheelSim.getCurrentDrawAmps()); + } + + @Override + public void setOpenLoop(double output) { + closedLoop = false; + appliedVolts = output; + } + + @Override + public void setVelocity(AngularVelocity angularVelocity) { + closedLoop = true; + this.feedforwardVolts = + RobotConstants.kNominalVoltage + * angularVelocity.in(RadiansPerSecond) + / maxAngularVelocity.in(RadiansPerSecond); + velocityController.setSetpoint(angularVelocity.in(RadiansPerSecond)); + } +} diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java new file mode 100644 index 0000000..aaa82fa --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java @@ -0,0 +1,80 @@ +package frc.robot.subsystems.launcher; + +import static frc.robot.subsystems.drive.DriveConstants.kCANBus; +import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*; +import static frc.robot.util.PhoenixUtil.tryUntilOk; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.subsystems.drive.PhoenixOdometryThread; +import java.util.Queue; + +public class FlywheelIOTalonFX implements FlywheelIO { + private final TalonFX flywheelTalon; + private final TalonFXConfiguration config; + private final Debouncer turnConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + // Voltage control requests + private final VoltageOut voltageRequest = new VoltageOut(0); + private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + + // Torque-current control requests + private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); + private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = + new PositionTorqueCurrentFOC(0.0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + new VelocityTorqueCurrentFOC(0.0); + + // Timestamp inputs from Phoenix thread + private final Queue timestampQueue; + + // Inputs from drive motor + private final StatusSignal flywheelPosition; + private final Queue flywheelPositionQueue; + private final StatusSignal flywheelVelocity; + private final StatusSignal flywheelAppliedVolts; + private final StatusSignal flywheelCurrent; + + public FlywheelIOTalonFX() { + flywheelTalon = new TalonFX(port, kCANBus); + // Configuration + config = new TalonFXConfiguration(); + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.Slot0 = flywheelGains; + tryUntilOk(5, () -> flywheelTalon.getConfigurator().apply(config, 0.25)); + + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + flywheelPosition = flywheelTalon.getPosition(); + flywheelPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(flywheelPosition.clone()); + flywheelVelocity = flywheelTalon.getVelocity(); + flywheelAppliedVolts = flywheelTalon.getMotorVoltage(); + flywheelCurrent = flywheelTalon.getStatorCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent, + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent); + } +} diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java new file mode 100644 index 0000000..7e672fb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java @@ -0,0 +1,68 @@ +package frc.robot.subsystems.launcher; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.*; + +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; +import frc.robot.Robot; + +public class HoodIOSim implements HoodIO { + + private final DCMotorSim hoodSim; + + private boolean closedLoop = false; + private PIDController positionController = new PIDController(kPSim, 0.0, kDSim); + private double appliedVolts = 0.0; + private double feedforwardVolts = 0.0; + + public HoodIOSim() { + hoodSim = + new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + } + + @Override + public void updateInputs(HoodIOInputs inputs) { + // Run closed-loop control + if (closedLoop) { + appliedVolts = + positionController.calculate(hoodSim.getAngularPositionRad()) + feedforwardVolts; + } else { + positionController.reset(); + } + + // Update simulation state + hoodSim.setInputVoltage( + MathUtil.clamp( + appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); + hoodSim.update(Robot.defaultPeriodSecs); + + // Update turn inputs + inputs.connected = true; + inputs.position = new Rotation2d(hoodSim.getAngularPositionRad()); + inputs.velocityRadPerSec = hoodSim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = appliedVolts; + inputs.currentAmps = Math.abs(hoodSim.getCurrentDrawAmps()); + } + + @Override + public void setOpenLoop(double output) { + closedLoop = false; + appliedVolts = output; + } + + @Override + public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { + closedLoop = true; + this.feedforwardVolts = + RobotConstants.kNominalVoltage + * angularVelocity.in(RadiansPerSecond) + / maxAngularVelocity.in(RadiansPerSecond); + positionController.setSetpoint(rotation.getRadians()); + } +} diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java new file mode 100644 index 0000000..4e23ed4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java @@ -0,0 +1,109 @@ +package frc.robot.subsystems.launcher; + +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.*; +import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.port; +import static frc.robot.util.SparkUtil.*; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +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; + +public class HoodIOSpark implements HoodIO { + + private final SparkBase hoodSpark; + private final AbsoluteEncoder hoodEncoder; + private final SparkClosedLoopController hoodController; + private final Debouncer turnConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + public HoodIOSpark() { + hoodSpark = new SparkMax(port, MotorType.kBrushless); + hoodEncoder = hoodSpark.getAbsoluteEncoder(); + hoodController = hoodSpark.getClosedLoopController(); + + var hoodConfig = new SparkMaxConfig(); + + hoodConfig + .inverted(false) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(NEO550Constants.kDefaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.kNominalVoltage); + + hoodConfig + .absoluteEncoder + .inverted(false) + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor) + .averageDepth(2); + + hoodConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .positionWrappingEnabled(true) + .positionWrappingInputRange(minInput, maxInput) + .pid(kPReal, 0.0, 0.0); + + hoodConfig + .signals + .absoluteEncoderPositionAlwaysOn(true) + .absoluteEncoderPositionPeriodMs(20) + .absoluteEncoderVelocityAlwaysOn(true) + .absoluteEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + + tryUntilOk( + hoodSpark, + 5, + () -> + hoodSpark.configure( + hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + } + + @Override + public void updateInputs(HoodIOInputs inputs) { + sparkStickyFault = false; + ifOk(hoodSpark, hoodEncoder::getPosition, (value) -> inputs.position = new Rotation2d(value)); + ifOk(hoodSpark, hoodEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); + ifOk( + hoodSpark, + new DoubleSupplier[] {hoodSpark::getAppliedOutput, hoodSpark::getBusVoltage}, + (values) -> inputs.appliedVolts = values[0] * values[1]); + ifOk(hoodSpark, hoodSpark::getOutputCurrent, (value) -> inputs.currentAmps = value); + inputs.connected = turnConnectedDebounce.calculate(!sparkStickyFault); + } + + @Override + public void setOpenLoop(double output) { + hoodSpark.setVoltage(output); + } + + @Override + public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { + double setpoint = MathUtil.inputModulus(rotation.getRadians(), minInput, maxInput); + double feedforwardVolts = + RobotConstants.kNominalVoltage + * angularVelocity.in(RadiansPerSecond) + / maxAngularVelocity.in(RadiansPerSecond); + hoodController.setSetpoint( + setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); + } +} diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index ca95ad4..c00e959 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -40,7 +40,8 @@ public class Launcher extends SubsystemBase { private final Alert flywheelDisconnectedAlert; private final Alert hoodDisconnectedAlert; - private Translation3d vectorTurretBaseToHub = new Translation3d(); + private Translation3d target = new Translation3d(); + private Translation3d vectorTurretBaseToTarget = new Translation3d(); private Pose3d turretBasePose = new Pose3d(); // private Translation3d v0_nominal = new Translation3d(); @@ -82,20 +83,10 @@ public void periodic() { 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(fuelReplanned); - updateBallisticsSim(fuelActual); - - Logger.recordOutput( - "Launcher/" + nominalKey + "/FuelTrajectory", getBallTrajectory(fuelNominal)); - Logger.recordOutput( - "Launcher/" + replannedKey + "/FuelTrajectory", getBallTrajectory(fuelReplanned)); - Logger.recordOutput("Launcher/" + actualKey + "/FuelTrajectory", getBallTrajectory(fuelActual)); + // Update and plot ball trajectories + updateBallisticsSim(fuelNominal, nominalKey); + updateBallisticsSim(fuelReplanned, replannedKey); + updateBallisticsSim(fuelActual, actualKey); } public void stop() { @@ -104,9 +95,15 @@ public void stop() { hoodIO.setOpenLoop(0.0); } - public void aimAtHub() { + public void aim(Translation3d target) { + this.target = target; + + // Get vector from static target to turret + turretBasePose = new Pose3d(chassisPoseSupplier.get()).plus(chassisToTurretBase); + vectorTurretBaseToTarget = target.minus(turretBasePose.getTranslation()); + // Set flywheel speed assuming a motionless robot - var v0_nominal = getV0(vectorTurretBaseToHub, impactAngle, nominalKey); + var v0_nominal = getV0(vectorTurretBaseToTarget, impactAngle, nominalKey); AngularVelocity flywheelSetpoint = RadiansPerSecond.of(v0_nominal.getNorm() / wheelRadius.in(Meters)); flywheelIO.setVelocity(flywheelSetpoint); @@ -119,26 +116,31 @@ public void aimAtHub() { var v_base = getTurretBaseSpeeds(turretBasePose.toPose2d().getRotation(), fieldRelative); // Get actual flywheel speed - // TODO: Replace with actual speed when flywheel simulation is added LinearVelocity flywheelSpeed = - MetersPerSecond.of(flywheelSetpoint.in(RadiansPerSecond) * wheelRadius.in(Meters)); + MetersPerSecond.of(flywheelInputs.velocityRadPerSec * wheelRadius.in(Meters)); // Replan shot using actual flywheel speed - var v0_total = getV0(vectorTurretBaseToHub, flywheelSpeed, replannedKey); + var v0_total = getV0(vectorTurretBaseToTarget, flywheelSpeed, replannedKey); // Point turret to align velocity vectors var v0_flywheel = v0_total.minus(v_base); + + // Check if v0_flywheel has non-zero horizontal component + double v0_horizontal = Math.hypot(v0_flywheel.getX(), v0_flywheel.getY()); + if (v0_horizontal < 1e-6) { + // Flywheel velocity is too low or target unreachable, stop mechanisms + return; + } + Rotation2d turretSetpoint = new Rotation2d(v0_flywheel.getX(), v0_flywheel.getY()); turretIO.setPosition( turretSetpoint.minus(turretBasePose.toPose2d().getRotation()), RadiansPerSecond.of(robotRelative.omegaRadiansPerSecond).unaryMinus().times(2.0)); - Rotation2d hoodSetpoint = - new Rotation2d(Math.hypot(v0_flywheel.getX(), v0_flywheel.getY()), v0_flywheel.getZ()); + Rotation2d hoodSetpoint = new Rotation2d(v0_horizontal, v0_flywheel.getZ()); hoodIO.setPosition(hoodSetpoint, RadiansPerSecond.of(0)); // Get actual hood & turret position - // TODO: Replace with actual hood position when hood simulation is added - Rotation2d hoodPosition = hoodSetpoint; + Rotation2d hoodPosition = hoodInputs.position; Rotation2d turretPosition = turretInputs.position.plus(turretBasePose.toPose2d().getRotation()); // Build actual velocities @@ -149,7 +151,7 @@ public void aimAtHub() { hoodPosition.getSin()) .times(flywheelSpeed.in(MetersPerSecond)) .plus(v_base); - log(vectorTurretBaseToHub, v0_actual, actualKey); + log(vectorTurretBaseToTarget, v0_actual, actualKey); // Spawn simulated fuel fuelSpawnTimer += Robot.defaultPeriodSecs; @@ -304,7 +306,7 @@ private static class BallisticObject { } } - private void updateBallisticsSim(ArrayList traj) { + private void updateBallisticsSim(ArrayList traj, String key) { double dt = Robot.defaultPeriodSecs; double hubZ = GameState.getMyHubPose().getZ(); @@ -319,6 +321,8 @@ private void updateBallisticsSim(ArrayList traj) { // Remove when below target height and falling return o.position.getZ() < hubZ && o.velocity.getZ() < 0; }); + + Logger.recordOutput("Launcher/" + key + "/FuelTrajectory", getBallTrajectory(traj)); } public Translation3d[] getBallTrajectory(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 594533f..54b1ac5 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -2,13 +2,15 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; 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.KrakenX60Constants; import frc.robot.Constants.MotorConstants.NEO550Constants; public final class LauncherConstants { @@ -17,8 +19,9 @@ public final class LauncherConstants { public static final Distance fuelRadius = Inches.of(3); public static final Distance ceilingHeight = Feet.of(11).plus(Inches.of(2)); public static final double g = 9.81; - public static final double fuelSpawnPeriod = 0.1; // seconds + // For logging + public static final double fuelSpawnPeriod = 0.1; // seconds public static final String nominalKey = "Nominal"; public static final String replannedKey = "Replanned"; public static final String actualKey = "Actual"; @@ -28,31 +31,72 @@ public static final class TurretConstants { 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 + public static final double encoderPositionFactor = 2 * Math.PI; // Radians + public static final double encoderVelocityFactor = (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); + public static final double minInput = 0.0; + public static final double maxInput = 2 * Math.PI; + public static final double kPReal = 0.35; // Motor controller public static final int port = 12; - public static final double turnMotorReduction = 5.0; - public static final AngularVelocity turnMaxAngularVelocity = - NEO550Constants.kFreeSpeed.div(turnMotorReduction); + public static final double motorReduction = 5.0; + public static final AngularVelocity maxAngularVelocity = + NEO550Constants.kFreeSpeed.div(motorReduction); // 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 DCMotor gearbox = DCMotor.getNeo550(1); + public static final double kPSim = 0.8; + public static final double kDSim = 0.05; } public static final class FlywheelConstants { public static final Distance wheelRadius = Inches.of(1.5); + + // Velocity Controller + public static final Slot0Configs flywheelGains = + new Slot0Configs() + .withKP(1.0) + .withKI(0.0) + .withKD(1.0) + .withKS(0.0) + .withKV(0.0) + .withKA(0.0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + // Motor controller + public static final int port = 2; + public static final double motorReduction = 1.0; + public static final AngularVelocity maxAngularVelocity = + KrakenX60Constants.kFreeSpeed.div(motorReduction); + + // Simulation + public static final double kPSim = 0.1; + public static final DCMotor gearbox = DCMotor.getKrakenX60(1); + } + + public static final class HoodConstants { + // Absolute encoder + public static final double encoderPositionFactor = 2 * Math.PI; // Radians + public static final double encoderVelocityFactor = (2 * Math.PI) / 60.0; // Rad/sec + + // Position controller + public static final double minInput = 0.0; + public static final double maxInput = 2 * Math.PI; + public static final double kPReal = 0.35; + + // Motor controller + public static final int port = 1; + public static final double motorReduction = 2.75; + public static final AngularVelocity maxAngularVelocity = + NEO550Constants.kFreeSpeed.div(motorReduction); + + // Simulation + public static final double kPSim = 0.2; + public static final double kDSim = 0.05; + public static final DCMotor gearbox = DCMotor.getNeo550(1); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java index 62c0605..d39e36c 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java @@ -10,60 +10,61 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants.RobotConstants; +import frc.robot.Robot; public class TurretIOSim implements TurretIO { private final DCMotorSim turnSim; - private boolean turnClosedLoop = false; - private PIDController turnController = new PIDController(turnKpSim, 0.0, turnKdSim); - private double turnAppliedVolts = 0.0; + private boolean closedLoop = false; + private PIDController positionController = new PIDController(kPSim, 0.0, kDSim); + private double appliedVolts = 0.0; private double feedforwardVolts = 0.0; public TurretIOSim() { turnSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(turnGearbox, 0.004, turnMotorReduction), - turnGearbox); + new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); // Enable wrapping for turn PID - turnController.enableContinuousInput(-Math.PI, Math.PI); + positionController.enableContinuousInput(-Math.PI, Math.PI); } @Override public void updateInputs(TurretIOInputs inputs) { // Run closed-loop control - if (turnClosedLoop) { - turnAppliedVolts = - turnController.calculate(turnSim.getAngularPositionRad()) + feedforwardVolts; + if (closedLoop) { + appliedVolts = + positionController.calculate(turnSim.getAngularPositionRad()) + feedforwardVolts; } else { - turnController.reset(); + positionController.reset(); } // Update simulation state - turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); - turnSim.update(0.02); + turnSim.setInputVoltage( + MathUtil.clamp( + appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); + turnSim.update(Robot.defaultPeriodSecs); // Update turn inputs inputs.connected = true; inputs.position = new Rotation2d(turnSim.getAngularPositionRad()); inputs.velocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); - inputs.appliedVolts = turnAppliedVolts; + inputs.appliedVolts = appliedVolts; inputs.currentAmps = Math.abs(turnSim.getCurrentDrawAmps()); } @Override public void setOpenLoop(double output) { - turnClosedLoop = false; - turnAppliedVolts = output; + closedLoop = false; + appliedVolts = output; } @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { - turnClosedLoop = true; + closedLoop = true; this.feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond) - / turnMaxAngularVelocity.in(RadiansPerSecond); - turnController.setSetpoint(rotation.getRadians()); + / maxAngularVelocity.in(RadiansPerSecond); + positionController.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 8d6a748..cf131ba 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -48,16 +48,16 @@ public TurretIOSpark() { turnConfig .absoluteEncoder .inverted(false) - .positionConversionFactor(turnEncoderPositionFactor) - .velocityConversionFactor(turnEncoderVelocityFactor) + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor) .averageDepth(2); turnConfig .closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) - .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) - .pid(turnKp, 0.0, 0.0); + .positionWrappingInputRange(minInput, maxInput) + .pid(kPReal, 0.0, 0.0); turnConfig .signals @@ -101,12 +101,11 @@ public void setOpenLoop(double output) { @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = - MathUtil.inputModulus( - rotation.plus(rotationOffset).getRadians(), turnPIDMinInput, turnPIDMaxInput); + MathUtil.inputModulus(rotation.plus(rotationOffset).getRadians(), minInput, maxInput); double feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond) - / turnMaxAngularVelocity.in(RadiansPerSecond); + / maxAngularVelocity.in(RadiansPerSecond); turnController.setSetpoint( setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); }