src/modules/robot/arm_solutions/JohannKosselSolution.cpp

   1 #include "JohannKosselSolution.h"
   2 #include <fastmath.h>
   3
   4 #define PIOVER180       0.01745329251994329576923690768489F
   5 #define SQ(x) powf(x, 2)
   6
   7 JohannKosselSolution::JohannKosselSolution(Config* passed_config) : config(passed_config){
   8     this->alpha_steps_per_mm = this->config->value(alpha_steps_per_mm_checksum)->as_number();
   9     this->beta_steps_per_mm  = this->config->value( beta_steps_per_mm_checksum)->as_number();
  10     this->gamma_steps_per_mm = this->config->value(gamma_steps_per_mm_checksum)->as_number();
  11
  12     // arm_length is the length of the arm from hinge to hinge
  13     this->arm_length         = this->config->value(arm_length_checksum)->by_default(250.0)->as_number();
  14     // arm_radius is the horizontal distance from hinge to hinge when the effector is centered
  15     this->arm_radius         = this->config->value(arm_radius_checksum)->by_default(124.0)->as_number();
  16
  17     init();
  18 }
  19
  20 void JohannKosselSolution::init() {
  21     this->arm_length_squared = SQ(this->arm_length);
  22
  23     // Effective X/Y positions of the three vertical towers.
  24     float DELTA_RADIUS= this->arm_radius;
  25     float SIN_60= 0.8660254037844386F;
  26     float COS_60= 0.5F;
  27     DELTA_TOWER1_X= -SIN_60*DELTA_RADIUS; // front left tower
  28     DELTA_TOWER1_Y= -COS_60*DELTA_RADIUS;
  29     DELTA_TOWER2_X= SIN_60*DELTA_RADIUS; // front right tower
  30     DELTA_TOWER2_Y= -COS_60*DELTA_RADIUS;
  31     DELTA_TOWER3_X= 0.0F; // back middle tower
  32     DELTA_TOWER3_Y= DELTA_RADIUS;
  33 }
  34
  35 void JohannKosselSolution::millimeters_to_steps( double millimeters[], int steps[] ){
  36     float cartesian[3];
  37     // convert input to float
  38     cartesian[0]= millimeters[0];
  39     cartesian[1]= millimeters[1];
  40     cartesian[2]= millimeters[2];
  41
  42     float delta_x = sqrtf(this->arm_length_squared
  43                          - SQ(DELTA_TOWER1_X-cartesian[0])
  44                          - SQ(DELTA_TOWER1_Y-cartesian[1])
  45                         ) + cartesian[2];
  46     float delta_y = sqrtf(this->arm_length_squared
  47                          - SQ(DELTA_TOWER2_X-cartesian[0])
  48                          - SQ(DELTA_TOWER2_Y-cartesian[1])
  49                         ) + cartesian[2];
  50     float delta_z = sqrtf(this->arm_length_squared
  51                          - SQ(DELTA_TOWER3_X-cartesian[0])
  52                          - SQ(DELTA_TOWER3_Y-cartesian[1])
  53                         ) + cartesian[2];
  54
  55
  56     steps[ALPHA_STEPPER] = lround( delta_x * this->alpha_steps_per_mm );
  57     steps[BETA_STEPPER ] = lround( delta_y * this->beta_steps_per_mm );
  58     steps[GAMMA_STEPPER] = lround( delta_z * this->gamma_steps_per_mm );
  59 }
  60
  61 void JohannKosselSolution::steps_to_millimeters( int steps[], double millimeters[] ){}
  62
  63 void JohannKosselSolution::set_steps_per_millimeter( double steps[] )
  64 {
  65     this->alpha_steps_per_mm = steps[0];
  66     this->beta_steps_per_mm  = steps[1];
  67     this->gamma_steps_per_mm = steps[2];
  68 }
  69
  70 void JohannKosselSolution::get_steps_per_millimeter( double steps[] )
  71 {
  72     steps[0] = this->alpha_steps_per_mm;
  73     steps[1] = this->beta_steps_per_mm;
  74     steps[2] = this->gamma_steps_per_mm;
  75 }
  76
  77 bool JohannKosselSolution::set_optional(char parameter, double value) {
  78
  79     switch(parameter) {
  80         case 'L': // sets arm_length
  81             this->arm_length= value;
  82             init();
  83             break;
  84         case 'R': // sets arm_radius
  85             this->arm_radius= value;
  86             init();
  87             break;
  88         default:
  89             return false;
  90     }
  91     return true;
  92 }
  93
  94 bool JohannKosselSolution::get_optional(char parameter, double *value) {
  95     if(value == NULL) return false;
  96
  97     switch(parameter) {
  98         case 'L': // get arm_length
  99             *value= this->arm_length;
 100             break;
 101         case 'R': // get arm_radius
 102             *value= this->arm_radius;
 103             break;
 104         default:
 105             return false;
 106     }
 107
 108     return true;
 109 };