chassis_accelerations.h

#pragma once
 
#include "core/math/geometry/rotation2d.h"
#include "core/math/geometry/translation2d.h"
#include "core/math/units.h"
 
using namespace units::literals;
 
/**
 * @brief Represents robot chassis accelerations.
 */
struct ChassisAccelerations {
  /** @brief Acceleration along the x-axis. (Forward is positive) */
  units::Acceleration ax = 0_inps2;
 
  /** @brief Acceleration along the y-axis. (Left is positive) */
  units::Acceleration ay = 0_inps2;
 
  /** @brief Angular acceleration of the robot frame. (Counter-Clockwise is positive) */
  units::AngularAcceleration alpha = 0_radps2;
 
  ChassisAccelerations() = default;
 
  ChassisAccelerations(
      units::Acceleration ax, units::Acceleration ay, units::AngularAcceleration alpha
  )
      : ax(ax), ay(ay), alpha(alpha) {}
 
  /**
   * @brief Converts this field-relative set of accelerations into a robot-relative
   * ChassisAccelerations object.
   * @param robotAngle The CCW-positive angle of the robot relative to the field.
   * @return Robot-relative ChassisAccelerations.
   */
  ChassisAccelerations to_robot_relative(const Rotation2d& robotAngle) const {
    Acceleration2d rotated = Acceleration2d(ax, ay).rotate_by(-robotAngle);
    return ChassisAccelerations(rotated.x(), rotated.y(), alpha);
  }
 
  /**
   * @brief Converts this robot-relative set of accelerations into a field-relative
   * ChassisAccelerations object.
   * @param robotAngle The CCW-positive angle of the robot relative to the field.
   * @return Field-relative ChassisAccelerations.
   */
  ChassisAccelerations to_field_relative(const Rotation2d& robotAngle) const {
    Acceleration2d rotated = Acceleration2d(ax, ay).rotate_by(robotAngle);
    return ChassisAccelerations(rotated.x(), rotated.y(), alpha);
  }
 
  // Operator overloads
  ChassisAccelerations operator+(const ChassisAccelerations& other) const {
    return ChassisAccelerations(ax + other.ax, ay + other.ay, alpha + other.alpha);
  }
 
  ChassisAccelerations operator-(const ChassisAccelerations& other) const {
    return ChassisAccelerations(ax - other.ax, ay - other.ay, alpha - other.alpha);
  }
 
  ChassisAccelerations operator-() const { return ChassisAccelerations(-ax, -ay, -alpha); }
 
  ChassisAccelerations operator*(double scalar) const {
    return ChassisAccelerations(ax * scalar, ay * scalar, alpha * scalar);
  }
 
  ChassisAccelerations operator/(double scalar) const {
    return ChassisAccelerations(ax / scalar, ay / scalar, alpha / scalar);
  }
 
  bool operator==(const ChassisAccelerations& other) const {
    return ax == other.ax && ay == other.ay && alpha == other.alpha;
  }
 
  bool operator!=(const ChassisAccelerations& other) const { return !(*this == other); }
};