tank_kinematics.h

#pragma once
 
#include "core/math/kinematics/chassis_accelerations.h"
#include "core/math/kinematics/chassis_velocities.h"
#include "core/math/kinematics/tank_wheel_accelerations.h"
#include "core/math/kinematics/tank_wheel_velocities.h"
#include "core/math/units.h"
 
using namespace units::literals;

class TankKinematics {
 public:
  units::Length track_width;

  explicit TankKinematics(units::Length track_width) : track_width(track_width) {}

  ChassisVelocities to_chassis_velocities(const TankWheelVelocities& wheel_velocities) const {
    units::Velocity vx = (wheel_velocities.left + wheel_velocities.right) / 2.0;
 
    // omega = (right - left) / track_width
    units::AngularVelocity omega =
        ((wheel_velocities.right.to(units::inps) - wheel_velocities.left.to(units::inps)) /
         track_width.to(units::in)) *
        units::radps;
 
    return ChassisVelocities(vx, 0_inps, omega);
  }

  TankWheelVelocities to_wheel_velocities(const ChassisVelocities& chassis_velocities) const {
    // v_L = vx - (track_width / 2) * omega
    // v_R = vx + (track_width / 2) * omega
    units::Velocity omega_linear =
        (chassis_velocities.omega.to(units::radps) * (track_width.to(units::in) / 2.0)) *
        units::inps;
 
    return TankWheelVelocities(chassis_velocities.vx - omega_linear,
                               chassis_velocities.vx + omega_linear);
  }

  ChassisAccelerations to_chassis_accelerations(const TankWheelAccelerations& wheel_accels) const {
    units::Acceleration ax = (wheel_accels.left + wheel_accels.right) / 2.0;
 
    // alpha = (right - left) / track_width
    units::AngularAcceleration alpha =
        ((wheel_accels.right.to(units::inps2) - wheel_accels.left.to(units::inps2)) /
         track_width.to(units::in)) *
        units::radps2;
 
    return ChassisAccelerations(ax, 0_inps2, alpha);
  }

  TankWheelAccelerations to_wheel_accelerations(const ChassisAccelerations& chassis_accels) const {
    // a_L = ax - (track_width / 2) * alpha
    // a_R = ax + (track_width / 2) * alpha
    units::Acceleration alpha_linear =
        (chassis_accels.alpha.to(units::radps2) * (track_width.to(units::in) / 2.0)) * units::inps2;
 
    return TankWheelAccelerations(chassis_accels.ax - alpha_linear,
                                  chassis_accels.ax + alpha_linear);
  }
};