src/modules/robot/arm_solutions/RotatableCartesianSolution.cpp

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