src/modules/robot/arm_solutions/RotatableCartesianSolution.cpp

   1 #include "RotatableCartesianSolution.h"
   2 #include <math.h>
   3 #include "ActuatorCoordinates.h"
   4 #include "checksumm.h"
   5 #include "ConfigValue.h"
   6
   7 // degrees * (pi / 180) = radians
   8 #define DEG2RAD       0.01745329251994329576923690768489
   9
  10 RotatableCartesianSolution::RotatableCartesianSolution(Config* config) {
  11     float alpha_angle  = config->value(alpha_angle_checksum)->by_default(0.0f)->as_number() * DEG2RAD;
  12     sin_alpha          = sinf(alpha_angle);
  13     cos_alpha          = cosf(alpha_angle);
  14 }
  15
  16 void RotatableCartesianSolution::cartesian_to_actuator(const float cartesian_mm[], ActuatorCoordinates &actuator_mm ){
  17     rotate( cartesian_mm, &actuator_mm[0], sin_alpha, cos_alpha );
  18 }
  19
  20 void RotatableCartesianSolution::actuator_to_cartesian(const ActuatorCoordinates &actuator_mm, float cartesian_mm[] ){
  21     rotate( &actuator_mm[0], cartesian_mm, - sin_alpha, cos_alpha );
  22 }
  23
  24 void RotatableCartesianSolution::rotate(const float in[], float out[], float sin, float cos ){
  25     out[ALPHA_STEPPER] = cos * in[X_AXIS] - sin * in[Y_AXIS];
  26     out[BETA_STEPPER ] = sin * in[X_AXIS] + cos * in[Y_AXIS];
  27     out[GAMMA_STEPPER] =       in[Z_AXIS];
  28 }