diff --git a/emsdk b/emsdk new file mode 160000 index 00000000..d6b88f4f --- /dev/null +++ b/emsdk @@ -0,0 +1 @@ +Subproject commit d6b88f4ffd8d6163aadb6ff48ca4b32ceec890dd diff --git a/simgui-ds.json b/simgui-ds.json index b681e123..c3c4b0c1 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -125,4 +125,4 @@ "guid": "Keyboard1" } ] -} +} \ No newline at end of file diff --git a/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java b/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java index 33ccfba4..b09b358b 100644 --- a/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java +++ b/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java @@ -378,21 +378,6 @@ public boolean isNear(Pose2d target, Distance translationTolerance, Angle angula && angularError <= angularTolerance.in(Radians); } - /** - * Checks if the robot is within a translational tolerance of a target pose. - * - * @param target The target pose to check against - * @param translationTolerance Maximum allowed translation error from target position - * @return {@code true} if within the translational tolerance, {@code false} otherwise - */ - public boolean isNear(Pose2d target, Distance translationTolerance) { - Pose2d current = getPosition2d(); - - double linearError = current.getTranslation().getDistance(target.getTranslation()); - - return linearError <= translationTolerance.in(Meters); - } - /** * Gets the current field-relative velocity of the robot as a {@link ChassisSpeeds} object. * diff --git a/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java b/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java index 4630c391..7062ad8f 100644 --- a/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java +++ b/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java @@ -1,10 +1,8 @@ package com.team6962.lib.swerve.commands; import static edu.wpi.first.units.Units.Hertz; -import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Seconds; import com.team6962.lib.control.MotionProfile; import com.team6962.lib.control.ProfiledController; @@ -13,15 +11,11 @@ import com.team6962.lib.math.AngleMath; import com.team6962.lib.math.TranslationalVelocity; import com.team6962.lib.swerve.CommandSwerveDrive; -import dev.doglog.DogLog; -import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.numbers.N2; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; @@ -187,13 +181,6 @@ public void initialize() { // Generate initial motion profiles createMotionProfiles(); - - DogLog.log( - "Drivetrain/DriveToState/FinalTarget", - new Pose2d(target.translation, new Rotation2d(target.angle))); - DogLog.log("Drivetrain/DriveToState/FinalVelocityX", target.translationalVelocity.x); - DogLog.log("Drivetrain/DriveToState/FinalVelocityY", target.translationalVelocity.y); - DogLog.log("Drivetrain/DriveToState/FinalAngularVelocity", target.angularVelocity); } /** Creates motion profiles for translation and rotation controllers. */ @@ -240,38 +227,16 @@ public void execute() { createMotionProfiles(); } - // Current target pose and speeds for logging + // Current target pose for logging Pose2d currentTarget = new Pose2d(); - ChassisSpeeds currentSpeeds = new ChassisSpeeds(); // Calculate and apply velocity commands for translation and rotation if (translationController != null) { - TranslationalVelocity currentTranslationalVelocity = + TranslationalVelocity outputTranslationalVelocity = translationController.calculate( swerveDrive.getPosition2d().getTranslation(), swerveDrive.getTranslationalVelocity()); - currentSpeeds = - new ChassisSpeeds( - currentTranslationalVelocity.x.in(MetersPerSecond), - currentTranslationalVelocity.y.in(MetersPerSecond), - currentSpeeds.omegaRadiansPerSecond); - - TranslationalVelocity profileTranslationalVelocity = translationController.sample(0); - - TranslationalVelocity nextTranslationalVelocity = translationController.sample(0.02); - - Vector acceleration = - nextTranslationalVelocity - .toVector() - .minus(profileTranslationalVelocity.toVector()) - .div(0.02); - - currentTranslationalVelocity = - currentTranslationalVelocity.plus( - new TranslationalVelocity(acceleration) - .times(swerveDrive.getConstants().Driving.AutoLinearAccelerationScalar)); - - swerveDrive.applyVelocityMotion(currentTranslationalVelocity); + swerveDrive.applyVelocityMotion(outputTranslationalVelocity); // Add translation to current target pose currentTarget = @@ -280,32 +245,14 @@ public void execute() { } if (headingController != null) { - MotionProfile.State angularState = - new MotionProfile.State( - swerveDrive.getYaw().in(Radians), swerveDrive.getYawVelocity().in(RadiansPerSecond)); + AngularVelocity outputAngularVelocity = + RadiansPerSecond.of( + headingController.calculate( + new MotionProfile.State( + swerveDrive.getYaw().in(Radians), + swerveDrive.getYawVelocity().in(RadiansPerSecond)))); - AngularVelocity currentAngularVelocity = - RadiansPerSecond.of(headingController.calculate(angularState)); - - currentSpeeds = - new ChassisSpeeds( - currentSpeeds.vxMetersPerSecond, - currentSpeeds.vyMetersPerSecond, - currentAngularVelocity.in(RadiansPerSecond)); - - AngularVelocity profileAngularVelocity = RadiansPerSecond.of(headingController.sample(0)); - - AngularVelocity nextAngularVelocity = RadiansPerSecond.of(headingController.sample(0.02)); - - AngularAcceleration angularAcceleration = - nextAngularVelocity.minus(profileAngularVelocity).div(Seconds.of(0.02)); - - currentAngularVelocity = - currentAngularVelocity.plus( - angularAcceleration.times( - Seconds.of(swerveDrive.getConstants().Driving.AutoAngularAccelerationScalar))); - - swerveDrive.applyVelocityMotion(currentAngularVelocity); + swerveDrive.applyVelocityMotion(outputAngularVelocity); // Add rotation to current target pose currentTarget = @@ -316,20 +263,6 @@ public void execute() { // Log current target pose swerveDrive.getFieldLogger().getField().getObject("Current Target").setPose(currentTarget); - - DogLog.log("Drivetrain/DriveToState/ProfilePose", currentTarget); - DogLog.log( - "Drivetrain/DriveToState/ProfileVelocityX", - currentSpeeds.vxMetersPerSecond, - MetersPerSecond); - DogLog.log( - "Drivetrain/DriveToState/ProfileVelocityY", - currentSpeeds.vyMetersPerSecond, - MetersPerSecond); - DogLog.log( - "Drivetrain/DriveToState/ProfileAngularVelocity", - currentSpeeds.omegaRadiansPerSecond, - RadiansPerSecond); } @Override diff --git a/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java b/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java index 45ec4092..917c4529 100644 --- a/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java +++ b/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java @@ -1,11 +1,7 @@ package com.team6962.lib.swerve.motion; -import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Hertz; import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.NewtonMeters; -import static edu.wpi.first.units.Units.Newtons; -import static edu.wpi.first.units.Units.Ohms; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; @@ -25,12 +21,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Force; import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Resistance; -import edu.wpi.first.units.measure.Torque; -import edu.wpi.first.units.measure.Voltage; /** * A swerve motion that drives the robot at a specified field-relative velocity. @@ -64,35 +55,18 @@ public class VelocityMotion implements SwerveMotion { /** Whether this motion has a non-zero rotation component. */ private final boolean hasRotation; - /** The feedforward forces for each swerve module, which can be null if not used. */ - private final Force[] forceFeedforwards; - /** * Creates a new VelocityMotion with the specified field-relative velocity. * * @param velocity The target field-relative chassis speeds - * @param forceFeedforwards The feedforward forces for each swerve module, which can be null if - * not used * @param swerveDrive The swerve drive to control */ - public VelocityMotion( - ChassisSpeeds velocity, Force[] forceFeedforwards, MotionSwerveDrive swerveDrive) { + public VelocityMotion(ChassisSpeeds velocity, MotionSwerveDrive swerveDrive) { this.velocity = velocity; this.hasTranslation = Math.abs(velocity.vxMetersPerSecond) > 0.01 || Math.abs(velocity.vyMetersPerSecond) > 0.01; this.hasRotation = Math.abs(velocity.omegaRadiansPerSecond) > 0.01; this.swerveDrive = swerveDrive; - this.forceFeedforwards = forceFeedforwards; - } - - /** - * Creates a new VelocityMotion with the specified field-relative velocity. - * - * @param velocity The target field-relative chassis speeds - * @param swerveDrive The swerve drive to control - */ - public VelocityMotion(ChassisSpeeds velocity, MotionSwerveDrive swerveDrive) { - this(velocity, null, swerveDrive); } /** @@ -114,14 +88,6 @@ public SwerveMotion fuseWith(SwerveMotion other) { "Cannot fuse two VelocityMotions with overlapping translation or rotation components."); } - if (!otherVelocityMotion.hasRotation && !otherVelocityMotion.hasTranslation) { - return this; - } - - if (!hasRotation && !hasTranslation) { - return otherVelocityMotion; - } - return new VelocityMotion( SwerveKinematicsUtil.addChassisSpeeds(velocity, otherVelocityMotion.velocity), swerveDrive); @@ -187,17 +153,6 @@ public void update(double deltaTimeSeconds) { LinearVelocity driveVelocity = MetersPerSecond.of(state.speedMetersPerSecond); Angle steerAngle = state.angle.getMeasure(); - Force ffForce = forceFeedforwards != null ? forceFeedforwards[i] : Newtons.of(0); - Torque ffWheelTorque = swerveDrive.getConstants().getWheelRadius(i).times(ffForce); - Torque ffMotorTorque = ffWheelTorque.div(swerveDrive.getConstants().DriveMotor.GearReduction); - double ffMotorTorqueNm = ffMotorTorque.in(NewtonMeters); - double motorKT = swerveDrive.getConstants().DriveMotor.SimulatedMotor.KtNMPerAmp; - double ffCurrentAmps = ffMotorTorqueNm / motorKT; - Current ffCurrent = Amps.of(ffCurrentAmps); - Resistance windingResistance = - Ohms.of(swerveDrive.getConstants().DriveMotor.SimulatedMotor.rOhms); - Voltage ffVoltage = ffCurrent.times(windingResistance); - module.setControl( new VelocityControlRequest( WheelMath.toAngular(driveVelocity, swerveDrive.getConstants().getWheelRadius(i)) @@ -207,7 +162,6 @@ public void update(double deltaTimeSeconds) { .withSlot(driveConstants.VelocitySlot) .withUpdateFreqHz(updateFrequencyHz) .withUseTimesync(useTimesync) - .withAdditionalFeedforward(ffVoltage) .toControlRequest(), new PositionControlRequest(steerAngle.in(Rotations)) .withMotionProfileType(steerConstants.PositionControlMotionProfile) diff --git a/src/main/java/com/team6962/lib/swerve/pathplanner/PathPlanner.java b/src/main/java/com/team6962/lib/swerve/pathplanner/PathPlanner.java index e0c5b103..4eef2a8c 100644 --- a/src/main/java/com/team6962/lib/swerve/pathplanner/PathPlanner.java +++ b/src/main/java/com/team6962/lib/swerve/pathplanner/PathPlanner.java @@ -68,7 +68,7 @@ public PathPlanner(CommandSwerveDrive drivetrain) { } } - drivetrain.applyMotion(new VelocityMotion(appliedSpeeds, forces, drivetrain)); + drivetrain.applyMotion(new VelocityMotion(appliedSpeeds, drivetrain)); }, new PPHolonomicDriveController( new PIDConstants( diff --git a/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java b/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java index b5fcd3a2..f14a101a 100644 --- a/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java +++ b/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java @@ -7,7 +7,6 @@ import com.team6962.lib.swerve.localization.Odometry; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -41,43 +40,6 @@ public class FieldLogger implements SwerveComponent { /** Cached previous pose to detect user-initiated changes. */ private Pose2d previousRobotPose; - /** Notifier for periodic field logging updates. */ - private Notifier fieldLoggerNotifier; - - /** Data structure to hold the latest robot and module poses for logging. */ - private LogData logData; - - /** - * Data structure to hold the robot pose and module poses for logging. This is cloned before being - * accessed in the logging thread to avoid concurrency issues. - */ - private static class LogData implements Cloneable { - /** The robot's current pose on the field. */ - public Pose2d robotPose; - - /** The poses of the swerve modules relative to the robot. */ - public Pose2d[] modulePoses; - - public LogData(Pose2d robotPose, Pose2d[] modulePoses) { - this.robotPose = robotPose; - this.modulePoses = modulePoses; - } - - @Override - public LogData clone() { - Pose2d clonedRobotPose = new Pose2d(robotPose.getTranslation(), robotPose.getRotation()); - Pose2d[] clonedModulePoses = null; - if (modulePoses != null) { - clonedModulePoses = new Pose2d[modulePoses.length]; - for (int i = 0; i < modulePoses.length; i++) { - clonedModulePoses[i] = - new Pose2d(modulePoses[i].getTranslation(), modulePoses[i].getRotation()); - } - } - return new LogData(clonedRobotPose, clonedModulePoses); - } - } - /** * Creates a new FieldLogger, which logs robot and module poses to NetworkTables. * @@ -89,33 +51,6 @@ public FieldLogger(DrivetrainConstants constants, Localization localization, Odo this.constants = constants; this.localization = localization; this.odometry = odometry; - fieldLoggerNotifier = new Notifier(this::threadedLog); - fieldLoggerNotifier.setName("FieldLogger"); - fieldLoggerNotifier.startPeriodic(0.02); - } - - /** - * Logs the robot pose and module poses to the Field2d widget. This method is called periodically - * by the Notifier in a separate thread. - */ - private void threadedLog() { - LogData dataCopy; - - synchronized (this) { - if (logData == null) { - return; - } - - dataCopy = logData.clone(); - } - - if (dataCopy.robotPose != null) { - field.setRobotPose(dataCopy.robotPose); - } - - if (dataCopy.modulePoses != null) { - field.getObject("Swerve Modules").setPoses(dataCopy.modulePoses); - } } @Override @@ -134,10 +69,13 @@ public void logTelemetry(String basePath) { } Pose2d robotPose = localization.getPosition2d(); - Pose2d[] modulePoses = null; + + previousRobotPose = robotPose; + + field.setRobotPose(robotPose); if (!constants.Timing.MinimizeLogging) { - modulePoses = new Pose2d[4]; + Pose2d[] modulePoses = new Pose2d[4]; SwerveModuleState[] moduleStates = odometry.getStates(); for (int i = 0; i < 4; i++) { @@ -149,15 +87,9 @@ public void logTelemetry(String basePath) { modulePoses[i] = robotPose.plus(relativePose.minus(new Pose2d())); } - } - previousRobotPose = robotPose; - - synchronized (this) { - logData = new LogData(robotPose, modulePoses); + field.getObject("Swerve Modules").setPoses(modulePoses); } - - field.setRobotPose(robotPose); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b759b801..9cbb0d33 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import frc.robot.auto.shoot.ShooterFunctions; import frc.robot.constants.RobotConstants; import frc.robot.controls.TeleopControls; +import frc.robot.subsystems.BrownoutProtection; import frc.robot.subsystems.hood.ShooterHood; import frc.robot.subsystems.hopper.Hopper; import frc.robot.subsystems.intakeextension.IntakeExtension; @@ -54,6 +55,7 @@ public class RobotContainer { private final ShooterFunctions hubFunctions; private final ShooterFunctions passFunctions; private final Autonomous autonomous; + private final BrownoutProtection brownoutProtection; private final Command noneAutonomous = Commands.none(); public RobotContainer() { @@ -85,6 +87,18 @@ public RobotContainer() { teleopControls = new TeleopControls(this); teleopControls.configureBindings(); + brownoutProtection = + new BrownoutProtection( + swerveDrive, + teleopControls.getTeleopSwerveCommand(), + teleopControls.getShootingActiveTrigger(), + teleopControls.getIntakingActiveTrigger(), + turret, + shooterHood, + intakeExtension, + hopper.getBeltFloor(), + intakeRollers); + driveStraightAuto = new DriveStraightAuto(this); autonomous = new Autonomous(this); diff --git a/src/main/java/frc/robot/constants/CompetitionBotSimConstants.java b/src/main/java/frc/robot/constants/CompetitionBotSimConstants.java new file mode 100644 index 00000000..13dbdd94 --- /dev/null +++ b/src/main/java/frc/robot/constants/CompetitionBotSimConstants.java @@ -0,0 +1,38 @@ +package frc.robot.constants; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; + +import com.team6962.lib.swerve.config.DrivetrainConstants; + +/** + * Simulation-only tuning for the competition robot. + * + *

This keeps simulation realism improvements isolated from the real robot constants so we can + * make autos and driver practice behave better in sim without changing match behavior on hardware. + */ +public class CompetitionBotSimConstants extends CompetitionBotConstants { + @Override + public DrivetrainConstants getDrivetrainConstants() { + DrivetrainConstants constants = super.getDrivetrainConstants(); + + return constants + .withStructure(constants.Structure.clone().estimateMomentOfInertia()) + .withDriving( + constants.Driving.clone() + .withAutoLinearVelocity(MetersPerSecond.of(3.2)) + .withAutoLinearAcceleration(MetersPerSecondPerSecond.of(2.6)) + .withAutoAngularVelocity(RotationsPerSecond.of(0.85)) + .withAutoAngularAcceleration(RotationsPerSecondPerSecond.of(0.4)) + .withTranslationFeedbackKP(8.0) + .withTranslationFeedbackKD(0.45) + .withAngleFeedbackKP(16.0) + .withAngleFeedbackKD(0.9)) + .withSimulation( + constants.Simulation.clone() + .withSimulationSubTicksPerPeriod(4) + .withEnableRampCollider(true)); + } +} diff --git a/src/main/java/frc/robot/controls/TeleopControls.java b/src/main/java/frc/robot/controls/TeleopControls.java index 299995bc..6f9f3a43 100644 --- a/src/main/java/frc/robot/controls/TeleopControls.java +++ b/src/main/java/frc/robot/controls/TeleopControls.java @@ -41,12 +41,16 @@ public class TeleopControls { private CommandXboxController operator = new CommandXboxController(1); private Distance shootingTestDistance = Inches.of(206); private AutoDepot autoDepot; + private XBoxTeleopSwerveCommand teleopSwerveCommand; private boolean fineControl = false; private AngularVelocity flywheelVelocity = ShooterRollersConstants.FIXED_FLYWHEEL_VELOCITY; private double tunableHoodAngle = 0; private double tunableRollerVelocity = 0; + private Trigger shootingActiveTrigger; + private Trigger intakingActiveTrigger; + public TeleopControls(RobotContainer robot) { this.robot = robot; // this.autoClimb = new AutoClimb(robot); @@ -97,7 +101,7 @@ public void configureBindings() { Trigger teleopEnabledTrigger = new Trigger(() -> RobotState.isTeleop() && RobotState.isEnabled()); - XBoxTeleopSwerveCommand teleopSwerveCommand = + teleopSwerveCommand = new XBoxTeleopSwerveCommand( robot.getSwerveDrive(), robot.getConstants().getTeleopSwerveConstants()); @@ -247,11 +251,11 @@ public void configureBindings() { // Intake extension and retraction - WORKS Trigger intakeRetract = operator.rightStick(); - Trigger intakeExtend = + intakingActiveTrigger = intakeRetract.negate().and(RobotState::isTeleop).and(RobotState::isEnabled); intakeRetract.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().retract()); - intakeExtend.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().extend()); + intakingActiveTrigger.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().extend()); // Automatically load fuel into the kicker when there is fuel in the hopper - WORKS, but not // fully tested @@ -294,39 +298,12 @@ public void configureBindings() { robot.getSwerveDrive().getPosition2d().getX() < TrenchDriving.OBSTACLES_CENTER_X.in(Meters)); - Trigger autoshootTrigger = - new Trigger(RobotState::isTeleop) - .and(RobotState::isEnabled) - .and(inAllianceZone) - .and(operator.a().negate()) - .and(operator.y().negate()) - .and(() -> !fineControl); - - autoshootTrigger.whileTrue(autoShoot); - - AutoShoot autoPass = - new AutoShoot( - robot.getSwerveDrive(), - robot.getTurret(), - robot.getShooterHood(), - robot.getShooterRollers(), - robot.getPassFunctions(), - () -> - robot.getSwerveDrive().getPosition2d().getY() > AutoShoot.HUB_TRANSLATION.getY() - ? AutoShoot.PASS_RIGHT_TRANSLATION - : AutoShoot.PASS_LEFT_TRANSLATION, - () -> tunableHoodAngle == 0 ? null : Degrees.of(tunableHoodAngle), - () -> tunableRollerVelocity == 0 ? null : RotationsPerSecond.of(tunableRollerVelocity)); - - Trigger autoPassTrigger = - new Trigger(RobotState::isTeleop) - .and(RobotState::isEnabled) - .and(inAllianceZone.negate()) - .and(operator.a().negate()) - .and(operator.y().negate()) - .and(() -> !fineControl); - - autoPassTrigger.whileTrue(autoPass); + shootingActiveTrigger = + new Trigger(RobotState::isTeleop) + .and(RobotState::isEnabled) + .and(operator.a().negate()) + .and(operator.y().negate()) + .and(() -> !fineControl); Trigger shootButtonsTrigger = operator.rightTrigger().or(driver.back()); @@ -343,7 +320,7 @@ public void configureBindings() { .whileTrue( teleopSwerveCommand.limitVelocity( MetersPerSecond.of(1.5), RotationsPerSecond.of(0.5))) // Temporary values - .and(autoPass.isReadyToShoot()) + .and(autoShoot.isReadyToShoot()) .whileTrue(robot.getHopper().feed()); // ShooterFunctions functions = robot.getHubFunctions(); @@ -392,4 +369,16 @@ private Command fineControlEnableRumble() { private Command fineControlDisableRumble() { return Commands.sequence(rumble(operator, RumbleType.kBothRumble, 1).withTimeout(1.0)); } + + public XBoxTeleopSwerveCommand getTeleopSwerveCommand() { + return teleopSwerveCommand; + } + + public Trigger getShootingActiveTrigger() { + return shootingActiveTrigger; + } + + public Trigger getIntakingActiveTrigger() { + return intakingActiveTrigger; + } } diff --git a/src/main/java/frc/robot/subsystems/BrownoutProtection.java b/src/main/java/frc/robot/subsystems/BrownoutProtection.java new file mode 100644 index 00000000..6185ab60 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/BrownoutProtection.java @@ -0,0 +1,253 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import java.util.function.BooleanSupplier; + +import com.team6962.lib.logging.CurrentDrawLogger; +import com.team6962.lib.swerve.CommandSwerveDrive; +import com.team6962.lib.swerve.commands.XBoxTeleopSwerveCommand; +import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.hood.ShooterHood; +import frc.robot.subsystems.hopper.beltfloor.BeltFloor; +import frc.robot.subsystems.intakeextension.IntakeExtension; +import frc.robot.subsystems.intakerollers.IntakeRollers; +import frc.robot.subsystems.turret.Turret; + +/** + * Dynamically slows subsystems when electrical headroom gets tight, prioritizing shooter/feed while + * shooting, intake while intaking, and drivetrain otherwise. + * + * Drive limits are applied as hard velocity caps directly in {@link XBoxTeleopSwerveCommand} so + * autonomous paths are unaffected + */ +public class BrownoutProtection extends SubsystemBase { + private static final int ELECTRICAL_SAMPLE_WINDOW = 8; + + private final CommandSwerveDrive swerveDrive; + private final XBoxTeleopSwerveCommand teleopSwerveCommand; + private final BooleanSupplier shootingActive; + private final BooleanSupplier intakingActive; + private final Turret turret; + private final ShooterHood shooterHood; + private final IntakeExtension intakeExtension; + private final BeltFloor beltFloor; + private final IntakeRollers intakeRollers; + + private final double[] recentBatteryVoltageSamplesVolts = new double[ELECTRICAL_SAMPLE_WINDOW]; + private final double[] recentTotalCurrentSamplesAmps = new double[ELECTRICAL_SAMPLE_WINDOW]; + private int recentElectricalSampleCount = 0; + private int nextElectricalSampleIndex = 0; + + private double startLimitingVoltage = 10.2; + private double minimumVoltage = 8.8; + private double startLimitingCurrentAmps = 180.0; + private double maximumCurrentAmps = 280.0; + private double minimumDrivePriorityVelocityScalingFactor = 0.45; + private double minimumReducedDriveVelocityScalingFactor = 0.3; + private double minimumPriorityMechanismScalingFactor = 0.65; + private double minimumReducedMechanismScalingFactor = 0.2; + + private enum PriorityMode { + DRIVING, + INTAKING, + SHOOTING + } + + public BrownoutProtection( + CommandSwerveDrive swerveDrive, + XBoxTeleopSwerveCommand teleopSwerveCommand, + BooleanSupplier shootingActive, + BooleanSupplier intakingActive, + Turret turret, + ShooterHood shooterHood, + IntakeExtension intakeExtension, + BeltFloor beltFloor, + IntakeRollers intakeRollers) { + this.swerveDrive = swerveDrive; + this.teleopSwerveCommand = teleopSwerveCommand; + this.shootingActive = shootingActive; + this.intakingActive = intakingActive; + this.turret = turret; + this.shooterHood = shooterHood; + this.intakeExtension = intakeExtension; + this.beltFloor = beltFloor; + this.intakeRollers = intakeRollers; + + DogLog.tunable( + "BrownoutProtection/Start Limiting Voltage", + startLimitingVoltage, + value -> startLimitingVoltage = value); + DogLog.tunable( + "BrownoutProtection/Minimum Voltage", minimumVoltage, value -> minimumVoltage = value); + DogLog.tunable( + "BrownoutProtection/Start Limiting Current", + startLimitingCurrentAmps, + value -> startLimitingCurrentAmps = value); + DogLog.tunable( + "BrownoutProtection/Maximum Current", + maximumCurrentAmps, + value -> maximumCurrentAmps = value); + DogLog.tunable( + "BrownoutProtection/Minimum Drive Priority Velocity Scaling Factor", + minimumDrivePriorityVelocityScalingFactor, + value -> minimumDrivePriorityVelocityScalingFactor = value); + DogLog.tunable( + "BrownoutProtection/Minimum Reduced Drive Velocity Scaling Factor", + minimumReducedDriveVelocityScalingFactor, + value -> minimumReducedDriveVelocityScalingFactor = value); + DogLog.tunable( + "BrownoutProtection/Minimum Priority Mechanism Scaling Factor", + minimumPriorityMechanismScalingFactor, + value -> minimumPriorityMechanismScalingFactor = value); + DogLog.tunable( + "BrownoutProtection/Minimum Reduced Mechanism Scaling Factor", + minimumReducedMechanismScalingFactor, + value -> minimumReducedMechanismScalingFactor = value); + } + + @Override + public void periodic() { + double batteryVoltageVolts = RobotController.getBatteryVoltage(); + double totalCurrentAmps = CurrentDrawLogger.getTotalCurrent().in(Amps); + + recordElectricalSample(batteryVoltageVolts, totalCurrentAmps); + + double recentMinimumBatteryVoltageVolts = getRecentMinimumBatteryVoltageVolts(); + double recentMaximumTotalCurrentAmps = getRecentMaximumTotalCurrentAmps(); + + double voltageHeadroomFraction = + mapToUnitRange(recentMinimumBatteryVoltageVolts, minimumVoltage, startLimitingVoltage); + double currentHeadroomFraction = + 1.0 + - mapToUnitRange( + recentMaximumTotalCurrentAmps, startLimitingCurrentAmps, maximumCurrentAmps); + // 1.0 means there is enough electrical headroom for full performance. 0.0 means outputs + // should be clamped to their configured minimum scaling factors. + double allowedPerformanceFraction = + RobotState.isDisabled() ? 1.0 : Math.min(voltageHeadroomFraction, currentHeadroomFraction); + + PriorityMode priorityMode = determinePriorityMode(); + + double prioritizedDriveVelocityScalingFactor = + MathUtil.interpolate( + minimumDrivePriorityVelocityScalingFactor, 1.0, allowedPerformanceFraction); + double reducedDriveVelocityScalingFactor = + MathUtil.interpolate( + minimumReducedDriveVelocityScalingFactor, 1.0, allowedPerformanceFraction); + double prioritizedMechanismScalingFactor = + MathUtil.interpolate( + minimumPriorityMechanismScalingFactor, 1.0, allowedPerformanceFraction); + double reducedMechanismScalingFactor = + MathUtil.interpolate(minimumReducedMechanismScalingFactor, 1.0, allowedPerformanceFraction); + + double driveVelocityScalingFactor = + priorityMode == PriorityMode.DRIVING + ? prioritizedDriveVelocityScalingFactor + : reducedDriveVelocityScalingFactor; + double shooterAimingScalingFactor = + priorityMode == PriorityMode.SHOOTING + ? prioritizedMechanismScalingFactor + : reducedMechanismScalingFactor; + double intakeMotionScalingFactor = + priorityMode == PriorityMode.INTAKING + ? prioritizedMechanismScalingFactor + : reducedMechanismScalingFactor; + double beltFloorVoltageScalingFactor = + priorityMode == PriorityMode.DRIVING + ? reducedMechanismScalingFactor + : prioritizedMechanismScalingFactor; + double intakeRollerVoltageScalingFactor = + priorityMode == PriorityMode.INTAKING + ? prioritizedMechanismScalingFactor + : reducedMechanismScalingFactor; + + // Apply drive limits as hard velocity caps in teleop command only + // Auton paths go through PathPlanner/VelocityMotion, are never affected + double maxLinearVelocityMetersPerSecond = + swerveDrive.getConstants().Driving.MaxLinearVelocity.in(MetersPerSecond); + double maxAngularVelocityRadiansPerSecond = + swerveDrive.getConstants().Driving.MaxAngularVelocity.in(RadiansPerSecond); + teleopSwerveCommand.addDynamicVelocityLimits( + MetersPerSecond.of(driveVelocityScalingFactor * maxLinearVelocityMetersPerSecond), + RadiansPerSecond.of(driveVelocityScalingFactor * maxAngularVelocityRadiansPerSecond)); + + turret.setMotionProfileConstraintScale(shooterAimingScalingFactor); + shooterHood.setMotionProfileConstraintScale(shooterAimingScalingFactor); + intakeExtension.setMotionProfileConstraintScale(intakeMotionScalingFactor); + beltFloor.setVoltageScale(beltFloorVoltageScalingFactor); + intakeRollers.setVoltageScale(intakeRollerVoltageScalingFactor); + + DogLog.log("BrownoutProtection/PriorityMode", priorityMode.name()); + DogLog.log("BrownoutProtection/BatteryVoltageVolts", batteryVoltageVolts); + DogLog.log( + "BrownoutProtection/RecentMinimumBatteryVoltageVolts", recentMinimumBatteryVoltageVolts); + DogLog.log("BrownoutProtection/TotalCurrentAmps", totalCurrentAmps); + DogLog.log("BrownoutProtection/RecentMaximumTotalCurrentAmps", recentMaximumTotalCurrentAmps); + DogLog.log("BrownoutProtection/VoltageHeadroomFraction", voltageHeadroomFraction); + DogLog.log("BrownoutProtection/CurrentHeadroomFraction", currentHeadroomFraction); + DogLog.log("BrownoutProtection/AllowedPerformanceFraction", allowedPerformanceFraction); + DogLog.log("BrownoutProtection/DriveVelocityScalingFactor", driveVelocityScalingFactor); + DogLog.log("BrownoutProtection/ShooterAimingScalingFactor", shooterAimingScalingFactor); + DogLog.log("BrownoutProtection/IntakeMotionScalingFactor", intakeMotionScalingFactor); + DogLog.log("BrownoutProtection/BeltFloorVoltageScalingFactor", beltFloorVoltageScalingFactor); + DogLog.log( + "BrownoutProtection/IntakeRollerVoltageScalingFactor", intakeRollerVoltageScalingFactor); + } + + private PriorityMode determinePriorityMode() { + if (shootingActive.getAsBoolean()) return PriorityMode.SHOOTING; + if (intakingActive.getAsBoolean()) return PriorityMode.INTAKING; + return PriorityMode.DRIVING; + } + + private void recordElectricalSample(double batteryVoltageVolts, double totalCurrentAmps) { + recentBatteryVoltageSamplesVolts[nextElectricalSampleIndex] = batteryVoltageVolts; + recentTotalCurrentSamplesAmps[nextElectricalSampleIndex] = totalCurrentAmps; + nextElectricalSampleIndex = (nextElectricalSampleIndex + 1) % ELECTRICAL_SAMPLE_WINDOW; + recentElectricalSampleCount = + Math.min(recentElectricalSampleCount + 1, ELECTRICAL_SAMPLE_WINDOW); + } + + private double getRecentMinimumBatteryVoltageVolts() { + double recentMinimumBatteryVoltageVolts = Double.POSITIVE_INFINITY; + + for (int i = 0; i < recentElectricalSampleCount; i++) { + recentMinimumBatteryVoltageVolts = + Math.min(recentMinimumBatteryVoltageVolts, recentBatteryVoltageSamplesVolts[i]); + } + + return recentElectricalSampleCount == 0 + ? startLimitingVoltage + : recentMinimumBatteryVoltageVolts; + } + + private double getRecentMaximumTotalCurrentAmps() { + double recentMaximumTotalCurrentAmps = Double.NEGATIVE_INFINITY; + + for (int i = 0; i < recentElectricalSampleCount; i++) { + recentMaximumTotalCurrentAmps = + Math.max(recentMaximumTotalCurrentAmps, recentTotalCurrentSamplesAmps[i]); + } + + return recentElectricalSampleCount == 0 ? 0.0 : recentMaximumTotalCurrentAmps; + } + + /** + * Maps a value into the inclusive unit interval, where {@code minimumValue} becomes {@code 0.0} + * and {@code maximumValue} becomes {@code 1.0}. + */ + private static double mapToUnitRange(double value, double minimumValue, double maximumValue) { + if (maximumValue <= minimumValue) { + return value >= maximumValue ? 1.0 : 0.0; + } + + return MathUtil.clamp((value - minimumValue) / (maximumValue - minimumValue), 0.0, 1.0); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java index 01ef1604..af87aea9 100644 --- a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java +++ b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.CoastOut; +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionVoltage; @@ -19,6 +20,7 @@ import com.team6962.lib.logging.CurrentDrawLogger; import com.team6962.lib.math.MeasureUtil; import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; @@ -56,6 +58,11 @@ public class ShooterHood extends SubsystemBase { private ShooterHoodSim simulation; private boolean isZeroed = false; private double kG = ShooterHoodConstants.kG; + private double baseMotionMagicCruiseVelocity = + ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity; + private double baseMotionMagicAcceleration = + ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicAcceleration; + private double motionProfileConstraintScale = 1.0; private Supplier shouldLowerHoodSupplier; @@ -80,6 +87,11 @@ public ShooterHood(Supplier shouldLowerHoodSupplier) { 1000.0; // 30.0; } + baseMotionMagicCruiseVelocity = + ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity; + baseMotionMagicAcceleration = + ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicAcceleration; + hoodMotor.getConfigurator().apply(ShooterHoodConstants.MOTOR_CONFIGURATION); candi.getConfigurator().apply(ShooterHoodConstants.CANDI_CONFIGURATION); @@ -153,24 +165,16 @@ public ShooterHood(Supplier shouldLowerHoodSupplier) { DogLog.tunable( "Hood/Hood Cruise Velocity", - ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity, + baseMotionMagicCruiseVelocity, newCruiseVelocity -> { - hoodMotor - .getConfigurator() - .apply( - ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic - .withMotionMagicCruiseVelocity(newCruiseVelocity)); + baseMotionMagicCruiseVelocity = newCruiseVelocity; }); DogLog.tunable( "Hood/Hood Acceleration", - ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicAcceleration, + baseMotionMagicAcceleration, newAcceleration -> { - hoodMotor - .getConfigurator() - .apply( - ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.withMotionMagicAcceleration( - newAcceleration)); + baseMotionMagicAcceleration = newAcceleration; }); DogLog.tunable( @@ -227,12 +231,16 @@ public void periodic() { "Hood/ProfileReferenceAngle", Rotations.of(profileReferenceSignal.getValue()).in(Degrees), Degrees); + DogLog.log("Hood/MotionMagicScale", motionProfileConstraintScale); // LoggingUtil.log("Hood/ControlRequest", hoodMotor.getAppliedControl()); - // Update the currently applied control request with a new gravity feedforward - // voltage if the control request is a MotionMagicVoltage - if (hoodMotor.getAppliedControl() instanceof MotionMagicVoltage motionMagicControlRequest) { + // Update currently applied control request with the latest gravity feedforward and + // brownout-scaled constraints if the hood is using Motion Magic position control. + if (hoodMotor.getAppliedControl() instanceof DynamicMotionMagicVoltage dynamicControlRequest) { + setPositionControl(Rotations.of(dynamicControlRequest.Position)); + } else if (hoodMotor.getAppliedControl() + instanceof MotionMagicVoltage motionMagicControlRequest) { setPositionControl(motionMagicControlRequest.getPositionMeasure()); } @@ -391,22 +399,23 @@ public Command moveAtVoltage(Voltage voltage) { } /** - * Applies a control request to move to a specific position with a MotionMagicVoltage control + * Applies a control request to move to a specific position with a Dynamic Motion Magic voltage * request, including an additional gravity feedforward. The gravity feedforward will be - * automatically updated in periodic(). + * automatically updated in periodic. * * @param position The target position to move to. */ private void setPositionControl(Angle position) { if (!isZeroed) { hoodMotor.setControl(new NeutralOut()); - } else if (shouldLowerHoodSupplier.get()) { - hoodMotor.setControl( - new MotionMagicVoltage(ShooterHoodConstants.MIN_ANGLE) - .withFeedForward(Math.cos(getPosition().in(Radians)) * kG)); } else { + Angle targetPosition = + shouldLowerHoodSupplier.get() ? ShooterHoodConstants.MIN_ANGLE : position; hoodMotor.setControl( - new MotionMagicVoltage(position) + new DynamicMotionMagicVoltage( + targetPosition.in(Rotations), + getScaledMotionMagicCruiseVelocity(), + getScaledMotionMagicAcceleration()) .withFeedForward(Math.cos(getPosition().in(Radians)) * kG)); } } @@ -415,9 +424,7 @@ private void setPositionVelocityControl(Angle position, AngularVelocity velocity if (!isZeroed) { hoodMotor.setControl(new NeutralOut()); } else if (shouldLowerHoodSupplier.get()) { - hoodMotor.setControl( - new MotionMagicVoltage(ShooterHoodConstants.MIN_ANGLE) - .withFeedForward(Math.cos(getPosition().in(Radians)) * kG)); + setPositionControl(ShooterHoodConstants.MIN_ANGLE); } else { hoodMotor.setControl( new PositionVoltage(position) @@ -445,15 +452,6 @@ public Command track( Supplier targetAngleSupplier, Supplier targetVelocitySupplier) { Command command = new Command() { - private TrapezoidProfile profile = - new TrapezoidProfile( - new TrapezoidProfile.Constraints( - ShooterHoodConstants.MOTOR_CONFIGURATION - .MotionMagic - .MotionMagicCruiseVelocity, - ShooterHoodConstants.MOTOR_CONFIGURATION - .MotionMagic - .MotionMagicAcceleration)); private TrapezoidProfile.State previousProfileState; private double previousUpdateTimestamp; @@ -475,6 +473,11 @@ public void execute() { Angle targetPosition = clampPositionToSafeRange(targetAngleSupplier.get()); + TrapezoidProfile profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + getScaledMotionMagicCruiseVelocity(), getScaledMotionMagicAcceleration())); + TrapezoidProfile.State profileState = profile.calculate( Timer.getFPGATimestamp() - previousUpdateTimestamp, @@ -504,6 +507,18 @@ public void execute() { return command; } + public void setMotionProfileConstraintScale(double scale) { + motionProfileConstraintScale = MathUtil.clamp(scale, 0.1, 1.0); + } + + private double getScaledMotionMagicCruiseVelocity() { + return baseMotionMagicCruiseVelocity * motionProfileConstraintScale; + } + + private double getScaledMotionMagicAcceleration() { + return baseMotionMagicAcceleration * motionProfileConstraintScale; + } + /** * Zeroes the hood without the hall effect sensor by assuming the hood is at the minimum angle. * diff --git a/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloor.java b/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloor.java index e6d63b74..7b8e4c74 100644 --- a/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloor.java +++ b/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloor.java @@ -13,6 +13,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.team6962.lib.logging.CurrentDrawLogger; import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -36,6 +37,7 @@ public class BeltFloor extends SubsystemBase { private StatusSignal statorCurrentSignal; private StatusSignal voltageSignal; private BeltFloorSim simulation; + private double voltageScale = 1.0; /** * Initializes the motor controller, configures status signals for logging, and sets up DogLog @@ -78,11 +80,11 @@ public BeltFloor() { * motor go clockwise. */ private Command feedDump(Voltage targetVoltage) { - - return startEnd( + return runEnd( () -> { // defines a local function to set motor voltage to make it go - beltFloorMotor.setControl(new VoltageOut(targetVoltage).withEnableFOC(false)); + beltFloorMotor.setControl( + new VoltageOut(targetVoltage.times(voltageScale)).withEnableFOC(false)); }, () -> { // defines a local function to stop motor @@ -148,6 +150,11 @@ public void periodic() { DogLog.log("Hopper/BeltFloor/StatorCurrent", getStatorCurrent()); DogLog.log("Hopper/BeltFloor/AngularAcceleration", getAngularAcceleration()); DogLog.log("Hopper/BeltFloor/SupplyCurrent", getSupplyCurrent()); + DogLog.log("Hopper/BeltFloor/VoltageScale", voltageScale); + } + + public void setVoltageScale(double scale) { + voltageScale = MathUtil.clamp(scale, 0.0, 1.0); } /** gets the angular velocity */ diff --git a/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloorSim.java b/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloorSim.java index 5bf61188..ac0e2c5c 100644 --- a/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloorSim.java +++ b/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloorSim.java @@ -33,10 +33,10 @@ public BeltFloorSim(TalonFX motor) { public void update() { motorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); double motorVoltage = invert(motorSim.getMotorVoltage(), false); - Angle position = physicsSim.getAngularPosition(); - AngularVelocity velocity = RadiansPerSecond.of(physicsSim.getAngularVelocityRadPerSec()); physicsSim.setInput(motorVoltage); physicsSim.update(0.02); + Angle position = physicsSim.getAngularPosition(); + AngularVelocity velocity = RadiansPerSecond.of(physicsSim.getAngularVelocityRadPerSec()); motorSim.setRawRotorPosition( invert(position, false) .times(HopperConstants.BELT_FLOOR_MOTOR_CONFIG.Feedback.SensorToMechanismRatio)); diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java index 85710e03..9fd0b07e 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java @@ -12,8 +12,8 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANdi; @@ -21,6 +21,7 @@ import com.team6962.lib.logging.CurrentDrawLogger; import com.team6962.lib.phoenix.StatusUtil; import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; @@ -56,6 +57,12 @@ public class IntakeExtension extends SubsystemBase { private IntakeExtensionSim simulation; + private double baseMotionMagicCruiseVelocity = + IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity; + private double baseMotionMagicAcceleration = + IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicAcceleration; + private double motionProfileConstraintScale = 1.0; + public boolean isZeroed = false; public IntakeExtension() { @@ -163,22 +170,16 @@ public IntakeExtension() { DogLog.tunable( "IntakeExtension/Velocity", - IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity, + baseMotionMagicCruiseVelocity, newVelocity -> { - MotionMagicConfigs config = new MotionMagicConfigs(); - StatusUtil.check(motor.getConfigurator().refresh(config)); - config.MotionMagicCruiseVelocity = newVelocity; - motor.getConfigurator().apply(config); + baseMotionMagicCruiseVelocity = newVelocity; }); DogLog.tunable( "IntakeExtension/Acceleration", - IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicAcceleration, + baseMotionMagicAcceleration, newAcceleration -> { - MotionMagicConfigs config = new MotionMagicConfigs(); - StatusUtil.check(motor.getConfigurator().refresh(config)); - config.MotionMagicAcceleration = newAcceleration; - motor.getConfigurator().apply(config); + baseMotionMagicAcceleration = newAcceleration; }); if (RobotBase.isSimulation()) { @@ -191,6 +192,26 @@ public IntakeExtension() { CurrentDrawLogger.add("Intake Extension", this::getSupplyCurrent); } + public void setMotionProfileConstraintScale(double scale) { + motionProfileConstraintScale = MathUtil.clamp(scale, 0.1, 1.0); + } + + private double getScaledMotionMagicCruiseVelocity() { + return baseMotionMagicCruiseVelocity * motionProfileConstraintScale; + } + + private double getScaledMotionMagicAcceleration() { + return baseMotionMagicAcceleration * motionProfileConstraintScale; + } + + private void setPositionControl(Distance position) { + motor.setControl( + new DynamicMotionMagicVoltage( + position.in(Meters), + getScaledMotionMagicCruiseVelocity(), + getScaledMotionMagicAcceleration())); + } + /** * Extends the intake outwards until it reaches the maximum position, then holds it there. Only * runs if the intake is zeroed, which happens when the CANdi is triggered. @@ -200,15 +221,14 @@ public IntakeExtension() { public Command extend() { return startEnd( () -> { - motor.setControl( - new MotionMagicVoltage(IntakeExtensionConstants.MAX_POSITION.in(Meters))); + setPositionControl(IntakeExtensionConstants.MAX_POSITION); }, () -> { if (!getPosition() .isNear( IntakeExtensionConstants.MAX_POSITION, IntakeExtensionConstants.POSITION_TOLERANCE)) { - motor.setControl(new MotionMagicVoltage(getPosition().in(Meters))); + setPositionControl(getPosition()); } }) .until( @@ -228,15 +248,14 @@ public Command extend() { public Command retract() { return startEnd( () -> { - motor.setControl( - new MotionMagicVoltage(IntakeExtensionConstants.RETRACT_POSITION.in(Meters))); + setPositionControl(IntakeExtensionConstants.RETRACT_POSITION); }, () -> { if (!getPosition() .isNear( IntakeExtensionConstants.RETRACT_POSITION, IntakeExtensionConstants.POSITION_TOLERANCE)) { - motor.setControl(new MotionMagicVoltage(getPosition().in(Meters))); + setPositionControl(getPosition()); } }) .until( @@ -268,7 +287,7 @@ public Command moveAtVoltage(Voltage voltage) { .plus(Volts.of(IntakeExtensionConstants.MOTOR_CONFIGURATION.Slot0.kG)))); }, () -> { - motor.setControl(new MotionMagicVoltage(getPosition().in(Meters))); + setPositionControl(getPosition()); }) .onlyIf(() -> isZeroed || voltage.in(Volts) < 0); } @@ -401,5 +420,12 @@ && getPosition().lt(IntakeExtensionConstants.MIN_POSITION)) { DogLog.log("Intake/HallSensorTriggered", isHallSensorTriggered()); DogLog.log("Intake/ClosedLoopReference", getClosedLoopReference()); DogLog.forceNt.log("Intake/IsZeroed", isZeroed); + DogLog.log("Intake/MotionMagicScale", motionProfileConstraintScale); + + if (motor.getAppliedControl() instanceof DynamicMotionMagicVoltage dynamicControlRequest) { + setPositionControl(Meters.of(dynamicControlRequest.Position)); + } else if (motor.getAppliedControl() instanceof MotionMagicVoltage motionMagicControlRequest) { + setPositionControl(Meters.of(motionMagicControlRequest.Position)); + } } } diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollerSim.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollerSim.java index 517e75ff..1e981e3f 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollerSim.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollerSim.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.DCMotorSim; public class IntakeRollerSim { @@ -28,7 +29,7 @@ public IntakeRollerSim(TalonFX motor) { /** updates physicsSim */ public void update() { - intakeMotor.setSupplyVoltage(12); + intakeMotor.setSupplyVoltage(RobotController.getBatteryVoltage()); physicsSim.setInputVoltage(intakeMotor.getMotorVoltage()); physicsSim.update(0.02); diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java index ff6b83af..75cc914f 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java @@ -7,18 +7,19 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.CoastOut; -import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.team6962.lib.logging.CurrentDrawLogger; import com.team6962.lib.phoenix.StatusUtil; import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -33,6 +34,7 @@ public class IntakeRollers extends SubsystemBase { private double intakeStallVoltage = 12.0; private Debouncer stallDebouncer = new Debouncer(0.25, DebounceType.kRising); private boolean stalling = false; + private double voltageScale = 1.0; /** Intializes motor and status signals Class for Intake Rollers */ public IntakeRollers() { @@ -68,9 +70,9 @@ public IntakeRollers() { /** Returns command to make the motor move and stop */ @SuppressWarnings("unused") private Command move(Voltage voltage) { - return startEnd( + return runEnd( () -> { - intakeMotor.setControl(new VoltageOut(voltage).withEnableFOC(false)); + intakeMotor.setControl(new VoltageOut(voltage.times(voltageScale)).withEnableFOC(false)); }, () -> { intakeMotor.setControl(new CoastOut()); @@ -86,7 +88,8 @@ public Command intake() { return runEnd( () -> { intakeMotor.setControl( - new VoltageOut(stalling ? intakeStallVoltage : intakeVoltage).withEnableFOC(false)); + new VoltageOut((stalling ? intakeStallVoltage : intakeVoltage) * voltageScale) + .withEnableFOC(false)); }, () -> { intakeMotor.setControl(new CoastOut()); @@ -100,9 +103,11 @@ public Command intake() { * @return The command to intake fuel at full speed. */ public Command intakeFast() { - return startEnd( + return runEnd( () -> { - intakeMotor.setControl(new DutyCycleOut(1).withEnableFOC(false)); + intakeMotor.setControl( + new VoltageOut(RobotController.getBatteryVoltage() * voltageScale) + .withEnableFOC(false)); }, () -> { intakeMotor.setControl(new CoastOut()); @@ -115,9 +120,11 @@ public Command intakeFast() { * @return */ public Command outtake() { - return startEnd( + return runEnd( () -> { - intakeMotor.setControl(new DutyCycleOut(-1).withEnableFOC(false)); + intakeMotor.setControl( + new VoltageOut(-RobotController.getBatteryVoltage() * voltageScale) + .withEnableFOC(false)); }, () -> { intakeMotor.setControl(new CoastOut()); @@ -173,9 +180,14 @@ public void periodic() { DogLog.log("intakeRollers/supplyCurrent", getSupplyCurrent()); DogLog.log("intakeRollers/appliedVoltage", getAppliedVoltage()); DogLog.log("intakeRollers/stalling", stalling); + DogLog.log("intakeRollers/voltageScale", voltageScale); stalling = stallDebouncer.calculate( getVelocity().abs(RotationsPerSecond) < 1.0 && getStatorCurrent().abs(Amps) > 100.0); } + + public void setVoltageScale(double scale) { + voltageScale = MathUtil.clamp(scale, 0.0, 1.0); + } } diff --git a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersSim.java b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersSim.java index 9b0478bf..358dcb79 100644 --- a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersSim.java +++ b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersSim.java @@ -33,10 +33,10 @@ public ShooterRollersSim(TalonFX motor) { public void update() { motorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); double motorVoltage = invert(motorSim.getMotorVoltage(), false); - Angle position = physicsSim.getAngularPosition(); - AngularVelocity velocity = RadiansPerSecond.of(physicsSim.getAngularVelocityRadPerSec()); physicsSim.setInput(motorVoltage); physicsSim.update(0.02); + Angle position = physicsSim.getAngularPosition(); + AngularVelocity velocity = RadiansPerSecond.of(physicsSim.getAngularVelocityRadPerSec()); motorSim.setRawRotorPosition( invert(position, false) .times(ShooterRollersConstants.MOTOR_CONFIGURATION.Feedback.SensorToMechanismRatio)); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 1ef1650b..aff93436 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.CoastOut; +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionVoltage; @@ -22,6 +23,7 @@ import com.team6962.lib.math.AngleMath; import com.team6962.lib.math.MeasureUtil; import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; @@ -79,6 +81,10 @@ public class Turret extends SubsystemBase { /** The current motor configuration */ private TalonFXConfiguration config; + private double baseMotionMagicCruiseVelocity; + private double baseMotionMagicAcceleration; + private double motionProfileConstraintScale = 1.0; + /** Assigns Status Signal variables to the different methods part of the motor.get() */ public Turret() { // Assigns PID values and sets motor config @@ -103,6 +109,8 @@ public Turret() { config.MotionMagic.MotionMagicCruiseVelocity = TurretConstants.MOTION_MAGIC_CRUISE_VELOCITY; config.MotionMagic.MotionMagicAcceleration = TurretConstants.MOTION_MAGIC_ACCELERATION; config.Feedback.SensorToMechanismRatio = TurretConstants.SENSOR_TO_MECHANISM_RATIO; + baseMotionMagicCruiseVelocity = config.MotionMagic.MotionMagicCruiseVelocity; + baseMotionMagicAcceleration = config.MotionMagic.MotionMagicAcceleration; config.MotorOutput.NeutralMode = TurretConstants.MOTOR_NEUTRAL_MODE; @@ -167,19 +175,17 @@ public Turret() { DogLog.tunable( "Turret/Turret Cruise Velocity", - config.MotionMagic.MotionMagicCruiseVelocity, - newVel -> - motor - .getConfigurator() - .apply(config.MotionMagic.withMotionMagicCruiseVelocity(newVel))); + baseMotionMagicCruiseVelocity, + newVel -> { + baseMotionMagicCruiseVelocity = newVel; + }); DogLog.tunable( "Turret/Turret Max Acceleration", - config.MotionMagic.MotionMagicAcceleration, - newAccel -> - motor - .getConfigurator() - .apply(config.MotionMagic.withMotionMagicAcceleration(newAccel))); + baseMotionMagicAcceleration, + newAccel -> { + baseMotionMagicAcceleration = newAccel; + }); DogLog.tunable("Turret/Turret kW", TurretConstants.kW, newKW -> TurretConstants.kW = newKW); @@ -247,10 +253,13 @@ public void periodic() { "Turret/ProfilePosition", Rotations.of(profilePositionSignal.getValue()).in(Radians), Radians); + DogLog.log("Turret/MotionMagicScale", motionProfileConstraintScale); // LoggingUtil.log("Turret/ControlRequest", motor.getAppliedControl()); - if (motor.getAppliedControl() instanceof MotionMagicVoltage control) { + if (motor.getAppliedControl() instanceof DynamicMotionMagicVoltage dynamicControlRequest) { + setPositionControl(Rotations.of(dynamicControlRequest.Position)); + } else if (motor.getAppliedControl() instanceof MotionMagicVoltage control) { setPositionControl(control.getPositionMeasure()); } } @@ -498,11 +507,6 @@ public Command track( Supplier targetAngleSupplier, Supplier targetVelocitySupplier) { Command command = new Command() { - private TrapezoidProfile profile = - new TrapezoidProfile( - new TrapezoidProfile.Constraints( - TurretConstants.MOTION_MAGIC_CRUISE_VELOCITY, - TurretConstants.MOTION_MAGIC_ACCELERATION)); private TrapezoidProfile.State previousProfileState; private double previousUpdateTimestamp; @@ -530,6 +534,11 @@ public void execute() { TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE)); + TrapezoidProfile profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + getScaledMotionMagicCruiseVelocity(), getScaledMotionMagicAcceleration())); + TrapezoidProfile.State profileState = profile.calculate( Timer.getFPGATimestamp() - previousUpdateTimestamp, @@ -587,15 +596,18 @@ public Command moveTo(Supplier targetAngleSupplier) { } /** - * Applies a Motion Magic position control request to move the turret to the target angle. If the - * turret is not yet zeroed, the motor is set to neutral mode instead. + * Applies a Dynamic Motion Magic position control request to move the turret to the target angle. + * If the turret is not yet zeroed, the motor is set to neutral mode instead. * * @param targetAngle The target angle to move the turret to */ private void setPositionControl(Angle targetAngle) { if (isZeroed()) { motor.setControl( - new MotionMagicVoltage(targetAngle) + new DynamicMotionMagicVoltage( + targetAngle.in(Rotations), + getScaledMotionMagicCruiseVelocity(), + getScaledMotionMagicAcceleration()) .withFeedForward( targetAngle.lt(TurretConstants.MIN_KW_ANGLE) ? -TurretConstants.kW @@ -634,6 +646,18 @@ public Command moveAtVoltage(Voltage voltage) { .onlyIf(() -> isZeroed()); } + public void setMotionProfileConstraintScale(double scale) { + motionProfileConstraintScale = MathUtil.clamp(scale, 0.1, 1.0); + } + + private double getScaledMotionMagicCruiseVelocity() { + return baseMotionMagicCruiseVelocity * motionProfileConstraintScale; + } + + private double getScaledMotionMagicAcceleration() { + return baseMotionMagicAcceleration * motionProfileConstraintScale; + } + public void zero() { motor.setPosition(Degrees.of(180)); }