Package com.marswars.geometry

Class LaunchCalculator

java.lang.Object
com.marswars.geometry.LaunchCalculator
public class LaunchCalculator extends Object
LaunchCalculator - Map-based trajectory calculator for shooting

Uses interpolating tree maps to calculate launch parameters based on distance to target. Supports motion compensation for shooting while moving. Based on code from FRC teams 6328 and 254.

Example usage: 


 // Create calculator with robot-to-launcher transform
 LaunchCalculator calculator = new LaunchCalculator(
     "LaunchCalculator",
     new Transform2d(new Translation2d(-0.3, 0.0), new Rotation2d())
 );
 
 // Add data points for hood angle (distance in meters -> angle in radians)
 calculator.addHoodAnglePoint(1.5, Math.toRadians(20.0));
 calculator.addHoodAnglePoint(2.0, Math.toRadians(25.0));
 calculator.addHoodAnglePoint(3.0, Math.toRadians(30.0));
 
 // Add data points for flywheel speed (distance in meters -> speed in rad/s)
 calculator.addFlywheelSpeedPoint(1.5, 200.0);
 calculator.addFlywheelSpeedPoint(2.0, 220.0);
 calculator.addFlywheelSpeedPoint(3.0, 250.0);
 
 // Add data points for time of flight (distance in meters -> time in seconds)
 calculator.addTimeOfFlightPoint(1.5, 0.8);
 calculator.addTimeOfFlightPoint(2.0, 1.0);
 calculator.addTimeOfFlightPoint(3.0, 1.2);
 
 // Calculate parameters during operation
 LaunchParameters params = calculator.calculateLaunchParameters(
     robotPose,
     robotVelocity,
     targetPose
 );
 
 if (params.isValid()) {
     // Use params.driveAngle, params.hoodAngle, params.flywheelSpeed, etc.
 }

  • Constructor Details

    • LaunchCalculator

      public LaunchCalculator(String logging_prefix, Transform2d robot_to_launcher)
      Creates a new LaunchCalculator
      Parameters:
      logging_prefix - Prefix for DogLog tunable entries
      robot_to_launcher - Transform from robot center to launcher position

    • LaunchCalculator

      public LaunchCalculator(String logging_prefix, Transform2d robot_to_launcher, double loop_period)
      Creates a new LaunchCalculator with custom loop period
      Parameters:
      logging_prefix - Prefix for DogLog tunable entries
      robot_to_launcher - Transform from robot center to launcher position
      loop_period - Robot loop period in seconds

  • Method Details

    • addHoodAnglePoint

      public void addHoodAnglePoint(double distance, double angle)
      Adds a hood angle data point to the interpolation map
      Parameters:
      distance - Distance to target in meters
      angle - Hood angle in radians

    • addFlywheelSpeedPoint

      public void addFlywheelSpeedPoint(double distance, double speed)
      Adds a flywheel speed data point to the interpolation map
      Parameters:
      distance - Distance to target in meters
      speed - Flywheel speed in rad/s

    • addTimeOfFlightPoint

      public void addTimeOfFlightPoint(double distance, double time)
      Adds a time of flight data point to the interpolation map
      Parameters:
      distance - Distance to target in meters
      time - Time of flight in seconds

    • setMinDistance

      public void setMinDistance(double min_distance)
      Sets the minimum valid shooting distance
      Parameters:
      min_distance - Minimum distance in meters

    • setMaxDistance

      public void setMaxDistance(double max_distance)
      Sets the maximum valid shooting distance
      Parameters:
      max_distance - Maximum distance in meters

    • setPhaseDelay

      public void setPhaseDelay(double phase_delay)
      Sets the phase delay (processing and actuator lag)
      Parameters:
      phase_delay - Phase delay in seconds

    • getMinTimeOfFlight

      public double getMinTimeOfFlight()
      Gets the minimum time of flight from the map
      Returns:
      Minimum time of flight in seconds

    • getMaxTimeOfFlight

      public double getMaxTimeOfFlight()
      Gets the maximum time of flight from the map
      Returns:
      Maximum time of flight in seconds

    • getNaiveTimeOfFlight

      public double getNaiveTimeOfFlight(double distance)
      Gets a naive (non-interpolated) time of flight estimate
      Parameters:
      distance - Distance to target in meters
      Returns:
      Time of flight in seconds

    • clearCache

      public void clearCache()
      Clears the cached launch parameters, forcing recalculation on next call

    • calculateLaunchParameters

      public LaunchCalculator.LaunchParameters calculateLaunchParameters(Pose2d robot_pose, ChassisSpeeds robot_velocity, Pose2d target_pose)
      Calculates launch parameters with motion compensation
      Parameters:
      robot_pose - Current robot pose on the field
      robot_velocity - Current robot velocity (field-relative)
      target_pose - Target pose to shoot at
      Returns:
      Calculated launch parameters

    • calculateLaunchParameters

      public LaunchCalculator.LaunchParameters calculateLaunchParameters(Pose2d robot_pose, ChassisSpeeds robot_velocity, Translation2d target_translation)
      Calculates launch parameters with motion compensation

      This is the main method for calculating a shot. It handles:

      • Phase delay compensation (accounts for processing and mechanism lag)
      • Motion compensation (predicts where the robot will be when the shot releases)
      • Launcher offset from robot center (so the launcher points at target, not the robot center)
      • Velocity feedforward for smooth tracking

      How to use the returned parameters: 

      
 LaunchParameters params = calculator.calculateLaunchParameters(pose, velocity, target);
 
 if (params.is_valid) {
     // ACTION REQUIRED - Apply these 5 parameters to your robot mechanisms:
     driveSubsystem.setRotation(params.heading_angle, params.heading_velocity);
     hoodSubsystem.setAngle(params.hood_angle, params.hood_velocity);
     shooterSubsystem.setSpeed(params.flywheel_speed);
     
     // DIAGNOSTIC ONLY - Log these 3 parameters for debugging/visualization:
     MwLog.log("Shooter/TargetDistance", params.distance);
     MwLog.log("Shooter/DirectDistance", params.distance_no_lookahead);
     MwLog.log("Shooter/TimeOfFlight", params.time_of_flight);
 }

      Parameter Summary:

      • ACTION REQUIRED (5): heading_angle, heading_velocity, hood_angle, hood_velocity, flywheel_speed
      • DIAGNOSTIC ONLY (3): distance, distance_no_lookahead, time_of_flight
      Parameters:
      robot_pose - Current robot pose on the field
      robot_velocity - Current robot velocity (field-relative)
      target_translation - Target translation to shoot at
      Returns:
      Calculated launch parameters (check is_valid before using!)

    • getLatestParameters

      public LaunchCalculator.LaunchParameters getLatestParameters()
      Gets the most recently calculated launch parameters (cached)
      Returns:
      Latest launch parameters, or invalid parameters if none calculated yet

    • getStationaryAimedPose

      public Pose2d getStationaryAimedPose(Translation2d robot_translation, Translation2d target)
      Calculates the aimed robot pose for stationary shooting

      Useful for calculating setpoints for autonomous routines or visualizations.

      Parameters:
      robot_translation - Current robot translation
      target - Target translation
      Returns:
      Robot pose aimed at target