src/modules/robot/arm_solutions/RotatableCartesianSolution.cpp

   1 #include "RotatableCartesianSolution.h"
   2 #include <math.h>
   3
   4 #define PIOVER180       0.01745329251994329576923690768489
   5
   6 RotatableCartesianSolution::RotatableCartesianSolution(Config* passed_config) : config(passed_config){
   7     this->alpha_steps_per_mm = this->config->value(alpha_steps_per_mm_checksum)->as_number();
   8     this->beta_steps_per_mm  = this->config->value( beta_steps_per_mm_checksum)->as_number();
   9     this->gamma_steps_per_mm = this->config->value(gamma_steps_per_mm_checksum)->as_number();
  10
  11     double alpha_angle  = PIOVER180 * this->config->value(alpha_angle_checksum)->by_default(0.0)->as_number();
  12     this->sin_alpha     = sin(alpha_angle);
  13     this->cos_alpha     = cos(alpha_angle);
  14 }
  15
  16 void RotatableCartesianSolution::millimeters_to_steps( double millimeters[], int steps[] ){
  17     double rotated[3];
  18     rotate( millimeters, rotated, this->sin_alpha, this->cos_alpha );
  19
  20     steps[ALPHA_STEPPER] = lround( rotated[X_AXIS] * this->alpha_steps_per_mm );
  21     steps[BETA_STEPPER ] = lround( rotated[Y_AXIS] * this->beta_steps_per_mm );
  22     steps[GAMMA_STEPPER] = lround( rotated[Z_AXIS] * this->gamma_steps_per_mm );
  23 }
  24
  25 void RotatableCartesianSolution::steps_to_millimeters( int steps[], double millimeters[] ){
  26     double rotated[3];
  27
  28     rotated[ALPHA_STEPPER] = steps[X_AXIS] / this->alpha_steps_per_mm;
  29     rotated[BETA_STEPPER ] = steps[Y_AXIS] / this->beta_steps_per_mm;
  30     rotated[GAMMA_STEPPER] = steps[Z_AXIS] / this->gamma_steps_per_mm;
  31
  32     rotate( rotated, millimeters, - this->sin_alpha, this->cos_alpha );
  33 }
  34
  35 void RotatableCartesianSolution::rotate(double in[], double out[], double sin, double cos ){
  36     out[X_AXIS] = cos * in[X_AXIS] - sin * in[Y_AXIS];
  37     out[Y_AXIS] = sin * in[X_AXIS] + cos * in[Y_AXIS];
  38     out[Z_AXIS] = in[Z_AXIS];
  39 }
  40
  41 void RotatableCartesianSolution::set_steps_per_millimeter( double steps[] )
  42 {
  43     this->alpha_steps_per_mm = steps[0];
  44     this->beta_steps_per_mm  = steps[1];
  45     this->gamma_steps_per_mm = steps[2];
  46 }
  47
  48 void RotatableCartesianSolution::get_steps_per_millimeter( double steps[] )
  49 {
  50     steps[0] = this->alpha_steps_per_mm;
  51     steps[1] = this->beta_steps_per_mm;
  52     steps[2] = this->gamma_steps_per_mm;
  53 }