--- /dev/null
+package frc.robot.commands.gpm;
+
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
+
+import edu.wpi.first.math.MathUtil;
+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.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.ShotInterpolation;
+import frc.robot.constants.ShuttleInterpolation;
+import frc.robot.controls.Operator;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.hood.HoodConstants;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.turret.Turret;
+import frc.robot.subsystems.turret.TurretConstants;
+import frc.robot.util.PhaseManager;
+import frc.robot.util.ShooterPhysics;
+import frc.robot.util.ShooterPhysics.TurretState;
+
+public class ManualShoot extends Command {
+ private Operator operator;
+ private Turret turret;
+ private Drivetrain drivetrain;
+ private Hood hood;
+ private Shooter shooter;
+ private Spindexer spindexer;
+
+ private double turretSetpoint;
+ private double hoodSetpoint;
+
+ private Pose2d drivepose;
+
+ private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
+ private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
+
+ private Rotation2d lastTurretAngle;
+ private Rotation2d turretAngle;
+ private double turretVelocity;
+
+ private double lastHoodAngle;
+ private double hoodAngle;
+ private double hoodVelocity;
+
+ private TurretState goalState;
+
+ private LoggedNetworkNumber phaseDelay = new LoggedNetworkNumber("/Tuning/OPERATOR/Phase Delay", 0.03); // Extrapolation
+ // delay due
+ // to latency
+
+ private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d();
+
+ private PhaseManager phaseManager = new PhaseManager();
+
+ private LoggedNetworkNumber hoodOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Hood Offset", 0.0);
+
+ private LoggedNetworkNumber turretOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Turret Offet", 0.0);
+
+ private double distanceFromTarget = 0.0;
+ private LoggedNetworkNumber shuttlingTOFMultiplier = new LoggedNetworkNumber(
+ "/Tuning/OPERATOR/Shuttling TOF Multiplier", 0.8);
+
+ // private double TOFAdjustment = 0.85;
+ // private double TOFAdjustment = 1.1;
+ private LoggedNetworkNumber TOFAdjustment = new LoggedNetworkNumber("/Tuning/OPERATOR/TOF Adjustment", 1.1);
+
+ public ManualShoot(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) {
+ this.turret = turret;
+ this.drivetrain = drivetrain;
+ this.hood = hood;
+ this.shooter = shooter;
+ this.spindexer = spindexer;
+ drivepose = drivetrain.getPose();
+
+ goalState = ShooterPhysics.getShotParams(
+ Translation2d.kZero,
+ FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
+ 8.0); // Random initial goalState to prevent it being null
+
+ SmartDashboard.putNumber("Manual Shooting Power Scale", 1.0);
+
+ addRequirements(turret, shooter);
+ }
+
+ public void updateSetpoints(Pose2d drivepose) {
+ Pose2d turretPosition = drivepose.transformBy(
+ new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero));
+
+ // If the robot is moving, adjust the target position based on velocity
+ ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
+ ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
+
+ // Rotational adjustment is not being used, since turret is in center of robot
+ double turretVelocityX = fieldRelVel.vxMetersPerSecond;
+ double turretVelocityY = fieldRelVel.vyMetersPerSecond;
+
+ double timeOfFlight = 0;
+ Pose2d lookaheadPose = turretPosition;
+
+ /*
+ * Loop (max 20) until lookaheadPose converges BECAUSE -->
+ * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate
+ * for 6m (t = 0.8s)
+ * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was
+ * derived using t = 1.0s
+ * So we make a bunch of guesses until it converges
+ * Early exit when change < 1mm to avoid unnecessary iterations
+ */
+ boolean shuttling = target.equals(FieldConstants.getHubTranslation().toTranslation2d());
+ for (int i = 0; i < 20; i++) {
+ Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(),
+ TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
+
+ // Translation3d target3d = new Translation3d(target.getX(), target.getY(),
+ // shuttling ?
+ // FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not
+ // the hub
+
+ Translation2d x = drivepose.getTranslation().plus(
+ new Translation2d(0, 1).rotateBy(operator.getLeftRotation())
+ ).times(operator.getLeftTrigger() * SmartDashboard.getNumber("Manual Shooting Power Scale", 1.0));
+
+ Translation3d target3d = new Translation3d(x);
+
+ goalState = ShooterPhysics.getShotParams(
+ Translation2d.kZero,
+ target3d.minus(lookahead3d),
+ 2.0);
+
+ if (!shuttling) {
+ timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get();
+ } else {
+ double distance = target.getDistance(lookaheadPose.getTranslation());
+ timeOfFlight = distance * shuttlingTOFMultiplier.get();
+ }
+
+ double offsetX = turretVelocityX * timeOfFlight;
+ double offsetY = turretVelocityY * timeOfFlight;
+ Pose2d newLookaheadPose = new Pose2d(
+ turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)),
+ turretPosition.getRotation());
+
+ // early exit if converged (change < 1mm)
+ if (i > 0 && lookaheadPose.getTranslation().getDistance(newLookaheadPose.getTranslation()) < 0.001) {
+ lookaheadPose = newLookaheadPose;
+ break;
+ }
+ lookaheadPose = newLookaheadPose;
+ }
+
+ // Get the field angle relative to the target pose
+ turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();
+ if (lastTurretAngle == null) {
+ lastTurretAngle = turretAngle;
+ }
+
+ // Take the filtered average as the turret's velocity when robot is moving
+ // translationally
+ turretVelocity = turretAngleFilter.calculate(turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
+
+ lastTurretAngle = turretAngle;
+
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Turret/Target Pose", target);
+ Logger.recordOutput("Lookahead Pose", lookaheadPose);
+ }
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putNumber("Time of flight", timeOfFlight);
+ SmartDashboard.putNumber("Turret X-Velocity", turretVelocityX);
+ SmartDashboard.putNumber("Turret Y-Velocity", turretVelocityY);
+ }
+
+ // Subtract the rotational angle of the robot from the setpoint
+ double adjustedTurretSetpoint = MathUtil
+ .angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
+
+ // Shortest path
+ double error = MathUtil.inputModulus(
+ Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180);
+ double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset.get();
+
+ // Stay within physical limits -- if shortest path is past max angle, we go long
+ // way around
+ if (potentialSetpoint > TurretConstants.MAX_ANGLE) {
+ potentialSetpoint -= 360;
+ } else if (potentialSetpoint < TurretConstants.MIN_ANGLE) {
+ potentialSetpoint += 360;
+ }
+
+ turretSetpoint = potentialSetpoint;
+
+ // Pitch is in radians
+ hoodAngle = goalState.pitch();
+ hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
+ hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
+ lastHoodAngle = hoodAngle;
+
+ distanceFromTarget = target.getDistance(lookaheadPose.getTranslation());
+ Logger.recordOutput("Shooting/distanceToTarget", distanceFromTarget);
+ }
+
+ public void updateDrivePose() {
+ Pose2d currentPose = drivetrain.getPose();
+
+ drivepose = new Pose2d(
+ currentPose.getTranslation(),
+ // Uncomment this if robot is backwards
+ currentPose.getRotation()// .plus(new Rotation2d(Math.PI))
+ );
+ ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
+
+ // Add a phase delay extrapolation component for latency delay
+ drivepose = drivepose.exp(
+ new Twist2d(
+ robotRelVel.vxMetersPerSecond * phaseDelay.get(),
+ robotRelVel.vyMetersPerSecond * phaseDelay.get(),
+ robotRelVel.omegaRadiansPerSecond * phaseDelay.get()));
+ }
+
+ /**
+ * Stops and stows all subsystems involved in the command
+ */
+ public void stowEverything() {
+ turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
+ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
+ shooter.setShooter(0.0);
+ spindexer.noIndexing = true;
+ }
+
+ public void underLadder() {
+ spindexer.noIndexing = true;
+ }
+
+ // shoot higher
+ public void bumpUpHoodOffset() {
+ hoodOffset.set(hoodOffset.get() + 1.0); // 1 deg
+ }
+
+ // shoot lower
+ public void bumpDownHoodOffset() {
+ hoodOffset.set(hoodOffset.get() - 1.0); // 1 deg
+ }
+
+ // aim more left
+ public void bumpUpTurretOffset() {
+ turretOffset.set(turretOffset.get() + 2.5); // 2.5 deg
+ }
+
+ // aim more right
+ public void bumpDownTurretOffset() {
+ turretOffset.set(turretOffset.get() - 2.5); // 2.5 deg
+ }
+
+ @Override
+ public void execute() {
+ // Phase manager stuff
+ phaseManager.update(drivepose, shooter, turret);
+ target = phaseManager.getTarget(drivepose);
+
+ updateDrivePose();
+ updateSetpoints(drivepose);
+
+ if (phaseManager.isIdle()) {
+ underLadder();
+ } else {
+ if (spindexer.noIndexing) {
+ spindexer.noIndexing = false;
+ }
+ turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint),
+ turretVelocity - drivetrain.getAngularRate(2));
+
+ boolean shuttling = !target.equals(FieldConstants.getHubTranslation().toTranslation2d()); // if we're aiming at
+ // the hub, we're not
+ // shuttling
+
+ // shuttling will move the hood so its angles very close to max (less arch)
+ if (shuttling) {
+ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)),
+ hoodVelocity);
+ } else {
+ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)),
+ hoodVelocity);
+ }
+
+ // if (FieldConstants.underTrench(x, y)) {
+ // System.out.println("Hood forced down");
+ // } else {
+ // hood.forceHoodDown(false);
+ // }
+
+ // different maps for shuttling vs shooting. Less powerful when shuttling.
+ if (shuttling) {
+ shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget));
+ } else {
+ shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget));
+ }
+
+ if (!Constants.DISABLE_LOGGING) {
+ // record when shuttling
+ Logger.recordOutput("Shuttling", shuttling);
+ // record distance for tuning if needed
+ Logger.recordOutput("Distance From Target", distanceFromTarget);
+ }
+ }
+
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint);
+ Logger.recordOutput("Hood Calculate Setpoint", hoodSetpoint);
+ Logger.recordOutput("Shooter Calculate Velocity", goalState.exitVel());
+
+ Logger.recordOutput("DistanceToTarget", target.getDistance(drivepose.getTranslation()));
+ }
+
+ // for operator
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString());
+
+ } else {
+ phaseDelay.set(0.03);
+ }
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ stowEverything();
+ }
+
+}
package frc.robot.controls;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.commands.DoNothing;
+import frc.robot.commands.gpm.ManualShoot;
import frc.robot.constants.Constants;
import frc.robot.subsystems.drivetrain.Drivetrain;
-import lib.controllers.GameController;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.turret.Turret;
+import lib.controllers.PS5Controller;
+import lib.controllers.PS5Controller.PS5Axis;
+import lib.controllers.PS5Controller.PS5Button;
-/**
- * Controls for the operator, which are almost a duplicate of most of the driver's controls
+/**
+ * Controls for the operator, which are almost a duplicate of most of the
+ * driver's controls
*/
public class Operator {
- private final GameController driver = new GameController(Constants.OPERATOR_JOY);
-
- private final Drivetrain drive;
-
- public Operator(Drivetrain drive) {
- this.drive = drive;
- }
-
- public void configureControls() {
- // Cancel commands, could be removed if the operator doesn't need this button
- driver.get(driver.RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
- drive.setIsAlign(false);
- drive.setDesiredPose(() -> null);
- CommandScheduler.getInstance().cancelAll();
- }));
- }
-
-
- public Trigger getRightTrigger(){
- return new Trigger(driver.RIGHT_TRIGGER_BUTTON);
- }
- public Trigger getLeftTrigger(){
- return new Trigger(driver.LEFT_TRIGGER_BUTTON);
- }
- public GameController getGameController(){
- return driver;
- }
+ private final PS5Controller driver = new PS5Controller(Constants.OPERATOR_JOY);
+
+ private final Drivetrain drive;
+ private final Turret turret;
+ private final Hood hood;
+ private final Shooter shooter;
+ private final Spindexer spindexer;
+
+ public Operator(Drivetrain drive, Turret turret, Hood hood, Shooter shooter, Spindexer spindexer) {
+ this.drive = drive;
+ this.turret = turret;
+ this.hood = hood;
+ this.shooter = shooter;
+ this.spindexer = spindexer;
+ }
+
+ public void configureControls() {
+ // Cancel commands, could be removed if the operator doesn't need this button
+ // driver.get(driver.RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
+ // drive.setIsAlign(false);
+ // drive.setDesiredPose(() -> null);
+ // CommandScheduler.getInstance().cancelAll();
+ // }));
+
+ driver.get(PS5Button.CROSS).toggleOnTrue(new ManualShoot(turret, drive, hood, shooter, spindexer));
+ }
+
+ public double getLeftXAxis() {
+ return driver.get(PS5Axis.LEFT_X);
+ }
+
+ public double getLeftYAxis() {
+ return driver.get(PS5Axis.LEFT_Y);
+ }
+
+ public double getRightXAxis() {
+ return driver.get(PS5Axis.RIGHT_X);
+ }
+
+ public double getRightYAxis() {
+ return driver.get(PS5Axis.RIGHT_Y);
+ }
+
+ public Rotation2d getLeftRotation() {
+ return new Rotation2d(getLeftXAxis(), getLeftYAxis());
+ }
+
+ public double getRightTrigger() {
+ return driver.get(PS5Axis.RIGHT_TRIGGER);
+ }
+
+ public double getLeftTrigger() {
+ return driver.get(PS5Axis.LEFT_TRIGGER);
+ }
+
}