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 morgan_scaling_x_checksum     CHECKSUM("morgan_scaling_x")
  22 #define morgan_scaling_y_checksum     CHECKSUM("morgan_scaling_y")
  23 #define morgan_homing_checksum        CHECKSUM("morgan_homing")
  24 #define morgan_undefined_min_checksum CHECKSUM("morgan_undefined_min")
  25 #define morgan_undefined_max_checksum CHECKSUM("morgan_undefined_max")
  26
  27 #define SQ(x) powf(x, 2)
  28 #define ROUND(x, y) (roundf(x * 1e ## y) / 1e ## y)
  29
  30 MorganSCARASolution::MorganSCARASolution(Config* config)
  31 {
  32     // arm1_length is the length of the inner main arm from hinge to hinge
  33     arm1_length         = config->value(arm1_length_checksum)->by_default(150.0f)->as_number();
  34     // arm2_length is the length of the inner main arm from hinge to hinge
  35     arm2_length         = config->value(arm2_length_checksum)->by_default(150.0f)->as_number();
  36     // morgan_offset_x is the x offset of bed zero position towards the SCARA tower center
  37     morgan_offset_x     = config->value(morgan_offset_x_checksum)->by_default(100.0f)->as_number();
  38     // morgan_offset_y is the y offset of bed zero position towards the SCARA tower center
  39     morgan_offset_y     = config->value(morgan_offset_y_checksum)->by_default(-60.0f)->as_number();
  40     // Axis scaling is used in final calibration
  41     morgan_scaling_x    = config->value(morgan_scaling_x_checksum)->by_default(1.0F)->as_number(); // 1 = 100% : No scaling
  42     morgan_scaling_y    = config->value(morgan_scaling_y_checksum)->by_default(1.0F)->as_number();
  43     // morgan_undefined is the ratio at which the SCARA position is undefined.
  44     // required to prevent the arm moving through singularity points
  45     // min: head close to tower
  46     morgan_undefined_min  = config->value(morgan_undefined_min_checksum)->by_default(0.95f)->as_number();
  47     // max: head on maximum reach
  48     morgan_undefined_max  = config->value(morgan_undefined_max_checksum)->by_default(0.95f)->as_number();
  49
  50     init();
  51 }
  52
  53 void MorganSCARASolution::init() {
  54
  55 }
  56
  57 float MorganSCARASolution::to_degrees(float radians) {
  58     return radians*(180.0F/3.14159265359f);
  59 }
  60
  61 void MorganSCARASolution::cartesian_to_actuator(const float cartesian_mm[], float actuator_mm[] )
  62 {
  63
  64     float SCARA_pos[2],
  65           SCARA_C2,
  66           SCARA_S2,
  67           SCARA_K1,
  68           SCARA_K2,
  69           SCARA_theta,
  70           SCARA_psi;
  71
  72     SCARA_pos[X_AXIS] = (cartesian_mm[X_AXIS] - this->morgan_offset_x)  * this->morgan_scaling_x;  //Translate cartesian to tower centric SCARA X Y AND apply scaling factor from this offset.
  73     SCARA_pos[Y_AXIS] = (cartesian_mm[Y_AXIS]  * this->morgan_scaling_y - this->morgan_offset_y);  // morgan_offset not to be confused with home offset. This makes the SCARA math work.
  74     // Y has to be scaled before subtracting offset to ensure position on bed.
  75
  76     if (this->arm1_length == this->arm2_length)
  77         SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-2.0f*SQ(this->arm1_length)) / (2.0f * SQ(this->arm1_length));
  78     else
  79         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));
  80
  81     // SCARA position is undefined if abs(SCARA_C2) >=1
  82     // In reality abs(SCARA_C2) >0.95 can be problematic.
  83
  84     if (SCARA_C2 > this->morgan_undefined_max)
  85         SCARA_C2 = this->morgan_undefined_max;
  86     else if (SCARA_C2 < -this->morgan_undefined_min)
  87         SCARA_C2 = -this->morgan_undefined_min;
  88
  89
  90     SCARA_S2 = sqrtf(1.0f-SQ(SCARA_C2));
  91
  92     SCARA_K1 = this->arm1_length+this->arm2_length*SCARA_C2;
  93     SCARA_K2 = this->arm2_length*SCARA_S2;
  94
  95     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
  96     SCARA_psi   = atan2f(SCARA_S2,SCARA_C2);
  97
  98
  99     actuator_mm[ALPHA_STEPPER] = to_degrees(SCARA_theta);             // Multiply by 180/Pi  -  theta is support arm angle
 100     actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_theta + SCARA_psi); // Morgan kinematics (dual arm)
 101     //actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_psi);             // real scara
 102     actuator_mm[GAMMA_STEPPER] = cartesian_mm[Z_AXIS];                // No inverse kinematics on Z - Position to add bed offset?
 103
 104 }
 105
 106 void MorganSCARASolution::actuator_to_cartesian(const float actuator_mm[], float cartesian_mm[] ) {
 107     // Perform forward kinematics, and place results in cartesian_mm[]
 108
 109     float y1, y2,
 110            actuator_rad[2];
 111
 112     actuator_rad[X_AXIS] = actuator_mm[X_AXIS]/(180.0F/3.14159265359f);
 113     actuator_rad[Y_AXIS] = actuator_mm[Y_AXIS]/(180.0F/3.14159265359f);
 114
 115     y1 = sinf(actuator_rad[X_AXIS])*this->arm1_length;
 116     y2 = sinf(actuator_rad[Y_AXIS])*this->arm2_length + y1;
 117
 118     cartesian_mm[X_AXIS] = (((cosf(actuator_rad[X_AXIS])*this->arm1_length) + (cosf(actuator_rad[Y_AXIS])*this->arm2_length)) / this->morgan_scaling_x) + this->morgan_offset_x;
 119     cartesian_mm[Y_AXIS] = (y2 + this->morgan_offset_y) / this->morgan_scaling_y;
 120     cartesian_mm[Z_AXIS] = actuator_mm[Z_AXIS];
 121
 122     cartesian_mm[0] = ROUND(cartesian_mm[0], 7);
 123     cartesian_mm[1] = ROUND(cartesian_mm[1], 7);
 124     cartesian_mm[2] = ROUND(cartesian_mm[2], 7);
 125 }
 126
 127 bool MorganSCARASolution::set_optional(const arm_options_t& options) {
 128
 129     arm_options_t::const_iterator i;
 130
 131     i= options.find('T');          // Theta arm1 length
 132     if(i != options.end()) {
 133         arm1_length= i->second;
 134
 135     }
 136     i= options.find('P');          // Psi arm2 length
 137     if(i != options.end()) {
 138         arm2_length= i->second;
 139     }
 140     i= options.find('X');          // Home initial position X
 141     if(i != options.end()) {
 142         morgan_offset_x= i->second;
 143     }
 144     i= options.find('Y');          // Home initial position Y
 145     if(i != options.end()) {
 146         morgan_offset_y= i->second;
 147     }
 148     i= options.find('A');          // Scaling X_AXIS
 149     if(i != options.end()) {
 150         morgan_scaling_x= i->second;
 151     }
 152     i= options.find('B');          // Scaling Y_AXIS
 153     if(i != options.end()) {
 154         morgan_scaling_y= i->second;
 155     }
 156     //i= options.find('C');          // Scaling Z_AXIS
 157     //if(i != options.end()) {
 158     //    morgan_scaling_z= i->second;
 159     //}
 160     i= options.find('D');          // Undefined min
 161     if(i != options.end()) {
 162         this->morgan_undefined_min = i->second;
 163     }
 164     i= options.find('E');          // undefined max
 165     if(i != options.end()) {
 166         this->morgan_undefined_max = i->second;
 167     }
 168
 169     init();
 170     return true;
 171 }
 172
 173 bool MorganSCARASolution::get_optional(arm_options_t& options, bool force_all) {
 174     options['T']= this->arm1_length;
 175     options['P']= this->arm2_length;
 176     options['X']= this->morgan_offset_x;
 177     options['Y']= this->morgan_offset_y;
 178     options['A']= this->morgan_scaling_x;
 179     options['B']= this->morgan_scaling_y;
 180     // options['C']= this->morgan_scaling_z;
 181     options['D']= this->morgan_undefined_min;
 182     options['E']= this->morgan_undefined_max;
 183
 184     return true;
 185 };