src/modules/robot/arm_solutions/RostockSolution.cpp

   1 #include "RostockSolution.h"
   2 #include <fastmath.h>
   3
   4 #define PIOVER180       0.01745329251994329576923690768489F
   5
   6 RostockSolution::RostockSolution(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     float alpha_angle  = PIOVER180 * this->config->value(alpha_angle_checksum)->by_default(30.0)->as_number();
  12     this->sin_alpha     = sinf(alpha_angle);
  13     this->cos_alpha     = cosf(alpha_angle);
  14     float beta_angle   = PIOVER180 * this->config->value( beta_relative_angle_checksum)->by_default(120.0)->as_number();
  15     this->sin_beta      = sinf(beta_angle);
  16     this->cos_beta      = cosf(beta_angle);
  17     float gamma_angle  = PIOVER180 * this->config->value(gamma_relative_angle_checksum)->by_default(240.0)->as_number();
  18     this->sin_gamma     = sinf(gamma_angle);
  19     this->cos_gamma     = cosf(gamma_angle);
  20
  21     // arm_length is the length of the arm from hinge to hinge
  22     this->arm_length         = this->config->value(arm_length_checksum)->by_default(250.0)->as_number();
  23     // arm_radius is the horizontal distance from hinge to hinge when the effector is centered
  24     this->arm_radius         = this->config->value(arm_radius_checksum)->by_default(124.0)->as_number();
  25
  26     this->arm_length_squared = powf(this->arm_length, 2);
  27 }
  28
  29 void RostockSolution::millimeters_to_steps( double millimeters[], int steps[] ){
  30     float mm[3], alpha_rotated[3], rotated[3];
  31
  32     // convert input to float
  33     mm[0]= millimeters[0];
  34     mm[1]= millimeters[1];
  35     mm[2]= millimeters[2];
  36
  37     if( this->sin_alpha == 0 && this->cos_alpha == 1){
  38         alpha_rotated[0] = mm[0];
  39         alpha_rotated[1] = mm[1];
  40         alpha_rotated[2] = mm[2];
  41     }else{
  42         rotate( mm, alpha_rotated, sin_alpha, cos_alpha );
  43     }
  44     steps[ALPHA_STEPPER] = lround( solve_arm( alpha_rotated ) * this->alpha_steps_per_mm );
  45
  46     rotate( alpha_rotated, rotated, sin_beta, cos_beta );
  47     steps[BETA_STEPPER ] = lround( solve_arm( rotated ) * this->beta_steps_per_mm );
  48
  49     rotate( alpha_rotated, rotated, sin_gamma, cos_gamma );
  50     steps[GAMMA_STEPPER] = lround( solve_arm( rotated ) * this->gamma_steps_per_mm );
  51 }
  52
  53 void RostockSolution::steps_to_millimeters( int steps[], double millimeters[] ){}
  54
  55 float RostockSolution::solve_arm( float millimeters[]) {
  56     return sqrtf(arm_length_squared - powf(millimeters[X_AXIS] - this->arm_radius, 2) - powf(millimeters[Y_AXIS], 2)) + millimeters[Z_AXIS];
  57 }
  58
  59 void RostockSolution::rotate(float in[], float out[], float sin, float cos ){
  60     out[X_AXIS] = cos * in[X_AXIS] - sin * in[Y_AXIS];
  61     out[Y_AXIS] = sin * in[X_AXIS] + cos * in[Y_AXIS];
  62     out[Z_AXIS] = in[Z_AXIS];
  63 }
  64
  65 void RostockSolution::set_steps_per_millimeter( double steps[] )
  66 {
  67     this->alpha_steps_per_mm = steps[0];
  68     this->beta_steps_per_mm  = steps[1];
  69     this->gamma_steps_per_mm = steps[2];
  70 }
  71
  72 void RostockSolution::get_steps_per_millimeter( double steps[] )
  73 {
  74     steps[0] = this->alpha_steps_per_mm;
  75     steps[1] = this->beta_steps_per_mm;
  76     steps[2] = this->gamma_steps_per_mm;
  77 }
  78