diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1ff153f..067728f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -144,9 +144,11 @@ public final class Constants { public static final double HOMING_SPEED = .05; public static final double TURRET_GEAR_RATIO = 6; public static final double TURRET_RANGE_ROTATIONS = 0.5; // The turret can rotate 180° + public static final double SHOOTER_HEIGHT_INCHES = 22.0; // Shooter is 22" from the floor + public static final double HUB_OPENING_HEIGHT_INCHES = 72.0; // Hub opening (hexagon top) is 72" from the floor public static final int HOOD_SERVO_CHANNEL = 0; - public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this + public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this public static final int HOOD_ENCODER_ID = 4; public static final int HOOD_ANGLE_MIN_DEG = 52; // Lowest possible position public static final int HOOD_ANGLE_MAX_DEG = 70; // Highest possible position diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 13f0865..e232cde 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -90,7 +90,7 @@ public class RobotContainer { // m_driverController.a().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem))); // m_driverController.x().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem))); //m_driverController.x().toggleOnTrue(getAutonomousCommand())((new UltraShooterCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem))); - m_driverController.x().toggleOnTrue(new UltraShooterCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)); + m_driverController.x().toggleOnTrue(new UltraShooterCommand(hopperSubsystem, turretSubsystem, driveSubsystem)); m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem, m_driverController.getHID())); m_driverController.y().onTrue(intakeSubsystem.deploy()); } diff --git a/src/main/java/frc/robot/Utils.java b/src/main/java/frc/robot/Utils.java index d249a61..6639f5f 100644 --- a/src/main/java/frc/robot/Utils.java +++ b/src/main/java/frc/robot/Utils.java @@ -216,6 +216,30 @@ public class Utils { return launchSpeedFPS; } + /** + * Estimates the fuel's time of flight until it his the target, + * where the target is the top of the hub when scoring and the floor when passing + * launch speed in feet/second. + * This performs Equations 3-7 and 3-8 in the AutoShooter document. + * @param scoring true if the bot is scoring into the hub (false means passing) + * @param launchAngleDeg the target launch angle in degrees + * @param launchSpeedFPS the target launch speed in ft/s + * @return the approxmate fuel flight time in seconds + */ + public static double findTimeOfFlightScoring(boolean scoring, int launchAngleDeg, double launchSpeedFPS){ + double velocityVertFPS = launchSpeedFPS * Math.sin(Math.toRadians((double)launchAngleDeg)); // The vertical component of fuel's velocity + double kGravityInFeet = 32.2; // Gravity constant in ft/s^2 + double deltaHFeet = 0; // Heigh difference between launcher and target + if (scoring) { + deltaHFeet = (Constants.ShooterConstants.SHOOTER_HEIGHT_INCHES - Constants.ShooterConstants.HUB_OPENING_HEIGHT_INCHES) / 12; + } else { + deltaHFeet = Constants.ShooterConstants.SHOOTER_HEIGHT_INCHES / 12; + } + + return ((velocityVertFPS + Math.sqrt(Math.pow(velocityVertFPS, 2) - (2 * kGravityInFeet * deltaHFeet))) / kGravityInFeet); + + } + /** * Calculates the flywheel RPM that's required to achieve the provided * launch speed in feet/second. diff --git a/src/main/java/frc/robot/commands/UltraShooterCommand.java b/src/main/java/frc/robot/commands/UltraShooterCommand.java index 82b2f2a..30affe7 100644 --- a/src/main/java/frc/robot/commands/UltraShooterCommand.java +++ b/src/main/java/frc/robot/commands/UltraShooterCommand.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; @@ -21,7 +22,6 @@ import frc.robot.Constants.ShootingRegion; import frc.robot.Utils; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.TurretSubsystem; @@ -29,21 +29,19 @@ public class UltraShooterCommand extends Command{ private HopperSubsystem hopperSubsystem; private TurretSubsystem turretSubsystem; - private IntakeSubsystem intakeSubsystem; // TODO: is this necessary? private DriveSubsystem driveSubsystem; private Command waitCommand = Commands.waitSeconds(1.5); private final Translation2d CENTER_TURRET_TO_ROBOT = new Translation2d(-0.1335024, -0.1824736); // Defines physical offset of turret from robot center - private Alliance alliance; // Shooting behavior is based on the robot's alliance + private Alliance alliance; // Shooting behavior is based on the robot's alliance - public UltraShooterCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem, DriveSubsystem driveSubsystem){ + public UltraShooterCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem){ this.hopperSubsystem = hopperSubsystem; this.turretSubsystem = turretSubsystem; - this.intakeSubsystem = intakeSubsystem; this.driveSubsystem = driveSubsystem; - addRequirements(hopperSubsystem, intakeSubsystem); // TODO: isintakeSubsystem necessary? + addRequirements(hopperSubsystem); } @Override @@ -59,7 +57,7 @@ public class UltraShooterCommand extends Command{ public void execute() { Boolean okToShoot = false; // Tracks whether conditions are OK for shooting. Boolean scoring = false; // Are we shooting into the hub? - Pose3d botPose = driveSubsystem.getBotPose(); // Find the pose once since we'll use it a few times + Pose3d botPose = driveSubsystem.getBotPose(); // Get and store the bot's pose once since we'll use it a few times // STEPS 1 & 2: Get the field location of the initial target // First, find the robot's current field location. @@ -111,12 +109,45 @@ public class UltraShooterCommand extends Command{ } // STEP 5: Find final target based on the intial target and the robot's velocity. - // THIS IS NOT IMPLEMENTED - SO ASSUME STATIONARY ROBOT. Make final target the same as initial. + // VELOCITY ADJUSTMENTS ARE COMMENTED OUT, SO ASSUME STATIONARY ROBOT. Make final target the same as initial. Pose3d finalTargetPose = initialTargetPose; double distanceToFinalTargetMeters = distanceToInitialTargetMeters; Logger.recordOutput("UltraShooter-FinalTarget", finalTargetPose); Logger.recordOutput("UltraShooter-DistanceToFinalTarget(Meters)", distanceToFinalTargetMeters); + /* + // VELOCITY ADJUSTEMENTS - READY TO TRY. Uncomment this block. + // Estimate the fuel's time of flight. + double timeOfFlightSeconds = Utils.findTimeOfFlightScoring(scoring, initialHoodAngleDegrees, intitialLaunchSpeedFPS); + // TBA: Get the bot's current velocity + // SwerveDriveSystem in RoboLib must make its ChassisSpeeds accessible. + // Get the robot-relative ChassisSpeeds and robot heading from the swerve system. + // Then create field-relative ChassisSpeeds using the robot heading. + + ChassisSpeeds robotBotRelativeSpeeds = driveSubsystem.driveSystem.GetRobotRelativeSpeeds(); + Rotation2d botHeading = driveSubsystem.driveSystem.GetRobotHeading(); // TODO: adjust based on alliance since this comes straight from the Gyro? + ChassisSpeeds robotFieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(robotBotRelativeSpeeds, botHeading); + + // Get X and Y velocity values. + double botXVelMPS = robotFieldRelativeSpeeds.vxMetersPerSecond(); + double botYVelMPS = robotFieldRelativeSpeeds.vyMetersPerSecond(); + + // Get translaion of initialTarget. + Translation3d initialTargetTr = initialTargetPose.getTranslation(); + + // Find X and Y displacements during flight. + double targetXDisplacementMeters = botXVelMPS * timeOfFlightSeconds; + double targetYDisplacementMeters = botYVelMPS * timeOfFlightSeconds; + + // Find final target translation. + Translation3d adjustedTarget = new Translation3d(initialTargetTr.getX() - targetXDisplacementMeters, + initialTargetTr.getY() - targetYDisplacementMeters, + initialTargetTr.getZ()); // We don’t need to adjust z. + + // TBA: Generate a Pose3d for the finalTarget + finalTargetPose = new Pose3d(adjustedTarget, initialTargetPose.getRotation()); + */ + // STEP 6: Find hood angle and launch speed for the final target. // THIS IS NOT IMPLEMENTED - SO ASSUME STATIONARY ROBOT. Make final target the same as initial. double finalLaunchSpeedFPS = intitialLaunchSpeedFPS; @@ -195,13 +226,16 @@ public class UltraShooterCommand extends Command{ Logger.recordOutput("UltraShooter-State", "INACTIVE"); - DriverStation.getAlliance().ifPresent((aliance) -> { - if (aliance == Alliance.Blue) { + // Removed the following since we're don't setting turret targets. + /* + DriverStation.getAlliance().ifPresent((alliance) -> { + if (alliance == Alliance.Blue) { turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE); } else { turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE); } }); + */ // turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY // TODO: Deactivate rumble if we decided to use the rumble feature when the robot prevents shooting diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 4ed9e6f..582ca6a 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -112,12 +112,13 @@ public class TurretSubsystem extends TestableSubsystem { turretSim = turretMotor.getSimState(); // Launcher configuration + // Feedforward and PID values were determined experimentally on 10/4/2026. launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless); SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); config.voltageCompensation(10); config.closedLoop.p(0.0002).i(0.000001).d(0.0005); // I = 0.0000001 - config.closedLoop.feedForward.kS(0.2).kV(0.000).kA(0.000); + config.closedLoop.feedForward.kS(0.2).kV(0.00005).kA(0.001); // config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); config.closedLoop.positionWrappingEnabled(false); @@ -195,7 +196,7 @@ public class TurretSubsystem extends TestableSubsystem { pos = -1.0; } - Logger.recordOutput("ShooterHood-Angle", pos); + Logger.recordOutput("ShooterHood-ServoPos", pos); }