swerve_utils.h

Go to the documentation of this file.

 
 
#pragma once
 
#include <frc/kinematics/SwerveModuleState.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/velocity.h>
 
#include <optional>
 
#include "interpolation.h"
 
namespace argos_lib {
  namespace swerve {
 
    frc::SwerveModuleState Optimize(frc::SwerveModuleState desiredState,
                                    units::degree_t currentModuleAngle,
                                    units::degrees_per_second_t currentModuleAngularRate,
                                    units::feet_per_second_t currentModuleDriveVel,
                                    units::feet_per_second_t maxVelocity);
 
    struct SwerveModulePositions {
      units::degree_t FrontLeft;
      units::degree_t FrontRight;
      units::degree_t RearRight;
      units::degree_t RearLeft;
    };
 
    struct TranslationSpeeds {
      double forwardSpeedPct;  
      double leftSpeedPct;     
    };
 
    template <class T, int size, class V>
    [[nodiscard]] constexpr TranslationSpeeds CircularInterpolate(
        const TranslationSpeeds rawSpeeds, const argos_lib::InterpolationMap<T, size, V> interpMap) {
      const double magnitude = std::sqrt(std::pow(rawSpeeds.forwardSpeedPct, 2) + std::pow(rawSpeeds.leftSpeedPct, 2));
      const double angle = std::atan2(rawSpeeds.leftSpeedPct, rawSpeeds.forwardSpeedPct);
      const double mappedMagnitude = interpMap(magnitude);
      return TranslationSpeeds{mappedMagnitude * std::cos(angle), mappedMagnitude * std::sin(angle)};
    }
 
  }  // namespace swerve
}  // namespace argos_lib