diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e3eca60..3211a41 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -142,7 +142,7 @@ public void robotPeriodic() { SmartDashboard.putBoolean("Hub Active?", hubActive()); if (Constants.currentMode == Constants.Mode.SIM) { Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose().get()); - Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getSimulationPose().get()); + Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getSimulationRawOdomPose()); } } } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 89565c7..f8a07da 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -1,8 +1,6 @@ package frc.robot.constants; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -36,7 +34,7 @@ public enum Mode { public static final boolean ENABLE_LOGGING = true; //Debugs - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; public static final boolean ARM_DEBUG = true; //Joystick @@ -141,13 +139,11 @@ public enum Mode { public static final double GRAVITY = 9.81; public static final double HUB_HEIGHT = 1.83; public static final double SHOOTER_HEIGHT = 0.5; - public static final double BLUE_HUB_X_POSITION = 4.6256; - public static final double BLUE_HUB_Y_POSITION = 4.0345; - public static final double RED_HUB_X_POSITION = 11.9154; - public static final double RED_HUB_Y_POSITION = 4.0345; - public static final double X_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .05; - public static final double Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .12; - + public static final Pose2d BLUE_HUB_POS = new Pose2d(4.6256, 4.0345, new Rotation2d()); + public static final Pose2d RED_HUB_POS = new Pose2d(11.9154, 4.0345, new Rotation2d()); + public static final Transform2d TURRET_OFFSET = new Transform2d(0.05, 0.12, new Rotation2d()); //TODO: needs value from hardware + public static final boolean ACCOUNT_FOR_ANGULAR_MOMENTUM = true; + public static final double DISTANCE_BETWEEN_ROBOT_AND_TURRET = TURRET_OFFSET.getTranslation().getNorm(); // Shift timings public static final int SHIFT_1_START = 130; public static final int SHIFT_2_START = 105; diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 074d616..2bc63a9 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -1,6 +1,10 @@ package frc.robot.subsystems; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Robot; @@ -27,12 +31,9 @@ public class ControllerSubsystem extends SubsystemBase { private static final double STOP_DELAY_SECONDS = 0.5; // Placeholder target poses until real field target values are finalized - private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION, - Constants.BLUE_HUB_Y_POSITION, Rotation2d.kZero); - private static final Pose2d RED_HUB_TARGET_POSE = new Pose2d(Constants.RED_HUB_X_POSITION, - Constants.RED_HUB_Y_POSITION, Rotation2d.kZero); + private static final Pose2d BLUE_HUB_TARGET_POSE = Constants.BLUE_HUB_POS; + private static final Pose2d RED_HUB_TARGET_POSE = Constants.RED_HUB_POS; private static final Pose2d SHUTTLE_TARGET_POSE = new Pose2d(1.0, 7.0, Rotation2d.kZero); - private static final String MANUAL_POSE_X_KEY = "controller/ManualPoseX"; private static final String MANUAL_POSE_Y_KEY = "controller/ManualPoseY"; private static final String MANUAL_POSE_R_KEY = "controller/ManualPoseRotation"; @@ -84,9 +85,10 @@ public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContai @Override public void periodic() { Pose2d robotPose = getRobotPose(); + ChassisSpeeds robotSpeeds = getRobotSpeeds(); ShootState currentState = getCurrentShootState(); updateStopDelayState(currentState); - updateTargets(currentState, robotPose); + updateTargets(currentState, robotPose, robotSpeeds); if (Constants.DEBUG) { SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString()); SmartDashboard.putNumber(DISTANCE_METERS_KEY, activeTargets.distanceMeters); @@ -119,6 +121,9 @@ private Pose2d getRobotPose() { } return drivebase.getPose(); } + private ChassisSpeeds getRobotSpeeds() { + return drivebase.getFieldVelocity(); + } private boolean shouldUseManualPose() { // This can be confusing in case you're on the robot and have disabled the @@ -141,7 +146,7 @@ private void updateStopDelayState(ShootState currentState) { } } - private void updateTargets(ShootState state, Pose2d robotPose) { + private void updateTargets(ShootState state, Pose2d robotPose, ChassisSpeeds robotSpeeds) { switch (state) { case STOPPED -> updateStoppedTargets(); case FIXED -> useShotTargets(FIXED_TARGETS); @@ -151,9 +156,9 @@ private void updateTargets(ShootState state, Pose2d robotPose) { // No color, do nothing... useShotTargets(FIXED_TARGETS); } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)) { - useShotTargets(calculateTargetsFromPose(state, BLUE_HUB_PROFILE, robotPosePredictionCalculation(BLUE_HUB_PROFILE.targetPose,robotPose))); + useShotTargets(calculateTargetsFromPose(state, BLUE_HUB_PROFILE, robotPose, robotSpeeds)); } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) { - useShotTargets(calculateTargetsFromPose(state, RED_HUB_PROFILE, robotPosePredictionCalculation(RED_HUB_PROFILE.targetPose,robotPose))); + useShotTargets(calculateTargetsFromPose(state, RED_HUB_PROFILE, robotPose, robotSpeeds)); } else { // Unknown color, do nothing... useShotTargets(FIXED_TARGETS); @@ -164,9 +169,9 @@ private void updateTargets(ShootState state, Pose2d robotPose) { if (Robot.allianceColor().isEmpty()) { useShotTargets(FIXED_TARGETS); } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)) { - useShotTargets(calculateTargetsFromPose(state,BLUE_SHUTTLE_PROFILE, robotPose)); + useShotTargets(calculateTargetsFromPose(state,BLUE_SHUTTLE_PROFILE, robotPose, robotSpeeds)); } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) { - useShotTargets(calculateTargetsFromPose(state, RED_SHUTTLE_PROFILE, robotPose)); + useShotTargets(calculateTargetsFromPose(state, RED_SHUTTLE_PROFILE, robotPose, robotSpeeds)); } else { useShotTargets(FIXED_TARGETS); } @@ -200,11 +205,17 @@ private void useShotTargets(ShotTargets shotTargets) { driverEnabled); } - private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile profile, Pose2d robotPose) { - double computedDistanceMeters = calculateDistanceMeters(state, robotPose, profile.targetPose); - double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile); - double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile); - double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile); + private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile profile, Pose2d robotPose, ChassisSpeeds robotSpeeds) { + Twist2d momentumAdjustment = getMomentumAdjustment(robotPose, Constants.ACCOUNT_FOR_ANGULAR_MOMENTUM, robotSpeeds, + equalizeFlightTime(calculateDistanceMeters(robotPose, profile.targetPose), robotPose, profile.targetPose, robotSpeeds)); + PoseControlProfile adjustedProfile = new PoseControlProfile(profile.targetPose, profile.defaultAnglerAngleDegrees, profile.defaultShooterVelocityRpm, profile.defaultTurretAngleDegrees); + adjustedProfile.targetPose = profile.targetPose.exp(momentumAdjustment); + Logger.recordOutput("adjustedAimPoint", adjustedProfile.targetPose); + double computedDistanceMeters = calculateDistanceMeters(state, robotPose, adjustedProfile.targetPose); + boolean shootHub = profile == BLUE_HUB_PROFILE || profile == RED_HUB_PROFILE; + double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, adjustedProfile, shootHub); + double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, adjustedProfile, shootHub); + double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, adjustedProfile); return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, true); } @@ -225,6 +236,9 @@ private double calculateDistanceMeters(ShootState state,Pose2d robotPose, Pose2d } } + private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { + return Math.max(Constants.MIN_HUB_DISTANCE, Math.min(Constants.MAX_HUB_DISTANCE, robotPose.getTranslation().getDistance(targetPose.getTranslation()))); + } private Pose2d robotPosePredictionCalculation(Pose2d targetPose, Pose2d robotPose) { double flightTime = calculateFlightTime(calculateDistanceMeters(ShootState.SHOOTING_HUB,robotPose,targetPose)); Pose2d robotPoseTransform = new Pose2d(robotPose.getTranslation(), new Rotation2d()); @@ -239,35 +253,73 @@ private Pose2d robotPosePredictionCalculation(Pose2d targetPose, Pose2d robotPos } return predictedPose; } - //Flight time derived from testing videos - private double calculateFlightTime(double computedDistanceMeters) { - return 0.208*computedDistanceMeters + 0.647; - } - private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) { - if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { - double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET; - return 0.169 * distance * distance - - 1.73 * distance - + 20.4; + private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile, boolean shootHub) { + computedDistanceMeters -= Constants.COMPUTATED_DISTANCE_OFFSET; + if (shootHub) { + return + 1.82 * computedDistanceMeters * computedDistanceMeters + - 5.68 * computedDistanceMeters + + 20.4; } return profile.defaultAnglerAngleDegrees; } - private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) { - if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { - double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET; - return (8.46 * distance * distance - - 237 * distance - - 1380); + + + // Twist2d is a change in pose (position and rotation) over time. + // the returned value is a field-relative displacement vector (change in position and rotation) over the ball flight time. + // this means how much we need to adjust our shot. + public Twist2d getMomentumAdjustment(Pose2d robotPose, boolean useAngularMomentumAdjustment, ChassisSpeeds robotSpeeds, double timeOfFlight) { + // calculate the change per second of the turret's position relative to the center due to robot rotation. This + // is basically angular speed. Result is in robot coordinate system. + ChassisSpeeds adjustSpeeds; + if (useAngularMomentumAdjustment) { + Translation2d angularVelocityAdjustment = Constants.TURRET_OFFSET.times(robotSpeeds.omegaRadiansPerSecond).getTranslation(); + + + // take the robot's current speed (relative to field) + // convert the turret's angular velocity from robot-relative to field-relative + // since the rotation velocity is perpendicular to the robot x-axis, so we need to rotate it by 90 degrees + // which is why we use -Y,X instead of X,Y for the conversion. + // add the two to get the effective speed of the turret/projectile. + adjustSpeeds = (new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, 0)).plus + (ChassisSpeeds.fromRobotRelativeSpeeds(-angularVelocityAdjustment.getY(), angularVelocityAdjustment.getX(), 0.0, robotPose.getRotation())); + } else { + adjustSpeeds = (new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, 0)); + } + // convert the effective speed to a twist (displacement over time) + // positive timeOfFlight tells us how much we move + // negative timeOfFlight tells us how much we need to move to compensate for our movement + return adjustSpeeds.toTwist2d(-timeOfFlight); + } + // Linear regression through 3 static shot distance vs time points. + private double calculateFlightTime(double computedDistanceMeters) { + return 0.208*computedDistanceMeters + 0.647; + } + + // Since t(x)=mx+b (previous function), we can solve for x + // t = m*(d - v_robot * t)+b + // t + v_robot * t = m * d + b + // t = (m * d + b) / (v_robot + 1) + private double equalizeFlightTime(double initialDistanceMeters, Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds) { + return (0.208*initialDistanceMeters+0.647)/(0.208*ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.getTranslation().minus(robotPose.getTranslation()).getAngle()).vxMetersPerSecond+1); + } + private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile, boolean shootHub) { + computedDistanceMeters -= Constants.COMPUTATED_DISTANCE_OFFSET; + if (shootHub) { + return + 91 * computedDistanceMeters * computedDistanceMeters + - 776 * computedDistanceMeters + - 1_380; } return profile.defaultShooterVelocityRpm; } + private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) { return Math.floor( - Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), - robotPose.getRotation().getRadians(), + Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose, profile.targetPose, Robot.allianceColor().get() == DriverStation.Alliance.Blue))); } @@ -332,7 +384,7 @@ private ShotTargets( // Class to save all the target poses and each subsystem position at that point // so we can calculate true values later on private static final class PoseControlProfile { - private final Pose2d targetPose; + private Pose2d targetPose; private final double defaultAnglerAngleDegrees; private final double defaultShooterVelocityRpm; private final double defaultTurretAngleDegrees; diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index 548d228..ab2cd4f 100644 --- a/src/main/java/frc/robot/utils/math/TurretCalculations.java +++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java @@ -1,5 +1,9 @@ package frc.robot.utils.math; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import org.littletonrobotics.junction.Logger; @@ -32,31 +36,11 @@ public class TurretCalculations { // all positions are in meters // robotRotation is in radians // Returns - turret angle in radians - public static double calculateTurretAngle(double robotPosX, double robotPosY, double robotRotation, boolean isBlueAlliance) { - - // Get turret offset from robot center - double turretOffsetX = GameConstants.X_DISTANCE_BETWEEN_ROBOT_AND_TURRET; - double turretOffsetY = GameConstants.Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET; - - // Rotate the offset by robot rotation (offset rotates with robot) - double rotatedOffsetX = turretOffsetX * Math.cos(robotRotation) - turretOffsetY * Math.sin(robotRotation); - double rotatedOffsetY = turretOffsetX * Math.sin(robotRotation) + turretOffsetY * Math.cos(robotRotation); - - // Calculate actual turret position on field - double turretPosX = robotPosX + rotatedOffsetX; - double turretPosY = robotPosY + rotatedOffsetY; + public static double calculateTurretAngle(Pose2d robotPos,Pose2d adjTargetPose, boolean isBlueAlliance) { - double hubPosX; - double hubPosY; + Pose2d turretPos = robotPos.transformBy(GameConstants.TURRET_OFFSET); - if (isBlueAlliance) { - // hub position determined by which alliance robot is on - hubPosX = GameConstants.BLUE_HUB_X_POSITION; - hubPosY = GameConstants.BLUE_HUB_Y_POSITION; - } else { - hubPosX = GameConstants.RED_HUB_X_POSITION; - hubPosY = GameConstants.RED_HUB_Y_POSITION; - } + Pose2d hubPos = adjTargetPose; /* * This finds the unadjusted pan angle (assuming there is no robot rotation) using @@ -66,7 +50,8 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do * of the input numbers. * */ - double panAngleUnadjusted = Math.atan2(hubPosY - turretPosY, hubPosX - turretPosX); + Translation2d hubRelativeToRobot = hubPos.relativeTo(turretPos).getTranslation(); + double panAngleUnadjusted = hubRelativeToRobot.getAngle().getRadians(); /* * Adjusts the pan angle to account for the robot's current rotation. We subtract the @@ -74,7 +59,7 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do * pan angle, which is the proper angle of the turret adjusted for the robot's rotation * and the fact that the turret 0 angle in in the back of the robot. */ - double panAngle = panAngleUnadjusted - (robotRotation + Math.PI); + double panAngle = panAngleUnadjusted - (robotPos.getRotation().getRadians() + Math.PI); // normalize angle between -PI and PI double normalizedPanAngle = panAngle - 2 * Math.PI * Math.floor((panAngle + Math.PI) / (2 * Math.PI));