From 9e8c2f3a6a77290b4ca7b5e5967481aedd360094 Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Mon, 23 Feb 2026 19:08:09 -0800 Subject: [PATCH] implement superstructure --- .../robot/commands/gpm/AutoShootCommand.java | 2 +- .../robot/commands/gpm/Superstructure.java | 229 ++++++++++++++++++ .../java/frc/robot/util/PhaseManager.java | 10 +- 3 files changed, 239 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/gpm/Superstructure.java diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java index 2505958..e12d054 100644 --- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java +++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java @@ -106,7 +106,7 @@ public class AutoShootCommand extends Command { */ 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(), FieldConstants.getHubTranslation().getZ()); // Add if statement so that it's only when it's shooting + Translation3d target3d = new Translation3d(target.getX(), target.getY(), FieldConstants.getHubTranslation().getZ()); goalState = ShooterPhysics.getShotParams( target3d.minus(lookahead3d), 2.0); diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java new file mode 100644 index 0000000..2a7ca77 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -0,0 +1,229 @@ +package frc.robot.commands.gpm; + +import org.littletonrobotics.junction.Logger; + +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.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.constants.ShotInterpolation; +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.Constraints; +import frc.robot.util.ShooterPhysics.TurretState; + +public class Superstructure extends Command { + private Turret turret; + private Drivetrain drivetrain; + private Hood hood; + private Shooter shooter; + private Spindexer spindexer; + + //TODO: find maximum interpolation + private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767, HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); + + 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 final double phaseDelay = 0.03; // Extrapolation delay due to latency + + private Translation2d target = null; + + private PhaseManager phaseManager = new PhaseManager(); + + public Superstructure(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( + FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())), + 8.0); // Random initial goalState to prevent it being null + + addRequirements(turret); + } + + public void updateSetpoints(Pose2d drivepose) { + Pose2d turretPosition = drivepose.transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(),TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d())); + + // 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 + + fieldRelVel.omegaRadiansPerSecond + * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.cos(drivepose.getRotation().getRadians()) + - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.sin(drivepose.getRotation().getRadians())); + double turretVelocityY = + fieldRelVel.vyMetersPerSecond + + fieldRelVel.omegaRadiansPerSecond + * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.cos(drivepose.getRotation().getRadians()) + - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.sin(drivepose.getRotation().getRadians())); + + double timeOfFlight; + Pose2d lookaheadPose = turretPosition; + + /* + * Loop (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 + */ + 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(), + target == FieldConstants.getHubTranslation().toTranslation2d() ? + FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub + + goalState = ShooterPhysics.getShotParams( + target3d.minus(lookahead3d), + 2.0); + + timeOfFlight = goalState.timeOfFlight(); + double offsetX = turretVelocityX * timeOfFlight; + double offsetY = turretVelocityY * timeOfFlight; + lookaheadPose = + new Pose2d( + turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), + turretPosition.getRotation()); + } + + // 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; + + Logger.recordOutput("Lookahead Pose", lookaheadPose); + + // 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; + + // Stay within +/- 200 -- if shortest path is past 200, we go long way around + double turretRange = TurretConstants.MAX_ANGLE - TurretConstants.MIN_ANGLE; + if (potentialSetpoint > turretRange/2) { + potentialSetpoint -= 360; + } else if (potentialSetpoint < -turretRange/2) { + 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; + + } + + 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.exp( + new Twist2d( + robotRelVel.vxMetersPerSecond * phaseDelay, + robotRelVel.vyMetersPerSecond * phaseDelay, + robotRelVel.omegaRadiansPerSecond * phaseDelay)); + } + + /** + * 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.stopSpindexer(); + } + + @Override + public void execute() { + // Phase manager stuff + phaseManager.update(drivepose, shooter, turret); + target = phaseManager.getTarget(); + + updateDrivePose(); + updateSetpoints(drivepose); + + if (phaseManager.isIdle()) { + stowEverything(); + } else { + turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2)); + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(hoodSetpoint), hoodVelocity); + shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); + + if (phaseManager.shouldFeed()) { + spindexer.maxSpindexer(); + } else { + spindexer.stopSpindexer(); + } + } + + SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint); + SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint); + SmartDashboard.putNumber("Shooter Calculate Velocity", goalState.exitVel()); + } + + @Override + public void end(boolean interrupted) { + stowEverything(); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/PhaseManager.java b/src/main/java/frc/robot/util/PhaseManager.java index 54250fb..b63e1e9 100644 --- a/src/main/java/frc/robot/util/PhaseManager.java +++ b/src/main/java/frc/robot/util/PhaseManager.java @@ -1,6 +1,7 @@ package frc.robot.util; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import frc.robot.constants.FieldConstants; import frc.robot.constants.FieldConstants.FieldZone; import frc.robot.subsystems.shooter.Shooter; @@ -76,6 +77,13 @@ public class PhaseManager { } public boolean shouldFeed() { - return !(currentState == CurrentState.STARTING_UP) && !(currentState == CurrentState.TURNING_AROUND); + // TODO: I'm gonna comment out starting up until i find a solution + return !(currentState == CurrentState.TURNING_AROUND); //&& !(currentState == CurrentState.STARTING_UP); + } + + public Translation2d getTarget() { + return wantedState == WantedState.SHOOTING ? + FieldConstants.getHubTranslation().toTranslation2d() + : FieldConstants.getOppositionTranslation(true).toTranslation2d(); } } \ No newline at end of file -- 2.39.5