src/modules/robot/arm_solutions/MorganSCARASolution.cpp

   1 #include "MorganSCARASolution.h"
   2 #include <fastmath.h>
   3 #include "checksumm.h"
   4 #include "ConfigValue.h"
   5 #include "libs/Kernel.h"
   6 //#include "StreamOutputPool.h"
   7 //#include "Gcode.h"
   8 //#include "SerialMessage.h"
   9 //#include "Conveyor.h"
  10 //#include "Robot.h"
  11 //#include "StepperMotor.h"
  12
  13 #include "libs/nuts_bolts.h"
  14
  15 #include "libs/Config.h"
  16
  17 #define arm1_length_checksum         CHECKSUM("arm1_length")
  18 #define arm2_length_checksum         CHECKSUM("arm2_length")
  19 #define morgan_offset_x_checksum     CHECKSUM("morgan_offset_x")
  20 #define morgan_offset_y_checksum     CHECKSUM("morgan_offset_y")
  21 #define axis_scaling_x_checksum      CHECKSUM("axis_scaling_x")
  22 #define axis_scaling_y_checksum      CHECKSUM("axis_scaling_y")
  23
  24 #define SQ(x) powf(x, 2)
  25 #define ROUND(x, y) (roundf(x * 1e ## y) / 1e ## y)
  26
  27 MorganSCARASolution::MorganSCARASolution(Config* config)
  28 {
  29     // arm1_length is the length of the inner main arm from hinge to hinge
  30     arm1_length         = config->value(arm1_length_checksum)->by_default(150.0f)->as_number();
  31     // arm2_length is the length of the inner main arm from hinge to hinge
  32     arm2_length         = config->value(arm2_length_checksum)->by_default(150.0f)->as_number();
  33     // morgan_offset_x is the x offset of bed zero position towards the SCARA tower center
  34     morgan_offset_x     = config->value(morgan_offset_x_checksum)->by_default(100.0f)->as_number();
  35     // morgan_offset_y is the y offset of bed zero position towards the SCARA tower center
  36     morgan_offset_y     = config->value(morgan_offset_y_checksum)->by_default(-65.0f)->as_number();
  37
  38     init();
  39 }
  40
  41 void MorganSCARASolution::init() {
  42
  43 }
  44
  45 float MorganSCARASolution::to_degrees(float radians) {
  46     return radians*(180.0F/3.14159265359f);
  47 }
  48
  49 void MorganSCARASolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] )
  50 {
  51
  52     float SCARA_pos[2],
  53           SCARA_C2,
  54           SCARA_S2,
  55           SCARA_K1,
  56           SCARA_K2,
  57           SCARA_theta,
  58           SCARA_psi;
  59
  60     SCARA_pos[X_AXIS] = cartesian_mm[X_AXIS] - this->morgan_offset_x;  //Translate cartesian to tower centric SCARA X Y
  61     SCARA_pos[Y_AXIS] = cartesian_mm[Y_AXIS] - this->morgan_offset_y;  // morgan_offset not to be confused with home offset. Makes the SCARA math work.
  62
  63     if (this->arm1_length == this->arm2_length)
  64         SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-2.0f*SQ(this->arm1_length)) / (2.0f * SQ(this->arm1_length));
  65     else
  66         SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-SQ(this->arm1_length)-SQ(this->arm2_length)) / (2.0f * SQ(this->arm1_length));
  67
  68     // SCARA position is undefined if abs(SCARA_C2) >=1
  69     // In reality abs(SCARA_C2) >0.95 is problematic.
  70
  71     if (SCARA_C2 > 0.95f)
  72         SCARA_C2 = 0.95f;
  73     else if (SCARA_C2 < -0.95f)
  74         SCARA_C2 = -0.95f;
  75
  76
  77     SCARA_S2 = sqrtf(1.0f-SQ(SCARA_C2));
  78
  79     SCARA_K1 = this->arm1_length+this->arm2_length*SCARA_C2;
  80     SCARA_K2 = this->arm2_length*SCARA_S2;
  81
  82     SCARA_theta = (atan2f(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2f(SCARA_K1, SCARA_K2))*-1.0f;    // Morgan Thomas turns Theta in oposite direction
  83     SCARA_psi   = atan2f(SCARA_S2,SCARA_C2);
  84
  85
  86     actuator_mm[ALPHA_STEPPER] = to_degrees(SCARA_theta);             // Multiply by 180/Pi  -  theta is support arm angle
  87     actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_theta + SCARA_psi); // Morgan kinematics (dual arm)
  88     //actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_psi);             // real scara
  89     actuator_mm[GAMMA_STEPPER] = cartesian_mm[Z_AXIS];                // No inverse kinematics on Z - Position to add bed offset?
  90
  91 }
  92
  93 void MorganSCARASolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ) {
  94     // Perform forward kinematics, and place results in cartesian_mm[]
  95
  96     float y1, y2,
  97            actuator_rad[2];
  98
  99     actuator_rad[X_AXIS] = actuator_mm[X_AXIS]/(180.0F/3.14159265359f);
 100     actuator_rad[Y_AXIS] = actuator_mm[Y_AXIS]/(180.0F/3.14159265359f);
 101
 102     y1 = sinf(actuator_rad[X_AXIS])*this->arm1_length;
 103     y2 = sinf(actuator_rad[Y_AXIS])*this->arm2_length + y1;
 104
 105     cartesian_mm[X_AXIS] = cosf(actuator_rad[X_AXIS])*this->arm1_length + cosf(actuator_rad[Y_AXIS])*this->arm2_length + this->morgan_offset_x;
 106     cartesian_mm[Y_AXIS] = y2 + this->morgan_offset_y;
 107     cartesian_mm[Z_AXIS] = actuator_mm[Z_AXIS];
 108
 109     cartesian_mm[0] = ROUND(cartesian_mm[0], 4);
 110     cartesian_mm[1] = ROUND(cartesian_mm[1], 4);
 111     cartesian_mm[2] = ROUND(cartesian_mm[2], 4);
 112 }
 113
 114 bool MorganSCARASolution::set_optional(const arm_options_t& options) {
 115
 116     arm_options_t::const_iterator i;
 117
 118     i= options.find('T');          // Theta arm1 length
 119     if(i != options.end()) {
 120         arm1_length= i->second;
 121
 122     }
 123     i= options.find('P');          // Psi arm2 length
 124     if(i != options.end()) {
 125         arm2_length= i->second;
 126     }
 127     i= options.find('X');          // Home initial position X
 128     if(i != options.end()) {
 129         morgan_offset_x= i->second;
 130     }
 131     i= options.find('Y');          // Home initial position Y
 132     if(i != options.end()) {
 133         morgan_offset_y= i->second;
 134     }
 135
 136     init();
 137     return true;
 138 }
 139
 140 bool MorganSCARASolution::get_optional(arm_options_t& options) {
 141     options['T']= this->arm1_length;
 142     options['P']= this->arm2_length;
 143     options['X']= this->morgan_offset_x;
 144     options['Y']= this->morgan_offset_y;
 145     return true;
 146 };