diff --git a/.vscode/settings.json b/.vscode/settings.json index 41949403..cdcc56da 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -58,6 +58,6 @@ "edu.wpi.first.math.**.struct.*", ], "java.dependency.enableDependencyCheckup": false, - "java.jdt.ls.vmargs": "-Xmx4G", + "java.jdt.ls.vmargs": "-Xmx8G", "java.compile.nullAnalysis.mode": "disabled" } diff --git a/build.gradle b/build.gradle index 33346d5c..84f3d73c 100644 --- a/build.gradle +++ b/build.gradle @@ -76,6 +76,10 @@ wpi.java.debugJni = false // Set this to true to enable desktop support. def includeDesktopSupport = true +repositories { + mavenCentral() +} + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. dependencies { diff --git a/src/main/deploy/all_shooter_hub_data.csv b/src/main/deploy/all_shooter_hub_data.csv new file mode 100644 index 00000000..cae389c8 --- /dev/null +++ b/src/main/deploy/all_shooter_hub_data.csv @@ -0,0 +1,27 @@ +57.5,18,25 +57.5,23,23 +57.5,26,21 +85,18,26.5 +85,28,23.5 +85,23,24.5 +107,18,28 +107,28,24.3 +107,38,23.9 +107,33,23.5 +107,23,25.5 +124,18,28 +124,23,26 +124,28,24 +150,18,32 +150,28,27 +150,38,26.5 +214,18,36 +214,28,30 +214,38,29 +214,23,34 +214,33,29.75 +263,18,42 +263,28,35 +263,38,31.5 +263,23,38.5 +263,33,32.5 \ No newline at end of file diff --git a/src/main/deploy/flywheelvelocitydatahub.csv b/src/main/deploy/flywheelvelocitydatahub.csv deleted file mode 100644 index f959b0c2..00000000 --- a/src/main/deploy/flywheelvelocitydatahub.csv +++ /dev/null @@ -1,7 +0,0 @@ -57.5,23 -85,23.5 -107,24.3 -124,25 -150,27 -214,30 -263,31.5 \ No newline at end of file diff --git a/src/main/deploy/hoodangledatahub.csv b/src/main/deploy/hoodangledatahub.csv deleted file mode 100644 index a9074c96..00000000 --- a/src/main/deploy/hoodangledatahub.csv +++ /dev/null @@ -1,27 +0,0 @@ -57.5,25,18 -57.5,23,23 -57.5,21,26 -85,26.5,18 -85,23.5,28 -85,24.5,23 -107,28,18 -107,24.3,28 -107,23.9,38 -107,23.5,33 -107,25.5,23 -124,28,18 -124,26,23 -124,24,28 -150,32,18 -150,27,28 -150,26.5,38 -214,36,18 -214,30,28 -214,29,38 -214,34,23 -214,29.75,33 -263,42,18 -263,35,28 -263,31.5,38 -263,38.5,23 -263,32.5,33 \ No newline at end of file diff --git a/src/main/deploy/shooter_hub_data.csv b/src/main/deploy/shooter_hub_data.csv new file mode 100644 index 00000000..434acfa4 --- /dev/null +++ b/src/main/deploy/shooter_hub_data.csv @@ -0,0 +1,9 @@ +83.75,18,22,0.868 +101.375,22,24,1.004 +114.25,24,24.5,1.012 +128.25,25,25,1.016 +144.25,26,26,1.029 +165.75,26.5,28,1.159 +189.25,27,29,1.212 +221.25,29,31.5,1.289 +235.75,29.5,32,1.318 diff --git a/src/main/deploy/sim_shooter_hub_data.csv b/src/main/deploy/sim_shooter_hub_data.csv new file mode 100644 index 00000000..6537fddc --- /dev/null +++ b/src/main/deploy/sim_shooter_hub_data.csv @@ -0,0 +1,5 @@ +50,18,15,0.5 +100,28,20,0.75 +150,38,25,1 +200,43,27,1.25 +250,48,30,1.5 \ No newline at end of file diff --git a/src/main/deploy/velocityrangehub.csv b/src/main/deploy/velocityrangehub.csv deleted file mode 100644 index 5dc99045..00000000 --- a/src/main/deploy/velocityrangehub.csv +++ /dev/null @@ -1,3 +0,0 @@ -57.5,1,1 -124,1,1 -263,1,1 \ No newline at end of file diff --git a/src/main/java/com/team6962/lib/commands/CommandUtil.java b/src/main/java/com/team6962/lib/commands/CommandUtil.java new file mode 100644 index 00000000..187cd42d --- /dev/null +++ b/src/main/java/com/team6962/lib/commands/CommandUtil.java @@ -0,0 +1,22 @@ +package com.team6962.lib.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; + +/** Utility class for command-related helper functions. */ +public class CommandUtil { + /** + * Checks if it is okay to replace the currently active command on a subsystem, and have the given + * command take control instead. A subsystem is clear to override if it has no current command, + * has only its default command running, or is already running the specified command. + * + * @param subsystem The subsystem to check + * @param command The command that wants to take control + * @return True if the subsystem's active command can be safely overridden + */ + public static boolean isClearToOverride(Subsystem subsystem, Command command) { + return subsystem.getCurrentCommand() == null + || subsystem.getCurrentCommand() == subsystem.getDefaultCommand() + || subsystem.getCurrentCommand() == command; + } +} diff --git a/src/main/java/com/team6962/lib/logging/LoggingUtil.java b/src/main/java/com/team6962/lib/logging/LoggingUtil.java index b6eb3fe2..2b3c807c 100644 --- a/src/main/java/com/team6962/lib/logging/LoggingUtil.java +++ b/src/main/java/com/team6962/lib/logging/LoggingUtil.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.controls.ControlRequest; import dev.doglog.DogLog; +import edu.wpi.first.wpilibj2.command.Command; import java.io.InputStream; import java.util.Map; import java.util.Optional; @@ -90,4 +91,23 @@ public static void logGitProperties() { System.err.println("Failed to load git properties: " + e.getMessage()); } } + + /** + * Wraps the given command with logging that indicates when the command starts and ends. Logs are + * written under the path "Commands/{key}" with a boolean value indicating whether the command is + * currently running (true) or not (false). + * + * @param key A unique key to identify the command in the logs. This should be a descriptive name + * for the command being logged (e.g., "Shoot", "DriveToClump", etc.). + * @param command The Command to wrap with logging. + * @return A new Command that behaves the same as the input command but also logs its execution + * status under "Commands/{key}". + */ + public static Command logCommand(String key, Command command) { + DogLog.log("Commands/" + key, false); + + return command + .beforeStarting(() -> DogLog.log("Commands/" + key, true)) + .finallyDo(() -> DogLog.log("Commands/" + key, false)); + } } diff --git a/src/main/java/com/team6962/lib/math/ConstantFunction.java b/src/main/java/com/team6962/lib/math/ConstantFunction.java new file mode 100644 index 00000000..472144af --- /dev/null +++ b/src/main/java/com/team6962/lib/math/ConstantFunction.java @@ -0,0 +1,22 @@ +package com.team6962.lib.math; + +import org.apache.commons.math3.analysis.MultivariateFunction; +import org.apache.commons.math3.analysis.UnivariateFunction; + +public class ConstantFunction implements UnivariateFunction, MultivariateFunction { + private final double value; + + public ConstantFunction(double value) { + this.value = value; + } + + @Override + public double value(double x) { + return value; + } + + @Override + public double value(double[] point) { + return value; + } +} diff --git a/src/main/java/com/team6962/lib/math/TranslationalVelocity.java b/src/main/java/com/team6962/lib/math/TranslationalVelocity.java index def63852..2cbe134c 100644 --- a/src/main/java/com/team6962/lib/math/TranslationalVelocity.java +++ b/src/main/java/com/team6962/lib/math/TranslationalVelocity.java @@ -158,6 +158,19 @@ public TranslationalVelocity times(double scalar) { MetersPerSecond.of(this.y.in(MetersPerSecond) * scalar)); } + /** + * Multiplies this TranslationalVelocity by a Time measure and returns the resulting + * Translation2d. + * + * @param time The Time measure to multiply by + * @return The resulting Translation2d + */ + public Translation2d times(edu.wpi.first.units.measure.Time time) { + return new Translation2d( + this.x.in(MetersPerSecond) * time.in(edu.wpi.first.units.Units.Seconds), + this.y.in(MetersPerSecond) * time.in(edu.wpi.first.units.Units.Seconds)); + } + /** * Divides this TranslationalVelocity by a scalar and returns the result. * diff --git a/src/main/java/com/team6962/lib/swerve/commands/TeleopSwerveCommand.java b/src/main/java/com/team6962/lib/swerve/commands/TeleopSwerveCommand.java index 21a7ca2f..eeb7590c 100644 --- a/src/main/java/com/team6962/lib/swerve/commands/TeleopSwerveCommand.java +++ b/src/main/java/com/team6962/lib/swerve/commands/TeleopSwerveCommand.java @@ -2,11 +2,11 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; +import com.team6962.lib.commands.CommandUtil; import com.team6962.lib.math.TranslationalVelocity; import com.team6962.lib.swerve.CommandSwerveDrive; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.button.Trigger; /** @@ -43,12 +43,15 @@ public TeleopSwerveCommand(CommandSwerveDrive swerveDrive) { Trigger rotationTrigger = new Trigger( - () -> isScheduled() && isClearToOverride(swerveDrive.useRotation(), rotationCommand)); + () -> + isScheduled() + && CommandUtil.isClearToOverride(swerveDrive.useRotation(), rotationCommand)); Trigger translationTrigger = new Trigger( () -> isScheduled() - && isClearToOverride(swerveDrive.useTranslation(), translationCommand)); + && CommandUtil.isClearToOverride( + swerveDrive.useTranslation(), translationCommand)); rotationTrigger.whileTrue(rotationCommand); translationTrigger.whileTrue(translationCommand); @@ -70,19 +73,4 @@ protected CommandSwerveDrive getSwerveDrive() { * @return The desired field-relative chassis speeds */ protected abstract ChassisSpeeds getDrivenVelocity(); - - /** - * Checks if it is okay to replace the currently active command on a subsystem, and have the given - * command take control instead. A subsystem is clear to override if it has no current command, - * has only its default command running, or is already running the specified command. - * - * @param subsystem The subsystem to check - * @param command The command that wants to take control - * @return True if the subsystem's active command can be safely overridden - */ - public static boolean isClearToOverride(Subsystem subsystem, Command command) { - return subsystem.getCurrentCommand() == null - || subsystem.getCurrentCommand() == subsystem.getDefaultCommand() - || subsystem.getCurrentCommand() == command; - } } diff --git a/src/main/java/com/team6962/lib/swerve/config/TimingConstants.java b/src/main/java/com/team6962/lib/swerve/config/TimingConstants.java index e846d0a0..d6f580f4 100644 --- a/src/main/java/com/team6962/lib/swerve/config/TimingConstants.java +++ b/src/main/java/com/team6962/lib/swerve/config/TimingConstants.java @@ -31,6 +31,9 @@ public class TimingConstants implements Cloneable { */ public boolean TimesyncControlRequests = false; + /** Whether to minimize logging to improve performance. */ + public boolean MinimizeLogging = false; + /** Constructs a TimingConstants object with default values. */ public TimingConstants() { SignalUpdateRate = Hertz.of(100); @@ -83,6 +86,17 @@ public TimingConstants withTimesyncControlRequests(boolean timesyncControlReques return this; } + /** + * Sets whether to minimize logging, and returns this TimingConstants for chaining. + * + * @param minimizeLogging True if logging should be minimized + * @return This TimingConstants object + */ + public TimingConstants withMinimizeLogging(boolean minimizeLogging) { + MinimizeLogging = minimizeLogging; + return this; + } + @Override public TimingConstants clone() { try { diff --git a/src/main/java/com/team6962/lib/swerve/localization/Gyroscope.java b/src/main/java/com/team6962/lib/swerve/localization/Gyroscope.java index ff022087..24dd8637 100644 --- a/src/main/java/com/team6962/lib/swerve/localization/Gyroscope.java +++ b/src/main/java/com/team6962/lib/swerve/localization/Gyroscope.java @@ -129,17 +129,20 @@ private boolean shouldUseGyroscope() { public void logTelemetry(String basePath) { basePath = LoggingUtil.ensureEndsWithSlash(basePath); - DogLog.log(basePath + "IsConnected", gyro.isConnected()); - DogLog.log(basePath + "UsingGyroscope", shouldUseGyroscope()); - - DogLog.log(basePath + "Yaw", getYaw().in(Radians), Radians); - DogLog.log(basePath + "Pitch", getPitch().in(Radians), Radians); - DogLog.log(basePath + "Roll", getRoll().in(Radians), Radians); - - DogLog.log(basePath + "YawVelocity", getYawVelocity().in(RadiansPerSecond), RadiansPerSecond); - DogLog.log( - basePath + "PitchVelocity", getPitchVelocity().in(RadiansPerSecond), RadiansPerSecond); - DogLog.log(basePath + "RollVelocity", getRollVelocity().in(RadiansPerSecond), RadiansPerSecond); + if (!constants.Timing.MinimizeLogging) { + DogLog.log(basePath + "IsConnected", gyro.isConnected()); + DogLog.log(basePath + "UsingGyroscope", shouldUseGyroscope()); + + DogLog.log(basePath + "Yaw", getYaw().in(Radians), Radians); + DogLog.log(basePath + "Pitch", getPitch().in(Radians), Radians); + DogLog.log(basePath + "Roll", getRoll().in(Radians), Radians); + + DogLog.log(basePath + "YawVelocity", getYawVelocity().in(RadiansPerSecond), RadiansPerSecond); + DogLog.log( + basePath + "PitchVelocity", getPitchVelocity().in(RadiansPerSecond), RadiansPerSecond); + DogLog.log( + basePath + "RollVelocity", getRollVelocity().in(RadiansPerSecond), RadiansPerSecond); + } } /** diff --git a/src/main/java/com/team6962/lib/swerve/localization/Localization.java b/src/main/java/com/team6962/lib/swerve/localization/Localization.java index ed6424f1..47d9bdd3 100644 --- a/src/main/java/com/team6962/lib/swerve/localization/Localization.java +++ b/src/main/java/com/team6962/lib/swerve/localization/Localization.java @@ -34,6 +34,9 @@ * field. */ public class Localization implements SwerveComponent { + /** Drivetrain constants containing configuration for the swerve drive. */ + private DrivetrainConstants constants; + /** * The pose estimator that fuses gyroscope, odometry, and vision data to estimate the robot's * position on the field. @@ -72,6 +75,7 @@ public class Localization implements SwerveComponent { public Localization( DrivetrainConstants constants, Pose3d initialPose, Odometry odometry, Gyroscope gyroscope) { + this.constants = constants; this.gyroscope = gyroscope; this.odometry = odometry; @@ -139,14 +143,6 @@ public synchronized void logTelemetry(String basePath) { DogLog.log(basePath + "Localization/Position", position); - DogLog.log(basePath + "Localization/PositionX", position.getX(), Meters); - DogLog.log(basePath + "Localization/PositionY", position.getY(), Meters); - DogLog.log(basePath + "Localization/PositionZ", position.getZ(), Meters); - - DogLog.log(basePath + "Localization/RotationYaw", position.getRotation().getZ(), Radians); - DogLog.log(basePath + "Localization/RotationPitch", position.getRotation().getY(), Radians); - DogLog.log(basePath + "Localization/RotationRoll", position.getRotation().getX(), Radians); - DogLog.log(basePath + "Localization/Heading", getHeading().in(Radians), Radians); DogLog.log(basePath + "Localization/VelocityX", velocity.vxMetersPerSecond, MetersPerSecond); @@ -156,26 +152,36 @@ public synchronized void logTelemetry(String basePath) { velocity.omegaRadiansPerSecond, RadiansPerSecond); - DogLog.log( - basePath + "Localization/AngularVelocityYaw", - gyroscope.getYawVelocity().in(RadiansPerSecond), - RadiansPerSecond); - DogLog.log( - basePath + "Localization/AngularVelocityPitch", - gyroscope.getPitchVelocity().in(RadiansPerSecond), - RadiansPerSecond); - DogLog.log( - basePath + "Localization/AngularVelocityRoll", - gyroscope.getRollVelocity().in(RadiansPerSecond), - RadiansPerSecond); - - DogLog.log(basePath + "Localization/TwistDX", twist.dx, Meters); - DogLog.log(basePath + "Localization/TwistDY", twist.dy, Meters); - DogLog.log(basePath + "Localization/TwistDTheta", twist.dtheta, Radians); - - DogLog.log(basePath + "Localization/ArcVelocityDX", arcVelocity.dx, MetersPerSecond); - DogLog.log(basePath + "Localization/ArcVelocityDY", arcVelocity.dy, MetersPerSecond); - DogLog.log(basePath + "Localization/ArcVelocityDTheta", arcVelocity.dtheta, RadiansPerSecond); + if (!constants.Timing.MinimizeLogging) { + DogLog.log(basePath + "Localization/PositionX", position.getX(), Meters); + DogLog.log(basePath + "Localization/PositionY", position.getY(), Meters); + DogLog.log(basePath + "Localization/PositionZ", position.getZ(), Meters); + + DogLog.log(basePath + "Localization/RotationYaw", position.getRotation().getZ(), Radians); + DogLog.log(basePath + "Localization/RotationPitch", position.getRotation().getY(), Radians); + DogLog.log(basePath + "Localization/RotationRoll", position.getRotation().getX(), Radians); + + DogLog.log( + basePath + "Localization/AngularVelocityYaw", + gyroscope.getYawVelocity().in(RadiansPerSecond), + RadiansPerSecond); + DogLog.log( + basePath + "Localization/AngularVelocityPitch", + gyroscope.getPitchVelocity().in(RadiansPerSecond), + RadiansPerSecond); + DogLog.log( + basePath + "Localization/AngularVelocityRoll", + gyroscope.getRollVelocity().in(RadiansPerSecond), + RadiansPerSecond); + + DogLog.log(basePath + "Localization/TwistDX", twist.dx, Meters); + DogLog.log(basePath + "Localization/TwistDY", twist.dy, Meters); + DogLog.log(basePath + "Localization/TwistDTheta", twist.dtheta, Radians); + + DogLog.log(basePath + "Localization/ArcVelocityDX", arcVelocity.dx, MetersPerSecond); + DogLog.log(basePath + "Localization/ArcVelocityDY", arcVelocity.dy, MetersPerSecond); + DogLog.log(basePath + "Localization/ArcVelocityDTheta", arcVelocity.dtheta, RadiansPerSecond); + } } /** diff --git a/src/main/java/com/team6962/lib/swerve/module/DriveMechanism.java b/src/main/java/com/team6962/lib/swerve/module/DriveMechanism.java index 43e93c54..f5c2a4a4 100644 --- a/src/main/java/com/team6962/lib/swerve/module/DriveMechanism.java +++ b/src/main/java/com/team6962/lib/swerve/module/DriveMechanism.java @@ -165,19 +165,8 @@ public void logTelemetry(String basePath) { basePath += "/"; } - DogLog.log(basePath + "Position", getPosition().in(Meters)); DogLog.log(basePath + "Velocity", getVelocity().in(MetersPerSecond)); - DogLog.log(basePath + "Acceleration", getAcceleration().in(MetersPerSecondPerSecond)); - DogLog.log(basePath + "AngularPosition", getAngularPosition().in(Radians)); - DogLog.log(basePath + "AngularVelocity", getAngularVelocity().in(RadiansPerSecond)); - DogLog.log( - basePath + "AngularAcceleration", getAngularAcceleration().in(RadiansPerSecondPerSecond)); - DogLog.log(basePath + "AppliedVoltage", getAppliedVoltage().in(Volts)); - DogLog.log(basePath + "StatorCurrent", getStatorCurrent().in(Amps)); DogLog.log(basePath + "SupplyCurrent", getSupplyCurrent().in(Amps)); - DogLog.log( - basePath + "DataTimestamp", - StatusUtil.toFPGATimestamp(positionSignal.getTimestamp().getTime())); // If running a position or velocity request, log the target position or // velocity in radians. The target is logged as part of the control @@ -207,7 +196,21 @@ public void logTelemetry(String basePath) { RadiansPerSecond); } - LoggingUtil.log(basePath + "ControlRequest", lastControlRequest); + if (!constants.Timing.MinimizeLogging) { + DogLog.log(basePath + "Position", getPosition().in(Meters)); + DogLog.log(basePath + "Acceleration", getAcceleration().in(MetersPerSecondPerSecond)); + DogLog.log(basePath + "AngularPosition", getAngularPosition().in(Radians)); + DogLog.log(basePath + "AngularVelocity", getAngularVelocity().in(RadiansPerSecond)); + DogLog.log( + basePath + "AngularAcceleration", getAngularAcceleration().in(RadiansPerSecondPerSecond)); + DogLog.log(basePath + "AppliedVoltage", getAppliedVoltage().in(Volts)); + DogLog.log(basePath + "StatorCurrent", getStatorCurrent().in(Amps)); + DogLog.log( + basePath + "DataTimestamp", + StatusUtil.toFPGATimestamp(positionSignal.getTimestamp().getTime())); + + LoggingUtil.log(basePath + "ControlRequest", lastControlRequest); + } } /** diff --git a/src/main/java/com/team6962/lib/swerve/module/SteerMechanism.java b/src/main/java/com/team6962/lib/swerve/module/SteerMechanism.java index 59794e81..fe0a83d2 100644 --- a/src/main/java/com/team6962/lib/swerve/module/SteerMechanism.java +++ b/src/main/java/com/team6962/lib/swerve/module/SteerMechanism.java @@ -172,15 +172,19 @@ public void logTelemetry(String basePath) { } DogLog.log(basePath + "Position", getPosition().in(Radians)); - DogLog.log(basePath + "Velocity", getVelocity().in(RadiansPerSecond)); - DogLog.log(basePath + "Acceleration", getAcceleration().in(RadiansPerSecondPerSecond)); - DogLog.log(basePath + "AppliedVoltage", getAppliedVoltage().in(Volts)); - DogLog.log(basePath + "StatorCurrent", getStatorCurrent().in(Amps)); - DogLog.log(basePath + "SupplyCurrent", getSupplyCurrent().in(Amps)); - DogLog.log(basePath + "ProfilePosition", profilePosition.in(Radians)); - DogLog.log( - basePath + "DataTimestamp", - StatusUtil.toFPGATimestamp(positionSignal.getTimestamp().getTime())); + if (!constants.Timing.MinimizeLogging) { + DogLog.log(basePath + "Velocity", getVelocity().in(RadiansPerSecond)); + DogLog.log(basePath + "Acceleration", getAcceleration().in(RadiansPerSecondPerSecond)); + DogLog.log(basePath + "AppliedVoltage", getAppliedVoltage().in(Volts)); + DogLog.log(basePath + "StatorCurrent", getStatorCurrent().in(Amps)); + DogLog.log(basePath + "SupplyCurrent", getSupplyCurrent().in(Amps)); + DogLog.log(basePath + "ProfilePosition", profilePosition.in(Radians)); + DogLog.log( + basePath + "DataTimestamp", + StatusUtil.toFPGATimestamp(positionSignal.getTimestamp().getTime())); + + LoggingUtil.log(basePath + "ControlRequest", lastControlRequest); + } // If running a position or velocity request, log the target position or // velocity in radians. The target is logged as part of the control @@ -205,8 +209,6 @@ public void logTelemetry(String basePath) { RotationsPerSecond.of(velocityControl.Velocity).in(RadiansPerSecond), RadiansPerSecond); } - - LoggingUtil.log(basePath + "ControlRequest", lastControlRequest); } /** 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 f09ea8bb..f14a101a 100644 --- a/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java +++ b/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java @@ -74,20 +74,22 @@ public void logTelemetry(String basePath) { field.setRobotPose(robotPose); - Pose2d[] modulePoses = new Pose2d[4]; - SwerveModuleState[] moduleStates = odometry.getStates(); - - for (int i = 0; i < 4; i++) { - Pose2d relativePose = - new Pose2d( - constants.Structure.WheelBase.div(2).in(Meters) * (i < 2 ? 1 : -1), - constants.Structure.TrackWidth.div(2).in(Meters) * (i % 2 == 0 ? 1 : -1), - moduleStates[i].angle); + if (!constants.Timing.MinimizeLogging) { + Pose2d[] modulePoses = new Pose2d[4]; + SwerveModuleState[] moduleStates = odometry.getStates(); + + for (int i = 0; i < 4; i++) { + Pose2d relativePose = + new Pose2d( + constants.Structure.WheelBase.div(2).in(Meters) * (i < 2 ? 1 : -1), + constants.Structure.TrackWidth.div(2).in(Meters) * (i % 2 == 0 ? 1 : -1), + moduleStates[i].angle); + + modulePoses[i] = robotPose.plus(relativePose.minus(new Pose2d())); + } - modulePoses[i] = robotPose.plus(relativePose.minus(new Pose2d())); + field.getObject("Swerve Modules").setPoses(modulePoses); } - - field.getObject("Swerve Modules").setPoses(modulePoses); } /** diff --git a/src/main/java/com/team6962/lib/vision/AprilTagVision.java b/src/main/java/com/team6962/lib/vision/AprilTagVision.java index 4f2e49b8..b7765b0c 100644 --- a/src/main/java/com/team6962/lib/vision/AprilTagVision.java +++ b/src/main/java/com/team6962/lib/vision/AprilTagVision.java @@ -9,11 +9,11 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.HashMap; -import java.util.List; import java.util.Map; import org.photonvision.simulation.VisionSystemSim; @@ -23,6 +23,9 @@ * drive's odometry. */ public class AprilTagVision extends SubsystemBase { + /** Notifier for periodic vision updates. */ + private final Notifier notifier = new Notifier(this::update); + /** The swerve drive subsystem to provide vision measurements to. */ private final CommandSwerveDrive swerveDrive; @@ -63,6 +66,8 @@ public AprilTagVision(CommandSwerveDrive swerveDrive, AprilTagVisionConstants vi } DogLog.log("Vision/Cameras/Count", visionConstants.Cameras.size()); + + notifier.startPeriodic(0.02); } /** @@ -70,31 +75,11 @@ public AprilTagVision(CommandSwerveDrive swerveDrive, AprilTagVisionConstants vi * * @param camera The AprilTagCamera to add to the system. */ - public void addCamera(AprilTagCamera camera) { + private void addCamera(AprilTagCamera camera) { cameras.put(camera.getName(), camera); } - /** - * Gets an immutable list of all cameras in the vision system. - * - * @return A list containing all registered AprilTagCamera instances. - */ - public List getCameras() { - return List.copyOf(cameras.values()); - } - - /** - * Retrieves a camera by name. - * - * @param name The name of the camera to retrieve. - * @return The AprilTagCamera with the given name, or null if not found. - */ - public AprilTagCamera getCamera(String name) { - return cameras.get(name); - } - - @Override - public void periodic() { + private void update() { // Update vision simulation with current robot pose if (visionSystemSim != null) { visionSystemSim.update(swerveDrive.getSimulation().getRobotPosition()); @@ -126,8 +111,7 @@ public void periodic() { : visionConstants.MinTagsForHeadingUpdateWhileDisabled); if (!canUpdateRotation) { - measurement = - measurement.withAdjustedRotation(swerveDrive.getLocalization().getRotation3d()); + measurement = measurement.withIgnoredRotation(); } // Add the measurement to the swerve drive's localization system diff --git a/src/main/java/com/team6962/lib/vision/AprilTagVisionMeasurement.java b/src/main/java/com/team6962/lib/vision/AprilTagVisionMeasurement.java index af061967..fee91db8 100644 --- a/src/main/java/com/team6962/lib/vision/AprilTagVisionMeasurement.java +++ b/src/main/java/com/team6962/lib/vision/AprilTagVisionMeasurement.java @@ -1,6 +1,7 @@ package com.team6962.lib.vision; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.numbers.N1; @@ -91,6 +92,23 @@ public AprilTagVisionMeasurement withAdjustedRotation(Rotation3d newRotation) { return new AprilTagVisionMeasurement(adjustedEstimate, this.stdDevs); } + /** + * Returns a copy of this vision measurement with the rotation component of the pose effectively + * ignored by setting its standard deviation to a very large value. This can be used to avoid + * setting the rotation of the robot when enabled. + * + * @return A new AprilTagVisionMeasurement with the rotation component ignored. + */ + public AprilTagVisionMeasurement withIgnoredRotation() { + return new AprilTagVisionMeasurement( + this.photonEstimate, + VecBuilder.fill( + this.stdDevs.get(0, 0), + this.stdDevs.get(0, 1), + this.stdDevs.get(0, 2), + Double.MAX_VALUE)); + } + /** * Creates a new AprilTagVisionMeasurement whose pose is expressed relative to the supplied * origin. diff --git a/src/main/java/frc/robot/Preferences.java b/src/main/java/frc/robot/Preferences.java index 41e2aaba..675472cb 100644 --- a/src/main/java/frc/robot/Preferences.java +++ b/src/main/java/frc/robot/Preferences.java @@ -20,7 +20,7 @@ public class Preferences { * should usually be false when visualizing with AdvantageScope's 3D field from a driver's * perspective. */ - public static final boolean reorientControlsInSimulation = true; + public static final boolean reorientControlsInSimulation = false; /** * The robot constants to use in simulation. This should be set to a constants class that diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c75c895d..44728ec1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.auto.DriveStraightAuto; -import frc.robot.auto.ShooterFunctions; +import frc.robot.auto.shoot.ShooterFunctions; import frc.robot.constants.RobotConstants; import frc.robot.controls.TeleopControls; import frc.robot.subsystems.climb.Climb; @@ -68,18 +68,14 @@ public RobotContainer() { aprilTagVision = new AprilTagVision(swerveDrive, constants.getAprilTagVisionConstants()); fuelClumpLocalization = new SphereClumpLocalization(swerveDrive, constants.getSphereCameraConstants()); + shooterFunctions = + new ShooterFunctions( + RobotBase.isSimulation() ? "sim_shooter_hub_data.csv" : "shooter_hub_data.csv"); teleopControls = new TeleopControls(this); teleopControls.configureBindings(); driveStraightAuto = new DriveStraightAuto(this); - try { - shooterFunctions = new ShooterFunctions(); - } catch (Exception e) { - e.printStackTrace(); - throw e; - } - configureAutonomousChooser(); visualizer = new RobotVisualizer(this); diff --git a/src/main/java/frc/robot/auto/ShooterFunctions.java b/src/main/java/frc/robot/auto/ShooterFunctions.java deleted file mode 100644 index 03e48f87..00000000 --- a/src/main/java/frc/robot/auto/ShooterFunctions.java +++ /dev/null @@ -1,383 +0,0 @@ -package frc.robot.auto; - -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import com.team6962.lib.math.CSVLoader; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Distance; -import java.io.IOException; -import org.apache.commons.math3.analysis.MultivariateFunction; -import org.apache.commons.math3.analysis.UnivariateFunction; -import org.apache.commons.math3.analysis.interpolation.MicrosphereInterpolator; -import org.apache.commons.math3.analysis.interpolation.SplineInterpolator; - -public class ShooterFunctions { - /** - * Path to the CSV file containing 2D hood angle calibration data for hub shots. Formatted as - * Distance, Velocity, Angle - */ - private static final String anglePathHub = "hoodangledatahub.csv"; - - /** - * Path to the CSV file containing 2-column flywheel velocity calibration data for hub shots. - * Formatted as Distance, Velocity - */ - private static final String velocityPathHub = "flywheelvelocitydatahub.csv"; - - /** - * Path to the CSV file containing hood angle and flywheel velocity calibration data for passing. - * Formatted as Distance, Angle - */ - private static final String passingPath = "passingdata.csv"; - - /** - * Path to the CSV file containing minimum and maximum velocities to shoot at. Formatted as - * Distance, Velocity, Angle - */ - private static final String velocityRangeHub = "velocityrangehub.csv"; - - /** Minimum valid distance to the target for hub shots, in inches. */ - private static final double minDistanceHub = 57.5; // Calibrated for our shooter A testing - - /** Maximum valid distance to the target for hub shots, in inches. */ - private static final double maxDistanceHub = 263.0; // Calibrated for our shooter A testing - - /** Minimum valid distance to the target for passing, in inches. */ - private static final double minDistancePass = 57.5; // Needs to be changed - - /** Maximum valid distance to the target for passing, in inches. */ - private static final double maxDistancePass = 263.0; // Needs to be changed - - private MultivariateFunction hoodAngleFunctionHub; - private UnivariateFunction flywheelVelocityFunctionHub; - private UnivariateFunction hoodAngleFunctionPass; - private UnivariateFunction flywheelVelocityFunctionPass; - private UnivariateFunction minVelocityHub; - private UnivariateFunction maxVelocityHub; - - public ShooterFunctions() { - try { - this.hoodAngleFunctionHub = loadHoodAngleDataHub(); - this.flywheelVelocityFunctionHub = loadFlywheelDataHub(); - this.hoodAngleFunctionPass = loadHoodAngleDataPass(); - this.flywheelVelocityFunctionPass = loadFlywheelDataPass(); - this.minVelocityHub = loadMinVelocityHub(); - this.maxVelocityHub = loadMaxVelocityHub(); - } catch (IOException e) { - e.printStackTrace(); - } - } - - /** - * Loads shooter calibration data from a CSV file and returns an interpolating function. - * - *

The returned {@code MultivariateFunction} accepts a {@code double[]} of length 2 and returns - * an interpolated scalar value for that 2D input. - * - * @return a {@code MultivariateFunction} that interpolates the 2-dimensional input to a scalar - * output using the data loaded from the CSV file - * @throws IOException if an I/O error occurs while reading the CSV file at {@code path} - */ - @SuppressWarnings("deprecation") - private MultivariateFunction loadHoodAngleDataHub() throws IOException { - MicrosphereInterpolator interpolator = new MicrosphereInterpolator(); - double[][] data = CSVLoader.loadCSV(anglePathHub); - double[][] x = new double[data.length][2]; - for (int i = 0; i < data.length; i++) { - x[i][0] = data[i][0]; - x[i][1] = data[i][1]; - } - double[] y = new double[data.length]; - for (int i = 0; i < data.length; i++) { - y[i] = data[i][2]; - } - return interpolator.interpolate(x, y); - } - - /** - * Loads two-column numeric data from the CSV file referenced by the field {@code path2} and - * returns a spline-based interpolating function. - * - * @return an {@code UnivariateFunction} representing the spline interpolation of the CSV data - * (f(x) ≈ y) - * @throws IOException if an I/O error occurs while reading the CSV file at {@code path2} - */ - private UnivariateFunction loadFlywheelDataHub() throws IOException { - SplineInterpolator interpolator = new SplineInterpolator(); - double[][] data = CSVLoader.loadCSV(velocityPathHub); - double[] x = new double[data.length]; - for (int i = 0; i < data.length; i++) { - x[i] = data[i][0]; - } - double[] y = new double[data.length]; - for (int i = 0; i < data.length; i++) { - y[i] = data[i][1]; - } - return interpolator.interpolate(x, y); - } - - /** - * Loads shooter calibration data for passing from a CSV file and returns an interpolating - * function. - * - * @return a {@code UnivariateFunction} that interpolates the distance to a hood angle setpoint - * using the data loaded from the CSV file - * @throws IOException if an I/O error occurs while reading the CSV file at {@code anglePathPass} - */ - private UnivariateFunction loadHoodAngleDataPass() throws IOException { - SplineInterpolator interpolator = new SplineInterpolator(); - double[][] data = CSVLoader.loadCSV(passingPath); - double[] x = new double[data.length]; - for (int i = 0; i < data.length; i++) { - x[i] = data[i][0]; - } - double[] y = new double[data.length]; - for (int i = 0; i < data.length; i++) { - y[i] = data[i][2]; - } - return interpolator.interpolate(x, y); - } - - /** - * Loads flywheel velocity calibration data for passing from a CSV file and returns an - * interpolating function. - * - * @return a {@code UnivariateFunction} that interpolates the distance to a flywheel velocity - * setpoint using the data loaded from the CSV file - * @throws IOException if an I/O error occurs while reading the CSV file at {@code - * velocityPathPass} - */ - private UnivariateFunction loadFlywheelDataPass() throws IOException { - SplineInterpolator interpolator = new SplineInterpolator(); - double[][] data = CSVLoader.loadCSV(passingPath); - double[] x = new double[data.length]; - for (int i = 0; i < data.length; i++) { - x[i] = data[i][0]; - } - double[] y = new double[data.length]; - for (int i = 0; i < data.length; i++) { - y[i] = data[i][1]; - } - return interpolator.interpolate(x, y); - } - - /** - * Loads minimum flywheel velocity data for hub shots from a CSV file and returns an interpolating - * function. - * - * @return a {@code UnivariateFunction} that interpolates the distance to a velocity setpoint - * using the data loaded from the CSV file - * @throws IOException if an I/O error occurs while reading the CSV file at {@code - * velocityRangeHub} - */ - private UnivariateFunction loadMinVelocityHub() throws IOException { - SplineInterpolator interpolator = new SplineInterpolator(); - double[][] data = CSVLoader.loadCSV(velocityRangeHub); - double[] x = new double[data.length]; - for (int i = 0; i < data.length; i++) { - x[i] = data[i][0]; - } - double[] y = new double[data.length]; - for (int i = 0; i < data.length; i++) { - y[i] = data[i][1]; - } - return interpolator.interpolate(x, y); - } - - /** - * Loads maximum flywheel velocity data for hub shots from a CSV file and returns an interpolating - * function. - * - * @return a {@code UnivariateFunction} that interpolates the distance to a velocity setpoint - * using the data loaded from the CSV file - * @throws IOException if an I/O error occurs while reading the CSV file at {@code - * velocityRangeHub} - */ - private UnivariateFunction loadMaxVelocityHub() throws IOException { - SplineInterpolator interpolator = new SplineInterpolator(); - double[][] data = CSVLoader.loadCSV(velocityRangeHub); - double[] x = new double[data.length]; - for (int i = 0; i < data.length; i++) { - x[i] = data[i][0]; - } - double[] y = new double[data.length]; - for (int i = 0; i < data.length; i++) { - y[i] = data[i][2]; - } - return interpolator.interpolate(x, y); - } - - /** - * Returns the MultivariateFunction used by the shooter subsystem to compute hood angle setpoints. - * - * @return the configured MultivariateFunction used to compute hood angle setpoints, or {@code - * null} if no function has been configured - */ - public MultivariateFunction getHoodAngleFunctionHub() { - return hoodAngleFunctionHub; - } - - /** - * Returns the UnivariateFunction used by the shooter subsystem to compute flywheel velocity - * setpoints. - * - * @return the configured UnivariateFunction used to compute flywheel velocity setpoints, or - * {@code null} if no function has been configured - */ - public UnivariateFunction getFlywheelVelocityFunctionHub() { - return flywheelVelocityFunctionHub; - } - - /** - * Returns the UnivariateFunction used by the shooter subsystem to compute hood angle setpoints - * for passing. - * - * @return the configured UnivariateFunction used to compute hood angle setpoints for passing, or - * {@code null} if no function has been configured - */ - public UnivariateFunction getHoodAngleFunctionPass() { - return hoodAngleFunctionPass; - } - - /** - * Returns the UnivariateFunction used by the shooter subsystem to compute flywheel velocity - * setpoints for passing. - * - * @return the configured UnivariateFunction used to compute flywheel velocity setpoints for - * passing, or {@code null} if no function has been configured - */ - public UnivariateFunction getFlywheelVelocityFunctionPass() { - return flywheelVelocityFunctionPass; - } - - /** - * Calculates the hood angle required to score given the current distance to the target and the - * shooter wheel angular velocity. Clamps the output to the maximum and minimum hood angles if the - * input distance is outside the valid range. - * - * @param distance the distance to the target (will be converted to inches for the internal - * calculation) - * @param velocity the shooter wheel angular velocity (will be converted to rotations per second - * for the internal calculation) - * @return an Angle representing the hood setpoint in degrees needed for the shot - */ - public Angle getHoodAngleHub(Distance distance, AngularVelocity velocity) { - if (isDistanceWithinValidHubRange(distance) - && isVelocityWithinValidHubRange(distance, velocity)) { - return Degrees.of( - hoodAngleFunctionHub.value( - new double[] {distance.in(Inches), velocity.in(RotationsPerSecond)})); - } else if (isVelocityWithinValidHubRange(distance, velocity)) { - return Degrees.of( - hoodAngleFunctionHub.value( - new double[] { - MathUtil.clamp(distance.in(Inches), minDistanceHub, maxDistanceHub), - velocity.in(RotationsPerSecond) - })); - } else - return Degrees.of( - hoodAngleFunctionHub.value( - new double[] { - MathUtil.clamp(distance.in(Inches), minDistanceHub, maxDistanceHub), - MathUtil.clamp( - velocity.in(RotationsPerSecond), - minVelocityHub.value(distance.in(Inches)), - maxVelocityHub.value(distance.in(Inches))) - })); - } - - /** - * Calculates the required flywheel angular velocity to reach a target at the specified distance - * and shooter angle. Clamps the output to the maximum and minimum flywheel velocities if the - * input distance is outside the valid range. - * - * @param distance the distance to the target; will be converted to inches before evaluation - * @param angle the shooter/target angle (currently unused by this method) - * @return the required flywheel angular velocity as an AngularVelocity (rotations per second) - */ - public AngularVelocity getFlywheelVelocityHub(Distance distance) { - if (isDistanceWithinValidHubRange(distance)) { - return RotationsPerSecond.of(flywheelVelocityFunctionHub.value(distance.in(Inches))); - } else - return RotationsPerSecond.of( - flywheelVelocityFunctionHub.value( - MathUtil.clamp(distance.in(Inches), minDistanceHub, maxDistanceHub))); - } - - /** - * Calculates the hood angle required for passing given the current distance to the target. Clamps - * the output to the maximum and minimum hood angles if the input distance is outside the valid - * range. - * - * @param distance the distance to the target (will be converted to inches for the internal - * calculation) - * @return an Angle representing the hood setpoint in degrees needed for passing - */ - public Angle getHoodAnglePass(Distance distance) { - if (isDistanceWithinValidPassRange(distance)) { - return Degrees.of(hoodAngleFunctionPass.value(distance.in(Inches))); - } else - return Degrees.of( - hoodAngleFunctionPass.value( - MathUtil.clamp(distance.in(Inches), minDistancePass, maxDistancePass))); - } - - /** - * Calculates the required flywheel angular velocity for passing given the current distance to the - * target. Clamps the output to the maximum and minimum flywheel velocities if the input distance - * is outside the valid range. - * - * @param distance the distance to the target; will be converted to inches before evaluation - * @return the required flywheel angular velocity for passing as an AngularVelocity (rotations per - * second) - */ - public AngularVelocity getFlywheelVelocityPass(Distance distance) { - if (isDistanceWithinValidPassRange(distance)) { - return RotationsPerSecond.of(flywheelVelocityFunctionPass.value(distance.in(Inches))); - } else - return RotationsPerSecond.of( - flywheelVelocityFunctionPass.value( - MathUtil.clamp(distance.in(Inches), minDistancePass, maxDistancePass))); - } - - /** - * Determines whether the given distance falls within the configured valid range. - * - * @param distance the Distance to check; will be converted to inches for comparison - * @return true if the distance (in inches) is greater than or equal to minDistance and less than - * or equal to maxDistance - */ - public boolean isDistanceWithinValidHubRange(Distance distance) { - double distanceInInches = distance.in(Inches); - return distanceInInches >= minDistanceHub && distanceInInches <= maxDistanceHub; - } - - /** - * Determines whether the given velocity falls within the configured valid range. - * - * @param distance the Distance to check; will be converted to inches for comparison - * @param velocity the AngularVelocity to check - * @return true if the velocity is within the valid range for the given distance - */ - public boolean isVelocityWithinValidHubRange(Distance distance, AngularVelocity velocity) { - double velocityInRPS = velocity.in(RotationsPerSecond); - return velocityInRPS >= minVelocityHub.value(distance.in(Inches)) - && velocityInRPS <= maxVelocityHub.value(distance.in(Inches)); - } - - /** - * Determines whether the given distance falls within the configured valid range for passing. - * - * @param distance the Distance to check; will be converted to inches for comparison - * @return true if the distance (in inches) is greater than or equal to minDistancePass and less - * than or equal to maxDistancePass - */ - public boolean isDistanceWithinValidPassRange(Distance distance) { - double distanceInInches = distance.in(Inches); - return distanceInInches >= minDistancePass && distanceInInches <= maxDistancePass; - } -} diff --git a/src/main/java/frc/robot/auto/shoot/AutoShoot.java b/src/main/java/frc/robot/auto/shoot/AutoShoot.java new file mode 100644 index 00000000..1b70aecc --- /dev/null +++ b/src/main/java/frc/robot/auto/shoot/AutoShoot.java @@ -0,0 +1,384 @@ +package frc.robot.auto.shoot; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; + +import com.team6962.lib.commands.CommandUtil; +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.filter.LinearFilter; +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.geometry.Twist2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.hood.ShooterHood; +import frc.robot.subsystems.shooterrollers.ShooterRollers; +import frc.robot.subsystems.turret.Turret; +import java.util.function.Supplier; +import org.apache.commons.math3.util.Pair; + +/** A command that automatically aims and spins up the shooter rollers to shoot at a target. */ +public class AutoShoot extends Command { + public static Translation2d HUB_TRANSLATION = + new Translation2d(4.62562575, 4.03463125); // Measured with CAD + + /** + * The swerve drive subsystem, used to get the shooter's pose and velocity for calculating + * shooting parameters. + */ + private CommandSwerveDrive swerveDrive; + + /** The turret subsystem, used to aim the shooter in azimuth. */ + private Turret turret; + + /** A filter used to reduce noise in the turret's target velocity measurements. */ + private LinearFilter turretVelocityFilter = LinearFilter.movingAverage((int) (0.1 / 0.02)); + + /** The hood subsystem, used to aim the shooter in elevation. */ + private ShooterHood hood; + + /** A filter used to reduce noise in the hood's target velocity measurements. */ + private LinearFilter hoodVelocityFilter = LinearFilter.movingAverage((int) (0.1 / 0.02)); + + /** The shooter rollers subsystem, used to spin up the shooter wheels to the target speed. */ + private ShooterRollers rollers; + + /** The shooter functions, which provide access to shooter calibration data. */ + private ShooterFunctions shooterFunctions; + + /** A supplier that provides the target position. */ + private Supplier targetSupplier; + + /** The target azimuth angle for the turret. */ + private Angle turretAngleTarget; + + /** The target azimuth velocity of the turret. */ + private AngularVelocity turretVelocityTarget = RotationsPerSecond.of(0); + + /** The target elevation angle for the hood. */ + private Angle hoodAngleTarget; + + /** The target elevation velocity of the hood. */ + private AngularVelocity hoodVelocityTarget = RotationsPerSecond.of(0); + + /** The target angular velocity for the rollers. */ + private AngularVelocity rollerSpeedTarget; + + /** Whether the system is ready to shoot. */ + private boolean readyToShoot; + + /** Whether this command is currently running. */ + private boolean thisCommandRunning = false; + + /** The last time that periodic() was executed. */ + private double previousPeriodicTimestamp = -1.0; + + /** + * Creates a new AutoShoot command, which automatically aims and spins up the shooter rollers to + * shoot at a target. + * + * @param swerveDrive the swerve drive + * @param turret the turret + * @param hood the hood + * @param rollers the shooter rollers + * @param targetSupplier a supplier that provides the target position in 3D space + */ + public AutoShoot( + CommandSwerveDrive swerveDrive, + Turret turret, + ShooterHood hood, + ShooterRollers rollers, + ShooterFunctions shooterFunctions, + Supplier targetSupplier, + Supplier hoodAngleOverride, + Supplier rollerVelocityOverride) { + this.swerveDrive = swerveDrive; + this.turret = turret; + this.hood = hood; + this.rollers = rollers; + this.targetSupplier = targetSupplier; + this.shooterFunctions = shooterFunctions; + + // Create triggers and bind commands to them in order to continuously update + // subsystem setpoints while this command is running. + Trigger runningTrigger = new Trigger(() -> thisCommandRunning); + + Command turretCommand = turret.track(() -> turretAngleTarget, () -> turretVelocityTarget); + Command hoodCommand = + hood.track( + () -> hoodAngleOverride.get() != null ? hoodAngleOverride.get() : hoodAngleTarget, + () -> + hoodAngleOverride.get() == null ? hoodVelocityTarget : RotationsPerSecond.of(0)) + .repeatedly(); + Command rollersCommand = + rollers + .shoot( + () -> + rollerVelocityOverride.get() != null + ? rollerVelocityOverride.get() + : rollerSpeedTarget) + .repeatedly(); + + runningTrigger.whileTrue(turretCommand); + runningTrigger + .and(() -> CommandUtil.isClearToOverride(hood, hoodCommand)) + .whileTrue(hoodCommand); + runningTrigger + .and(() -> CommandUtil.isClearToOverride(rollers, rollersCommand)) + .whileTrue(rollersCommand); + } + + @Override + public void initialize() { + readyToShoot = false; + turretAngleTarget = null; + hoodAngleTarget = null; + rollerSpeedTarget = null; + turretVelocityTarget = null; + hoodVelocityTarget = null; + previousPeriodicTimestamp = Timer.getFPGATimestamp(); + } + + @Override + public void end(boolean interrupted) { + thisCommandRunning = false; + readyToShoot = false; + + DogLog.log("AutoShoot/Running", false); + DogLog.log("AutoShoot/ReadyToShoot", readyToShoot); + } + + /** + * Returns a trigger that is active when the shooter is ready to shoot. + * + * @return a trigger that is active when the shooter is ready to shoot + */ + public Trigger isReadyToShoot() { + return new Trigger(() -> readyToShoot); + } + + /** + * Predicts the destination of a projectile fired from the shooter given the shooter's pose, + * velocity, azimuth angle, and hood angle. + * + * @param distance the distance to the target if the shooter is stationary + * @param shooterPose the pose of the shooter + * @param shooterVelocity the velocity of the shooter + * @param azimuthAngle the azimuth angle of the shooter + * @param hoodAngle the hood angle of the shooter + * @return the predicted destination of the projectile + */ + private Translation2d predictDestination( + Distance distance, + Pose2d shooterPose, + TranslationalVelocity shooterVelocity, + Angle azimuthAngle, + Angle hoodAngle) { + + // Calculate the displacement of the projectile during flight + Translation2d displacement = + new Translation2d(distance.in(Meters), new Rotation2d(azimuthAngle)); + + // Account for the shooter's initial velocity during flight + Time flightTime = shooterFunctions.getFlightTime(distance); + displacement = + displacement.plus( + shooterVelocity + .times(flightTime) + .times( + AutoShootConstants.initialVelocityDisplacementScalarFunction.value( + new double[] {distance.in(Inches), hoodAngle.in(Degrees)}))); + + // Calculate the final destination of the projectile + return shooterPose.getTranslation().plus(displacement); + } + + /** + * Calculates the static shooting angles (azimuth and hood) required to hit the target from the + * shooter's pose. + * + * @param shooterPose the pose of the shooter + * @param target the target position + * @return a pair containing the azimuth and hood angles + */ + private Pair getStaticShootingAngles(Pose2d shooterPose, Translation2d target) { + // Calculate the azimuth angle to the target + Angle azimuthAngle = target.minus(shooterPose.getTranslation()).getAngle().getMeasure(); + + // Calculate the hood angle using the hood angle function + Distance horizontalDistance = Meters.of(shooterPose.getTranslation().getDistance(target)); + + Angle hoodAngle = shooterFunctions.getHoodAngle(horizontalDistance); + + return new Pair(azimuthAngle, hoodAngle); + } + + /** + * Calculates the moving shooting angles (azimuth and hood) required to hit the target from the + * shooter's pose and velocity. + * + * @param shooterPose the pose of the shooter + * @param shooterVelocity the velocity of the shooter + * @param rollersAngularVelocity the angular velocity of the shooter rollers + * @param target the target position + * @return a pair containing the azimuth and hood angles + */ + private Pair getMovingShootingAngles( + Pose2d shooterPose, TranslationalVelocity shooterVelocity, Translation2d target) { + Pair angles = getStaticShootingAngles(shooterPose, target); + Translation2d adjustedTarget = target; + + // Iteratively refine the shooting angles until the predicted destination is + // within the optimization tolerance of the target. + for (int i = 0; i < AutoShootConstants.optimizationIterations; i++) { + // Predict the destination for if the projectile were to be fired with the + // calculated angles + Distance distance = Meters.of(shooterPose.getTranslation().getDistance(adjustedTarget)); + + Translation2d predictedDestination = + predictDestination( + distance, shooterPose, shooterVelocity, angles.getFirst(), angles.getSecond()); + + // Adjust the target based on the error between the predicted destination and the + // actual target + Translation2d error = predictedDestination.minus(target); + adjustedTarget = adjustedTarget.minus(error); + + // Calculate the static shooting angles to hit the adjusted target + angles = getStaticShootingAngles(shooterPose, adjustedTarget); + } + + return angles; + } + + private static class ShootingParameters { + public Angle turretAngle; + public Angle hoodAngle; + public AngularVelocity rollerSpeed; + + public ShootingParameters(Angle turretAngle, Angle hoodAngle, AngularVelocity rollerSpeed) { + this.turretAngle = turretAngle; + this.hoodAngle = hoodAngle; + this.rollerSpeed = rollerSpeed; + } + + public void log(String path) { + DogLog.log(path + "/TurretAngle", turretAngle.in(Radians), Radians); + DogLog.log(path + "/HoodAngle", hoodAngle.in(Degrees), Degrees); + DogLog.log(path + "/RollerSpeed", rollerSpeed.in(RotationsPerSecond), RotationsPerSecond); + } + } + + private ShootingParameters calculate(Time poseExtrapolationTime) { + // Get the target position and initial shooter state + Translation2d target = targetSupplier.get(); + + Twist2d twist = swerveDrive.getArcVelocity(); + twist.dx *= poseExtrapolationTime.in(Seconds); + twist.dy *= poseExtrapolationTime.in(Seconds); + twist.dtheta *= poseExtrapolationTime.in(Seconds); + Pose2d shooterPose = + swerveDrive.getPosition2d().exp(twist).plus(AutoShootConstants.shooterTransform); + TranslationalVelocity shooterVelocity = swerveDrive.getTranslationalVelocity(); + + DogLog.log("AutoShoot/Distance", shooterPose.getTranslation().getDistance(target)); + + // Calculate the ideal shooting angles and roller speed to hit the target + Pair idealAngles = getMovingShootingAngles(shooterPose, shooterVelocity, target); + + Angle turretAngleTarget = idealAngles.getFirst().minus(shooterPose.getRotation().getMeasure()); + Angle hoodAngleTarget = idealAngles.getSecond(); + AngularVelocity rollerSpeedTarget = + shooterFunctions.getFlywheelVelocity( + Meters.of(target.getDistance(shooterPose.getTranslation()))); + + return new ShootingParameters(turretAngleTarget, hoodAngleTarget, rollerSpeedTarget); + } + + @Override + public void execute() { + // Get the target position and initial shooter state + Translation2d target = targetSupplier.get(); + + DogLog.log("AutoShoot/TargetX", target.getX()); + DogLog.log("AutoShoot/TargetY", target.getY()); + + // Calculate the ideal shooting angles and roller speed to hit the target + ShootingParameters appliedShootingParameters = calculate(Seconds.of(0.03)); + ShootingParameters currentShootingParameters = calculate(Seconds.of(0)); + + appliedShootingParameters.log("AutoShoot/AppliedShootingParameters"); + currentShootingParameters.log("AutoShoot/CurrentShootingParameters"); + + Angle previousTurretAngleTarget = turretAngleTarget; + Angle previousHoodAngleTarget = hoodAngleTarget; + + // Set the target angles and roller speed + turretAngleTarget = appliedShootingParameters.turretAngle; + hoodAngleTarget = appliedShootingParameters.hoodAngle; + rollerSpeedTarget = appliedShootingParameters.rollerSpeed; + + if (previousTurretAngleTarget != null && previousHoodAngleTarget != null) { + double timeSinceLastPeriodic = Timer.getFPGATimestamp() - previousPeriodicTimestamp; + + turretVelocityTarget = + RotationsPerSecond.of( + turretVelocityFilter.calculate( + AngleMath.toContinuous( + AngleMath.toDiscrete(turretAngleTarget), previousTurretAngleTarget) + .minus(previousTurretAngleTarget) + .div(Seconds.of(timeSinceLastPeriodic)) + .in(RotationsPerSecond))); + + hoodVelocityTarget = + RotationsPerSecond.of( + hoodVelocityFilter.calculate( + hoodAngleTarget + .minus(previousHoodAngleTarget) + .div(Seconds.of(timeSinceLastPeriodic)) + .in(RotationsPerSecond))); + + DogLog.log( + "AutoShoot/AppliedShootingParameters/TurretVelocity", + turretVelocityTarget.in(RadiansPerSecond), + RadiansPerSecond); + DogLog.log( + "AutoShoot/AppliedShootingParameters/HoodVelocity", + hoodVelocityTarget.in(DegreesPerSecond), + DegreesPerSecond); + } + + previousPeriodicTimestamp = Timer.getFPGATimestamp(); + + // Determine if the system is ready to shoot based on whether the shooter is at the target + // angles + // and roller speed + readyToShoot = + rollers + .getAngularVelocity() + .isNear(rollerSpeedTarget, AutoShootConstants.flywheelVelocityTolerance) + && hood.getPosition().isNear(hoodAngleTarget, AutoShootConstants.hoodAngleTolerance) + && AngleMath.toContinuous(AngleMath.toDiscrete(turret.getPosition()), turretAngleTarget) + .isNear(turretAngleTarget, AutoShootConstants.turretAngleTolerance); + + thisCommandRunning = true; + + DogLog.log("AutoShoot/Running", true); + DogLog.log("AutoShoot/ReadyToShoot", readyToShoot); + } +} diff --git a/src/main/java/frc/robot/auto/shoot/AutoShootConstants.java b/src/main/java/frc/robot/auto/shoot/AutoShootConstants.java new file mode 100644 index 00000000..41ee7dfa --- /dev/null +++ b/src/main/java/frc/robot/auto/shoot/AutoShootConstants.java @@ -0,0 +1,47 @@ +package frc.robot.auto.shoot; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.team6962.lib.math.ConstantFunction; +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.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import org.apache.commons.math3.analysis.MultivariateFunction; + +/** + * Constants for the AutoShoot command. These constants include functions that take in various + * parameters and return the corresponding shooting parameters (hood angle, roller speed, etc). + * These functions are used to calculate the optimal shooting parameters based on the current state + * of the robot and the target. + */ +public class AutoShootConstants { + /** + * Function that takes in (target distance, hood angle) and returns initial velocity displacement + * scalar. This function is used to account for drag and the Magnus effect when shooting on the + * move. Uses inches and degrees as inputs. + */ + public static final MultivariateFunction initialVelocityDisplacementScalarFunction = + new ConstantFunction(0.9); + + /** + * The number of iterations to run the optimization for when calculating the optimal shooting + * parameters. + */ + public static final int optimizationIterations = 20; + + /** Maximum allowable flywheel velocity error to shoot. */ + public static final AngularVelocity flywheelVelocityTolerance = RotationsPerSecond.of(0.5); + + /** Maximum allowable hood angle error to shoot. */ + public static final Angle hoodAngleTolerance = Degrees.of(2); + + /** Maximum allowable turret angle error to shoot. */ + public static final Angle turretAngleTolerance = Degrees.of(2); + + /** Transform representing the shooter's position and orientation relative to the robot. */ + public static final Transform2d shooterTransform = + new Transform2d(new Translation2d(-0.17145, 0.17145), new Rotation2d()); +} diff --git a/src/main/java/frc/robot/auto/shoot/ShooterFunctions.java b/src/main/java/frc/robot/auto/shoot/ShooterFunctions.java new file mode 100644 index 00000000..cef08c14 --- /dev/null +++ b/src/main/java/frc/robot/auto/shoot/ShooterFunctions.java @@ -0,0 +1,173 @@ +package frc.robot.auto.shoot; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; + +import com.team6962.lib.math.CSVLoader; +import com.team6962.lib.math.MeasureUtil; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Time; +import java.io.IOException; +import org.apache.commons.math3.analysis.UnivariateFunction; +import org.apache.commons.math3.analysis.interpolation.LinearInterpolator; + +/** + * A set of functions that can be used to calculate shooting parameters (hood angle, flywheel + * velocity, etc) based on the current state of the robot and the target. + * + *

These functions are generated from a CSV file containing empirical data of the shooter's + * performance at various distances and angles. The CSV file should have the following columns: + * distance (in inches), hood angle (in degrees), flywheel velocity (in rotations per second), and + * flight time (in seconds). The class uses spline interpolation to create continuous functions from + * the discrete data points in the CSV file. + */ +public class ShooterFunctions { + /** The minimum distance for which the shooter functions are valid. */ + private Distance minDistance; + + /** The maximum distance for which the shooter functions are valid. */ + private Distance maxDistance; + + /** The minimum hood angle for which the shooter functions are valid. */ + private Angle minHoodAngle; + + /** The maximum hood angle for which the shooter functions are valid. */ + private Angle maxHoodAngle; + + /** The function that maps distance to hood angle. */ + private UnivariateFunction hoodAngleFunction; + + /** The function that maps distance to flywheel velocity. */ + private UnivariateFunction flywheelVelocityFunction; + + /** The function that maps hood angle to distance. */ + private UnivariateFunction distanceFunction; + + /** The function that maps distance to flight time. */ + private UnivariateFunction flightTimeFunction; + + public ShooterFunctions(String filePath) { + try { + LinearInterpolator interpolator = new LinearInterpolator(); + double[][] data = CSVLoader.loadCSV(filePath); + + double[] distances = new double[data.length]; + double[] hoodAngles = new double[data.length]; + double[] flywheelVelocities = new double[data.length]; + double[] flightTimes = new double[data.length]; + + double minDistanceInches = Double.MAX_VALUE; + double maxDistanceInches = Double.MIN_VALUE; + double minHoodAngleDegrees = Double.MAX_VALUE; + double maxHoodAngleDegrees = Double.MIN_VALUE; + + for (int i = 0; i < data.length; i++) { + distances[i] = data[i][0]; + hoodAngles[i] = data[i][1]; + flywheelVelocities[i] = data[i][2]; + flightTimes[i] = data[i][3]; + + double distance = data[i][0]; + double hoodAngle = data[i][1]; + + if (distance < minDistanceInches) { + minDistanceInches = distance; + } + + if (distance > maxDistanceInches) { + maxDistanceInches = distance; + } + + if (hoodAngle < minHoodAngleDegrees) { + minHoodAngleDegrees = hoodAngle; + } + + if (hoodAngle > maxHoodAngleDegrees) { + maxHoodAngleDegrees = hoodAngle; + } + } + + this.minDistance = Inches.of(minDistanceInches); + this.maxDistance = Inches.of(maxDistanceInches); + this.minHoodAngle = Degrees.of(minHoodAngleDegrees); + this.maxHoodAngle = Degrees.of(maxHoodAngleDegrees); + + this.hoodAngleFunction = interpolator.interpolate(distances, hoodAngles); + this.flywheelVelocityFunction = interpolator.interpolate(distances, flywheelVelocities); + this.distanceFunction = interpolator.interpolate(hoodAngles, distances); + this.flightTimeFunction = interpolator.interpolate(distances, flightTimes); + } catch (IOException e) { + e.printStackTrace(); + } + } + + /** + * Calculates the hood angle required to score given a distance to the target. Clamps the output + * to the maximum and minimum hood angles if the input distance is outside the valid range. + * + * @param distance the distance to the target + * @return an Angle representing the hood angle needed for the shot + */ + public Angle getHoodAngle(Distance distance) { + return Degrees.of( + hoodAngleFunction.value(MeasureUtil.clamp(distance, minDistance, maxDistance).in(Inches))); + } + + /** + * Predicts the distance to the target based on the given hood angle, assuming that the flywheel + * is at the right speed. Clamps the input hood angle to the maximum and minimum hood angles if it + * is outside the valid range. + * + * @param hoodAngle the hood angle + * @return a Distance representing the predicted distance to the target based on the hood angle + */ + public Distance getDistance(Angle hoodAngle) { + return Inches.of( + distanceFunction.value( + MeasureUtil.clamp(hoodAngle, minHoodAngle, maxHoodAngle).in(Degrees))); + } + + /** + * Calculates the required flywheel angular velocity to reach a target at the specified distance + * and shooter angle. Clamps the output to the maximum and minimum flywheel velocities if the + * input distance is outside the valid range. + * + * @param distance the distance to the target + * @return the required flywheel angular velocity as an AngularVelocity (rotations per second) + */ + public AngularVelocity getFlywheelVelocity(Distance distance) { + if (isDistanceWithinValidRange(distance)) { + return RotationsPerSecond.of(flywheelVelocityFunction.value(distance.in(Inches))); + } else + return RotationsPerSecond.of( + flywheelVelocityFunction.value( + MeasureUtil.clamp(distance, minDistance, maxDistance).in(Inches))); + } + + /** + * Calculates the flight time for a shot given the current distance to the target. Clamps the + * input distance to the maximum and minimum valid distances if it is outside the valid range. + * + * @param distance the distance to the target + * @return the flight time for the shot as a Time + */ + public Time getFlightTime(Distance distance) { + return Seconds.of( + flightTimeFunction.value(MeasureUtil.clamp(distance, minDistance, maxDistance).in(Inches))); + } + + /** + * Determines whether the given distance falls within the configured valid range. + * + * @param distance the Distance to check + * @return true if the distance is greater than or equal to minDistance and less than or equal to + * maxDistance + */ + public boolean isDistanceWithinValidRange(Distance distance) { + return distance.gte(minDistance) && distance.lte(maxDistance); + } +} diff --git a/src/main/java/frc/robot/constants/CompetitionBotConstants.java b/src/main/java/frc/robot/constants/CompetitionBotConstants.java index e949aea4..a9b896a1 100644 --- a/src/main/java/frc/robot/constants/CompetitionBotConstants.java +++ b/src/main/java/frc/robot/constants/CompetitionBotConstants.java @@ -185,7 +185,8 @@ public DrivetrainConstants getDrivetrainConstants() { .withControlLoopFrequency(Hertz.of(100)) .withSignalUpdateRate(Hertz.of(100)) .withTimesyncControlRequests(false) - .withUseThreadedControlLoop(true)) + .withUseThreadedControlLoop(true) + .withMinimizeLogging(true)) .withDriving( baseConstants .Driving @@ -261,7 +262,18 @@ public AprilTagVisionConstants getAprilTagVisionConstants() { new Rotation3d( Degrees.of(-18.224755).in(Radians), Degrees.of(-26.25).in(Radians), - Math.PI / 4)))) + Math.PI / 4))), + new AprilTagCameraConstants( + "Monochrome-4", + new Transform3d( + new Translation3d( + Inches.of(-10.293558).in(Meters), + Inches.of(-12.118).in(Meters), + Inches.of(20.601426).in(Meters)), + new Rotation3d( + Degrees.of(18.224755).in(Radians), + Degrees.of(-26.25).in(Radians), + -Math.PI / 4)))) // Note that standard deviations are not fully tuned .withSingleTagStdDevs(VecBuilder.fill(0.3, 0.3, 0.3, 1.5)) .withMultiTagStdDevs(VecBuilder.fill(0.1, 0.1, 0.1, 0.5)) @@ -280,22 +292,22 @@ public AprilTagVisionConstants getAprilTagVisionConstants() { @Override public SphereCameraConstants getSphereCameraConstants() { - return super.getSphereCameraConstants() - .withName("Color-2") - .withClassId(0) - .withFOVHeight(Rotation2d.fromDegrees(48.9)) - .withFOVWidth(Rotation2d.fromDegrees(70)) - .withCameraHeightPixels(800) - .withCameraWidthPixels(1280) - .withFocalLengthX(907.41) - .withFocalLengthY(907.64) - .withMaxDetectionRange(Meters.of(18.37)) // diagonal length of the field - .withSphereDiameter(Inches.of(5.91)) - .withMaxTargets(50) // Temporary value until we tune object detection on the practice field - .withRobotToCameraTransform( - new Transform3d( - new Translation3d(Inches.of(16.25).in(Meters), 0, Inches.of(20.0).in(Meters)), - new Rotation3d(0, Math.PI / 6, 0))); + return super.getSphereCameraConstants(); + // .withName("Color-2") + // .withClassId(0) + // .withFOVHeight(Rotation2d.fromDegrees(48.9)) + // .withFOVWidth(Rotation2d.fromDegrees(70)) + // .withCameraHeightPixels(800) + // .withCameraWidthPixels(1280) + // .withFocalLengthX(907.41) + // .withFocalLengthY(907.64) + // .withMaxDetectionRange(Meters.of(18.37)) // diagonal length of the field + // .withSphereDiameter(Inches.of(5.91)) + // .withMaxTargets(50) // Temporary value until we tune object detection on the practice field + // .withRobotToCameraTransform( + // new Transform3d( + // new Translation3d(Inches.of(16.25).in(Meters), 0, Inches.of(20.0).in(Meters)), + // new Rotation3d(0, Math.PI / 6, 0))); } @Override diff --git a/src/main/java/frc/robot/controls/TeleopControls.java b/src/main/java/frc/robot/controls/TeleopControls.java index ecb43add..fc183959 100644 --- a/src/main/java/frc/robot/controls/TeleopControls.java +++ b/src/main/java/frc/robot/controls/TeleopControls.java @@ -1,14 +1,18 @@ package frc.robot.controls; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import com.team6962.lib.swerve.commands.TeleopSwerveCommand; +import com.team6962.lib.commands.CommandUtil; +import com.team6962.lib.logging.LoggingUtil; import com.team6962.lib.swerve.commands.XBoxTeleopSwerveCommand; import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.RobotBase; @@ -21,6 +25,8 @@ import frc.robot.RobotContainer; import frc.robot.auto.AutoClimb; import frc.robot.auto.DriveToClump; +import frc.robot.auto.shoot.AutoShoot; +import frc.robot.auto.shoot.ShooterFunctions; import frc.robot.subsystems.climb.ClimbConstants; import frc.robot.subsystems.hood.ShooterHoodConstants; import frc.robot.subsystems.intakeextension.IntakeExtensionConstants; @@ -33,9 +39,12 @@ public class TeleopControls { private DriveToClump driveToClump; private CommandXboxController driver = new CommandXboxController(0); private CommandXboxController operator = new CommandXboxController(1); + private Distance shootingTestDistance = Inches.of(206); private boolean fineControl = false; private AngularVelocity flywheelVelocity = ShooterRollersConstants.FIXED_FLYWHEEL_VELOCITY; + private double tunableHoodAngle = ShooterHoodConstants.MIN_ANGLE.in(Degrees); + private double tunableRollerVelocity = 0; public TeleopControls(RobotContainer robot) { this.robot = robot; @@ -51,6 +60,27 @@ public TeleopControls(RobotContainer robot) { value -> { flywheelVelocity = RotationsPerSecond.of(value); }); + + DogLog.tunable( + "Shooting Test Distance (in)", + shootingTestDistance.in(Inches), + value -> { + shootingTestDistance = Inches.of(value); + }); + + DogLog.tunable( + "Override Shooting/Hood Angle", + tunableHoodAngle, + value -> { + tunableHoodAngle = value; + }); + + DogLog.tunable( + "Override Shooting/Roller Velocity", + tunableRollerVelocity, + value -> { + tunableRollerVelocity = value; + }); } public void configureBindings() { @@ -157,12 +187,7 @@ public void configureBindings() { .ignoringDisable(true)); // Shoot - WORKS - operator - .rightTrigger() - .whileTrue( - Commands.parallel( - robot.getShooterRollers().shoot(() -> flywheelVelocity), - robot.getHopper().feedPulsing())); + operator.rightTrigger().whileTrue(robot.getHopper().feedPulsing()); // Pass fuel to alliance zone operator.back().whileTrue(Commands.print("Pass Left")); // this might be switched with start @@ -246,6 +271,7 @@ public void configureBindings() { .and(driver.leftStick().negate()) .and(operator.leftBumper().negate()) .and(operator.rightTrigger().negate()) + .and(driver.a().negate()) .and(() -> !robot.getHopper().getSensors().isKickerFull()) .and(() -> !robot.getHopper().isEmpty()); @@ -254,11 +280,70 @@ public void configureBindings() { // Climb retraction Command autodescend = robot.getClimb().descend(); Trigger climbRetract = - new Trigger(() -> TeleopSwerveCommand.isClearToOverride(robot.getClimb(), autodescend)) + new Trigger(() -> CommandUtil.isClearToOverride(robot.getClimb(), autodescend)) .and(RobotState::isTeleop) .and(RobotState::isEnabled); climbRetract.onTrue(robot.getClimb().descend()); + + AutoShoot autoShoot = + new AutoShoot( + robot.getSwerveDrive(), + robot.getTurret(), + robot.getShooterHood(), + robot.getShooterRollers(), + robot.getShooterFunctions(), + () -> AutoShoot.HUB_TRANSLATION, + () -> tunableHoodAngle == 0 ? null : Degrees.of(tunableHoodAngle), + () -> tunableRollerVelocity == 0 ? null : RotationsPerSecond.of(tunableRollerVelocity)); + + Trigger autoshootTrigger = + new Trigger(RobotState::isTeleop).and(RobotState::isEnabled).and(() -> !fineControl); + + autoshootTrigger.whileTrue(autoShoot); + + operator + .leftStick() + .and(autoShoot.isReadyToShoot()) + .whileTrue(robot.getHopper().feedSynchronized()); + + ShooterFunctions functions = robot.getShooterFunctions(); + + driver + .a() + .whileTrue( + LoggingUtil.logCommand( + "TestShoot/BaseCommand", + Commands.parallel( + LoggingUtil.logCommand( + "TestShoot/MoveHood", + robot + .getShooterRollers() + .shoot(() -> functions.getFlywheelVelocity(shootingTestDistance))), + LoggingUtil.logCommand( + "TestShoot/SpinRollers", + robot + .getShooterHood() + .moveTo(() -> functions.getHoodAngle(shootingTestDistance))), + Commands.sequence( + LoggingUtil.logCommand( + "TestShoot/Wait", + Commands.waitUntil( + () -> + robot + .getShooterRollers() + .getAngularVelocity() + .isNear( + functions.getFlywheelVelocity(shootingTestDistance), + RotationsPerSecond.of(1)) + && robot + .getShooterHood() + .getPosition() + .isNear( + functions.getHoodAngle(shootingTestDistance), + Degrees.of(1)))), + LoggingUtil.logCommand( + "TestShoot/Feed", robot.getHopper().feedPulsing()))))); } private Command rumble( diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index 7f9931c6..b65084b5 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -7,12 +7,12 @@ import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANdi; import com.ctre.phoenix6.hardware.TalonFX; -import com.team6962.lib.logging.LoggingUtil; import dev.doglog.DogLog; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; @@ -44,7 +44,7 @@ public class Climb extends SubsystemBase { private boolean isClimbed; public Climb() { - motor = new TalonFX(ClimbConstants.MOTOR_ID, ClimbConstants.CANBUS_NAME); + motor = new TalonFX(ClimbConstants.MOTOR_ID, new CANBus(ClimbConstants.MOTOR_CANBUS_NAME)); if (RobotBase.isSimulation()) { ClimbConstants.MOTOR_CONFIGURATION.Slot0.kG = @@ -72,7 +72,7 @@ public Climb() { motor.getConfigurator().apply(ClimbConstants.MOTOR_CONFIGURATION); - candi = new CANdi(ClimbConstants.CANDI_CAN_ID, ClimbConstants.CANBUS_NAME); + candi = new CANdi(ClimbConstants.CANDI_CAN_ID, new CANBus(ClimbConstants.CANDI_CANBUS_NAME)); candi.getConfigurator().apply(ClimbConstants.CANDI_CONFIGURATION); accelerationSignal = motor.getAcceleration(); @@ -203,9 +203,10 @@ public void periodic() { voltageSignal, statorCurrentSignal, supplyCurrentSignal, - hallEffectSensorSignal, profilePositionSignal); + hallEffectSensorSignal.refresh(); + DogLog.log("Climb/Acceleration", getAcceleration()); DogLog.log("Climb/Velocity", getVelocity()); DogLog.log("Climb/Position", getPosition()); @@ -214,7 +215,6 @@ public void periodic() { DogLog.log("Climb/SupplyCurrent", getSupplyCurrent()); DogLog.log("Climb/HallSensorTriggered", isHallEffectSensorTriggered()); DogLog.log("Climb/ProfilePosition", profilePositionSignal.getValue()); - LoggingUtil.log("Climb/ControlRequest", motor.getAppliedControl()); if (RobotState.isDisabled()) { motor.setControl(new MotionMagicVoltage(getPosition().in(Meters))); diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java index 2fb8808d..eb436b03 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java @@ -27,7 +27,8 @@ public class ClimbConstants { public static final Distance DRUM_RADIUS = Inches.of(0.375); public static final int CANDI_CAN_ID = 40; public static final int MOTOR_ID = 50; - public static final String CANBUS_NAME = "drivetrain"; + public static final String MOTOR_CANBUS_NAME = "drivetrain"; + public static final String CANDI_CANBUS_NAME = "subsystems"; public static final Distance POSITION_TOLERANCE = Inches.of(0.125); public static final Distance MIN_HEIGHT = Inches.of(0); diff --git a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java index c2778a67..1adae79c 100644 --- a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java +++ b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.BaseStatusSignal; @@ -11,12 +12,14 @@ import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANdi; import com.ctre.phoenix6.hardware.TalonFX; import com.team6962.lib.logging.LoggingUtil; import com.team6962.lib.math.MeasureUtil; import dev.doglog.DogLog; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; @@ -24,6 +27,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; @@ -59,6 +63,19 @@ public ShooterHood() { new TalonFX(ShooterHoodConstants.MOTOR_CAN_ID, new CANBus(ShooterHoodConstants.CANBUS)); candi = new CANdi(ShooterHoodConstants.CANDI_CAN_ID, new CANBus(ShooterHoodConstants.CANBUS)); + if (RobotBase.isSimulation()) { + ShooterHoodConstants.MOTOR_CONFIGURATION.Slot0.kP = 80.0; + ShooterHoodConstants.MOTOR_CONFIGURATION.Slot0.kD = 3.0; + ShooterHoodConstants.MOTOR_CONFIGURATION.Slot0.kV = 0.0; // 4.3; + ShooterHoodConstants.MOTOR_CONFIGURATION.Slot0.kA = 0.0; + ShooterHoodConstants.kG = 0.22; + kG = ShooterHoodConstants.kG; + ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity = + 1000.0; // 30.0; + ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicAcceleration = + 1000.0; // 30.0; + } + hoodMotor.getConfigurator().apply(ShooterHoodConstants.MOTOR_CONFIGURATION); candi.getConfigurator().apply(ShooterHoodConstants.CANDI_CONFIGURATION); @@ -380,6 +397,91 @@ private void setPositionControl(Angle position) { } } + private void setPositionVelocityControl(Angle position, AngularVelocity velocity) { + if (isZeroed) { + hoodMotor.setControl( + new PositionVoltage(position) + .withVelocity(velocity) + .withFeedForward(Math.cos(getPosition().in(Radians)) * kG)); + } else { + hoodMotor.setControl(new NeutralOut()); + } + } + + /** + * Creates a Command that moves the hood to a target angle provided by the given supplier, while + * attempting to maintain the target velocity, which should be the velocity of the target angle. + * This is a done using a combination of basic PID and feedforward, with motion profiling used + * when error becomes large, ensuring fast movement when the hood reaches its limits and needs to + * rotate 360 degrees to reach the target angle. + * + * @param targetAngleSupplier supplier that is sampled repeatedly to provide the desired target + * angle + * @param targetVelocitySupplier supplier that is sampled repeatedly to provide the desired target + * velocity, which should be the velocity of the target angle; used for feedforward and can + * help improve tracking performance when the target angle is changing rapidly + * @return a Command that, while scheduled, continuously moves the hood toward the supplied target + * angle while attempting to maintain the supplied target velocity + */ + 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; + + @Override + public void initialize() { + previousProfileState = + new TrapezoidProfile.State( + getPosition().in(Rotations), getVelocity().in(RotationsPerSecond)); + previousUpdateTimestamp = Timer.getFPGATimestamp(); + } + + @Override + public void execute() { + Angle targetPosition = clampPositionToSafeRange(targetAngleSupplier.get()); + + AngularVelocity targetVelocity = targetVelocitySupplier.get(); + + TrapezoidProfile.State profileState = + profile.calculate( + Timer.getFPGATimestamp() - previousUpdateTimestamp, + previousProfileState, + new TrapezoidProfile.State( + targetPosition.in(Rotations), + 0 // This can also be set to targetVelocity.in(RotationsPerSecond) + // if the profile should go to the target position and velocity. However, 0 + // seems to perform better in simulation + )); + + previousProfileState = profileState; + previousUpdateTimestamp = Timer.getFPGATimestamp(); + + if (targetPosition.isNear(getPosition(), Degrees.of(3))) { + setPositionVelocityControl(targetPosition, targetVelocity); + } else { + setPositionVelocityControl( + Rotations.of(profileState.position), + RotationsPerSecond.of(profileState.velocity)); + } + } + }; + + command.addRequirements(this); + + return command; + } + /** * 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/hood/ShooterHoodConstants.java b/src/main/java/frc/robot/subsystems/hood/ShooterHoodConstants.java index 2325d7b0..057d19e7 100644 --- a/src/main/java/frc/robot/subsystems/hood/ShooterHoodConstants.java +++ b/src/main/java/frc/robot/subsystems/hood/ShooterHoodConstants.java @@ -38,7 +38,7 @@ public final class ShooterHoodConstants { public static final Distance ARM_LENGTH = Inches.of(6.85); /** Gravity compensation feedforward constant (volts). */ - public static final double kG = 0.29; + public static double kG = 0.29; public static final Voltage FINE_CONTROL_VOLTAGE = Volts.of(0.5); diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index f4a1d867..35ecddaf 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.Seconds; import dev.doglog.DogLog; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -71,11 +72,14 @@ public Command feedSynchronized() { Commands.either( feedSynchronizedWithoutUnjam().withTimeout(Seconds.of(0.5)), unjamDuringSynchronizedFeed(), - () -> sensors.isFeedingSuccessfully() || sensors.isKickerEmpty()) + () -> + sensors.isFeedingSuccessfully() + || sensors.isKickerEmpty() + || RobotBase.isSimulation()) .repeatedly()); } - private Command feedSynchronizedWithoutUnjam() { + public Command feedSynchronizedWithoutUnjam() { return Commands.parallel(beltFloor.feed(), kicker.feed()) .deadlineFor( Commands.startEnd( diff --git a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollers.java b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollers.java index 92f0c567..81fe84b1 100644 --- a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollers.java +++ b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollers.java @@ -101,11 +101,12 @@ public Command shoot(Supplier targetVelocity) { if (getAngularVelocity() .plus(ShooterRollersConstants.BANG_BANG_TOLERANCE) .lt(targetVelocity.get())) { - shooterRollerMotor1.setControl(new DutyCycleOut(1)); + shooterRollerMotor1.setControl(new DutyCycleOut(1).withEnableFOC(false)); } else { // defines a local function to set motor voltage to make it go shooterRollerMotor1.setControl( - new VelocityVoltage(targetVelocity.get().in(RotationsPerSecond))); + new VelocityVoltage(targetVelocity.get().in(RotationsPerSecond)) + .withEnableFOC(false)); } }, () -> { diff --git a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersConstants.java b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersConstants.java index 69f4df52..6d82cc11 100644 --- a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersConstants.java +++ b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersConstants.java @@ -30,7 +30,7 @@ public final class ShooterRollersConstants { .withFeedback(new FeedbackConfigs()) .withSlot0( new Slot0Configs() - .withKV(0.124137931) + .withKV(0.12) .withKS(RobotBase.isSimulation() ? 0 : 0.423) .withKP(0.1)) // kS should not exist when in simulation .withCurrentLimits( diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 2411dad8..ce65364b 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -4,6 +4,7 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; @@ -12,6 +13,7 @@ import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANdi; import com.ctre.phoenix6.hardware.TalonFX; @@ -20,6 +22,7 @@ import com.team6962.lib.math.AngleMath; import com.team6962.lib.math.MeasureUtil; import dev.doglog.DogLog; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; @@ -27,6 +30,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -462,6 +466,82 @@ public Command moveTo(Angle targetAngle) { .onlyIf(this::isZeroed); } + /** + * Creates a Command that moves the turret to a target angle provided by the given supplier, while + * attempting to maintain the target velocity, which should be the velocity of the target angle. + * This is a done using a combination of basic PID and feedforward, with motion profiling used + * when error becomes large, ensuring fast movement when the turret reaches its limits and needs + * to rotate 360 degrees to reach the target angle. + * + * @param targetAngleSupplier supplier that is sampled repeatedly to provide the desired target + * angle + * @param targetVelocitySupplier supplier that is sampled repeatedly to provide the desired target + * velocity, which should be the velocity of the target angle; used for feedforward and can + * help improve tracking performance when the target angle is changing rapidly + * @return a Command that, while scheduled, continuously moves the turret toward the supplied + * target angle while attempting to maintain the supplied target velocity + */ + 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; + + @Override + public void initialize() { + previousProfileState = + new TrapezoidProfile.State( + getPosition().in(Rotations), getVelocity().in(RotationsPerSecond)); + previousUpdateTimestamp = Timer.getFPGATimestamp(); + } + + @Override + public void execute() { + Angle targetPosition = + clampPositionToSafeRange( + optimizeTarget( + targetAngleSupplier.get(), + getPosition(), + TurretConstants.MIN_ANGLE, + TurretConstants.MAX_ANGLE)); + + AngularVelocity targetVelocity = targetVelocitySupplier.get(); + + TrapezoidProfile.State profileState = + profile.calculate( + Timer.getFPGATimestamp() - previousUpdateTimestamp, + previousProfileState, + new TrapezoidProfile.State( + targetPosition.in(Rotations), + 0 // This can also be set to targetVelocity.in(RotationsPerSecond) + // if the profile should go to the target position and velocity. However, 0 + // seems to perform better in simulation + )); + + previousProfileState = profileState; + previousUpdateTimestamp = Timer.getFPGATimestamp(); + + if (targetPosition.isNear(getPosition(), Degrees.of(5))) { + setPositionVelocityControl(targetPosition, targetVelocity); + } else { + setPositionVelocityControl( + Rotations.of(profileState.position), + RotationsPerSecond.of(profileState.velocity)); + } + } + }; + + command.addRequirements(this); + + return command; + } + /** * Creates a command that continuously drives the turret to a target angle provided by the given * supplier. @@ -510,6 +590,16 @@ private void setPositionControl(Angle targetAngle) { } } + private void setPositionVelocityControl(Angle targetAngle, AngularVelocity targetVelocity) { + if (isZeroed()) { + motor.setControl(new PositionVoltage(targetAngle).withVelocity(targetVelocity)); + } else if (RobotState.isEnabled()) { + motor.setControl(new NeutralOut()); + } else { + motor.setControl(new CoastOut()); + } + } + public Command moveAtVoltage(Voltage voltage) { return startEnd( () -> { diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index d6784925..c194be23 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -98,7 +98,5 @@ public class TurretConstants { MOMENT_OF_INERTIA * SIMULATED_MOTOR.nominalVoltageVolts / SIMULATED_MOTOR.stallTorqueNewtonMeters - / SENSOR_TO_MECHANISM_RATIO - * 2 - * Math.PI; + / SENSOR_TO_MECHANISM_RATIO; } diff --git a/src/main/java/frc/robot/subsystems/visualizer/RobotVisualizer.java b/src/main/java/frc/robot/subsystems/visualizer/RobotVisualizer.java index e6d41913..45c1d707 100644 --- a/src/main/java/frc/robot/subsystems/visualizer/RobotVisualizer.java +++ b/src/main/java/frc/robot/subsystems/visualizer/RobotVisualizer.java @@ -2,9 +2,11 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.Meters; 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.Volts; import com.team6962.lib.swerve.simulation.MapleSim; @@ -30,6 +32,7 @@ public class RobotVisualizer extends SubsystemBase { private RobotContainer robot; private IntakeSimulation intakeSim; + private int tickIndex = 0; /** * Creates a new RobotVisualizer. @@ -74,6 +77,8 @@ private Translation3d getFuelTranslation(int index) { @Override public void simulationPeriodic() { + tickIndex++; + if (RobotBase.isSimulation()) { try { robot.getSwerveDrive().getSimulation().getArenaLock().lock(); @@ -115,6 +120,35 @@ public void simulationPeriodic() { fuelCount = RobotVisualizationConstants.maxRetractedFuel; } + if (robot.getHopper().getKicker().getAppliedVoltage().in(Volts) > 1.0 + && robot.getShooterRollers().getMotorVoltage().in(Volts) > 1.0 + && fuelCount > 0 + && tickIndex % 5 == 0) { + fuelCount = Math.max(0, fuelCount - 1); + intakeSim.setGamePiecesCount(fuelCount); + + robot + .getSwerveDrive() + .getSimulation() + .getMapleSim() + .getArena() + .addGamePieceProjectile( + new RebuiltFuelOnFly( + robot.getSwerveDrive().getPosition2d().getTranslation(), + RobotVisualizationConstants.shooterTranslation + .toTranslation2d() + .rotateBy(new Rotation2d(robot.getTurret().getPosition().unaryMinus())), + robot.getSwerveDrive().getVelocity(), + new Rotation2d( + robot.getTurret().getPosition().plus(robot.getSwerveDrive().getYaw())), + Meters.of(RobotVisualizationConstants.shooterTranslation.getZ()) + .plus(Inches.of(6)), + InchesPerSecond.of(2) + .times( + robot.getShooterRollers().getAngularVelocity().in(RadiansPerSecond)), + Degrees.of(90).minus(robot.getShooterHood().getPosition()))); + } + List fuelPositions = robot .getSwerveDrive()