servoSettings.cpp

Go to the documentation of this file.

#include <galaxis/servoSettings.hpp>
#include <cstddef>
#include <Eigen/Dense>
#include <cmath>
 
namespace galaxis {
  namespace servoSettings {
    constexpr float slope = 0.0043f; /* angle = slope * servo */
    float servoSettingToAngle(int8_t servoSetting) {
      /* TODO check for maximum angle */
      float angle = slope * servoSetting;
 
      if(angle > galaxis::servoSettings::maximumAngle) {
        angle = galaxis::servoSettings::maximumAngle;
      } else if(angle < -galaxis::servoSettings::maximumAngle) {
        angle = -galaxis::servoSettings::maximumAngle;
      }
 
      return angle;
    }
 
    int8_t angleToServoSetting(float angle) {
      int servoSetting = (int)(angle / slope);
      if(servoSetting < -100) {
        servoSetting = -100;
      } else if(servoSetting > 100) {
        servoSetting = 100;
      }
 
      return servoSetting;
    }
  } // namespace servoSettings
} // namespace galaxis