diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e3eca60f..3211a41e 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 136adf4d..f4662b13 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 @@ -133,13 +131,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 = .4; // needs value from hardware - public static final double Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .4; // needs value from hardware - + 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(.15, 0, 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; @@ -160,4 +156,5 @@ public enum Mode { public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later public static final double MAX_VISION_DISTANCE_SIMULATION = 6; + } diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 8574fe51..07ade16c 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.wpilibj.DriverStation; import frc.robot.Robot; import frc.robot.utils.math.TurretCalculations; @@ -8,8 +12,6 @@ import org.littletonrobotics.junction.Logger; import frc.robot.RobotContainer; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -22,10 +24,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"; @@ -77,10 +78,11 @@ 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); @@ -113,6 +115,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 drivetrain @@ -134,7 +139,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); @@ -144,16 +149,16 @@ 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(BLUE_HUB_PROFILE, robotPose)); + useShotTargets(calculateTargetsFromPose(BLUE_HUB_PROFILE, robotPose, robotSpeeds)); } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) { - useShotTargets(calculateTargetsFromPose(RED_HUB_PROFILE, robotPose)); + useShotTargets(calculateTargetsFromPose(RED_HUB_PROFILE, robotPose, robotSpeeds)); } else { // Unknown color, do nothing... useShotTargets(FIXED_TARGETS); } } - case SHUTTLING -> useShotTargets(calculateTargetsFromPose(SHUTTLE_PROFILE, robotPose)); + case SHUTTLING -> useShotTargets(calculateTargetsFromPose(SHUTTLE_PROFILE, robotPose, robotSpeeds)); } } @@ -183,42 +188,92 @@ private void useShotTargets(ShotTargets shotTargets) { driverEnabled); } - private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose) { - double computedDistanceMeters = calculateDistanceMeters(robotPose, profile.targetPose); - double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile); - double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile); - double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile); + private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose, ChassisSpeeds robotSpeeds) { + Logger.recordOutput("equalizedTime",equalizeFlightTime(calculateDistanceMeters(robotPose, profile.targetPose), robotPose, profile.targetPose, robotSpeeds)); + Logger.recordOutput("nonEqualizedTime", calculateFlightTime(calculateDistanceMeters(robotPose, profile.targetPose))); + 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(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); } private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { - return robotPose.getTranslation().getDistance(targetPose.getTranslation()); + return robotPose.transformBy(Constants.TURRET_OFFSET).getTranslation().getDistance(targetPose.getTranslation()); } - private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) { - if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { - double distance = UnitConversion.METER_TO_FOOT * computedDistanceMeters; + private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile, boolean shootHub) { + if (shootHub) { return - 0.169 * distance * distance - - 1.73 * distance + 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; - return Math.floor( - 8.46 * distance * distance - - 237 * distance - - 1_380); + + + // 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) { + Logger.recordOutput("robot to target translation", target.getTranslation().minus(robotPose.getTranslation())); + Logger.recordOutput("robot to target angle", target.getTranslation().minus(robotPose.getTranslation()).getAngle()); + Logger.recordOutput("speed Approaching Hub", ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.getTranslation().minus(robotPose.getTranslation()).getAngle()).vxMetersPerSecond); + 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) { + 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(), DriverStation.getAlliance().get()== DriverStation.Alliance.Blue))); + return Math.floor(Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose, profile.targetPose, DriverStation.getAlliance().get()== DriverStation.Alliance.Blue))); } //Getters for all the subsystems to set posistion. @@ -282,7 +337,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/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index ef7becba..4fb69bae 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -124,6 +124,7 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig public void periodic() { //add vision pose here //addVisionMeasurement(new Pose2d(new Translation2d(16, 2), new Rotation2d())); + Logger.recordOutput("fieldVelocities",getFieldVelocity()); } @Override diff --git a/src/main/java/frc/robot/utils/math/LaunchCalculations.java b/src/main/java/frc/robot/utils/math/LaunchCalculations.java deleted file mode 100644 index 5ada137a..00000000 --- a/src/main/java/frc/robot/utils/math/LaunchCalculations.java +++ /dev/null @@ -1,77 +0,0 @@ -package frc.robot.utils.math; - -import frc.robot.constants.GameConstants; - -public class LaunchCalculations { - - /* - * The intention of this class is to provide the launch angle and initial velocity given - * the robot's distance from the hub. We will test this on the robot, and if the physics - * don't work out practically, we will use this class to give initial test values for the - * robot. We will run these test values to obtain 4 points experimentally. Then, we will - * use interpolation to generate a function that will then give the correct launch angle - * and velocity according to our current distance from the robot. - */ - - // The origin is defined as the bottom right corner of the blue alliance - - // All angles are in radians, all distances are in meters, all velocities in m/s - - public static double calculateShooterVelocity(double robotPosX, double robotPosY, double launchAngle, boolean isBlueAlliance) { - - // Target value -- what we're trying to find - double shooterVelocity; // Initial velocity of the shooter -- related to flywheel speed - - // calculates the position of the turret with respect to the origin using the robot center - // and the constant distance between the robot center and the turret. - double turretPosX = robotPosX + GameConstants.X_DISTANCE_BETWEEN_ROBOT_AND_TURRET; - double turretPosY = robotPosY + GameConstants.Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET; - - // Given values -- from Constants file - double hubHeight = GameConstants.HUB_HEIGHT; // height of the top of the hub - double shooterHeight = GameConstants.SHOOTER_HEIGHT; // height of shooter - double hubPosX; - double hubPosY; - - double deltaDistance; // distance between the robot and the hub - double deltaHeight; // height difference between shooter and hub - - 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; - } - - // Compute distance between hub and robot with Pythagorean Theorem - deltaDistance = Math.sqrt(Math.pow(hubPosY - turretPosY, 2) - + Math.pow(hubPosX - turretPosX, 2)); - - // Compute the change in height between the shooter and the hub - deltaHeight = hubHeight - shooterHeight; - - /* - * Compute the shooter velocity. It is a physics formula that computes the initial velocity given - * the launch angle, the distance between the robot and the hub, the height change, and - * the gravitational constant g. This formula has been tested in projectile motion simulators. - * It starts with kinematic equations, then you set up a system of two of the kinematic equationsm - * eliminate time, and solve for the initial velocity. - */ - shooterVelocity = Math.sqrt( - ( - (GameConstants.GRAVITY) * Math.pow(deltaDistance, 2) * Math.pow(1 / Math.cos(launchAngle), 2) - ) - / - 2 * ( - deltaDistance * Math.tan(launchAngle) - deltaHeight - ) - ); - - return shooterVelocity; - - } - -} diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index 7e88c7b0..b493afe6 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 frc.robot.constants.GameConstants; public class TurretCalculations { @@ -26,31 +30,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; + public static double calculateTurretAngle(Pose2d robotPos,Pose2d adjTargetPose, boolean isBlueAlliance) { - // 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); + Pose2d turretPos = robotPos.transformBy(GameConstants.TURRET_OFFSET); - // Calculate actual turret position on field - double turretPosX = robotPosX + rotatedOffsetX; - double turretPosY = robotPosY + rotatedOffsetY; - - double hubPosX; - double hubPosY; - - 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 @@ -60,7 +44,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 @@ -68,7 +53,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));