mathUtil.hpp

Go to the documentation of this file.

/*
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */
#pragma once
 
#include "okapi/api/device/motor/abstractMotor.hpp"
#include <algorithm>
#include <cstdint>
#include <math.h>
#include <type_traits>
 
namespace okapi {
static constexpr double inchToMM = 25.4;
 
static constexpr double mmToInch = 0.0393700787;
 
static constexpr double degreeToRadian = 0.01745329252;
 
static constexpr double radianToDegree = 57.2957795;
 
static constexpr double imeTorqueTPR = 627.2;
 
static constexpr std::int32_t imeSpeedTPR = 392;
 
static constexpr double imeTurboTPR = 261.333;
 
static constexpr double ime269TPR = 240.448;
 
static constexpr std::int32_t imev5RedTPR = 1800;
 
static constexpr std::int32_t imev5GreenTPR = 900;
 
static constexpr std::int32_t imev5BlueTPR = 300;
 
static constexpr std::int32_t quadEncoderTPR = 360;
 
static constexpr double pi = 3.1415926535897932;
 
static constexpr double pi2 = 1.5707963267948966;
 
static constexpr double gravity = 9.80665;
 
static constexpr auto OKAPI_PROS_ERR = INT32_MAX;
 
static constexpr auto OKAPI_PROS_ERR_F = INFINITY;
 
static constexpr double v5MotorMaxVoltage = 12000;
 
static constexpr std::int8_t motorUpdateRate = 10;
 
static constexpr std::int8_t adiUpdateRate = 10;
 
constexpr double ipow(const double base, const int expo) {
  return (expo == 0) ? 1
         : expo == 1 ? base
         : expo > 1  ? ((expo & 1) ? base * ipow(base, expo - 1)
                                   : ipow(base, expo / 2) * ipow(base, expo / 2))
                     : 1 / ipow(base, -expo);
}
 
constexpr double cutRange(const double value, const double min, const double max) {
  const double middle = max - ((max - min) / 2);
 
  if (value > min && value < middle) {
    return min;
  } else if (value <= max && value >= middle) {
    return max;
  }
 
  return value;
}
 
constexpr double deadband(const double value, const double min, const double max) {
  return std::clamp(value, min, max) == value ? 0 : value;
}
 
constexpr double remapRange(const double value,
                            const double oldMin,
                            const double oldMax,
                            const double newMin,
                            const double newMax) {
  return (value - oldMin) * ((newMax - newMin) / (oldMax - oldMin)) + newMin;
}
 
template <typename E> constexpr auto toUnderlyingType(const E e) noexcept {
  return static_cast<std::underlying_type_t<E>>(e);
}
 
constexpr auto boolToSign(const bool b) noexcept {
  return b ? 1 : -1;
}
 
constexpr long modulus(const long lhs, const long rhs) noexcept {
  return ((lhs % rhs) + rhs) % rhs;
}
 
constexpr std::int32_t gearsetToTPR(const AbstractMotor::gearset igearset) noexcept {
  switch (igearset) {
  case AbstractMotor::gearset::red:
    return imev5RedTPR;
  case AbstractMotor::gearset::green:
    return imev5GreenTPR;
  case AbstractMotor::gearset::blue:
  case AbstractMotor::gearset::invalid:
  default:
    return imev5BlueTPR;
  }
}
 
constexpr std::int8_t transformADIPort(const std::int8_t port) {
  if (port >= 'a' && port <= 'h') {
    return port - ('a' - 1);
  } else if (port >= 'A' && port <= 'H') {
    return port - ('A' - 1);
  } else {
    return port;
  }
}
} // namespace okapi