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 #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     // morgan_undefined is the ratio at which the SCARA position is undefined.
  41     // required to prevent the arm moving through singularity points
  42     // min: head close to tower
  43     morgan_undefined_min  = config->value(morgan_undefined_min_checksum)->by_default(0.95f)->as_number();
  44     // max: head on maximum reach
  45     morgan_undefined_max  = config->value(morgan_undefined_max_checksum)->by_default(0.95f)->as_number();
  46
  47     init();
  48 }
  49
  50 void MorganSCARASolution::init() {
  51
  52 }
  53
  54 float MorganSCARASolution::to_degrees(float radians) {
  55     return radians*(180.0F/3.14159265359f);
  56 }
  57
  58 void MorganSCARASolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] )
  59 {
  60
  61     float SCARA_pos[2],
  62           SCARA_C2,
  63           SCARA_S2,
  64           SCARA_K1,
  65           SCARA_K2,
  66           SCARA_theta,
  67           SCARA_psi;
  68
  69     SCARA_pos[X_AXIS] = cartesian_mm[X_AXIS] - this->morgan_offset_x;  //Translate cartesian to tower centric SCARA X Y
  70     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.
  71
  72     if (this->arm1_length == this->arm2_length)
  73         SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-2.0f*SQ(this->arm1_length)) / (2.0f * SQ(this->arm1_length));
  74     else
  75         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));
  76
  77     // SCARA position is undefined if abs(SCARA_C2) >=1
  78     // In reality abs(SCARA_C2) >0.95 is problematic.
  79
  80     if (SCARA_C2 > this->morgan_undefined_max)
  81         SCARA_C2 = this->morgan_undefined_max;
  82     else if (SCARA_C2 < -this->morgan_undefined_min)
  83         SCARA_C2 = -this->morgan_undefined_min;
  84
  85
  86     SCARA_S2 = sqrtf(1.0f-SQ(SCARA_C2));
  87
  88     SCARA_K1 = this->arm1_length+this->arm2_length*SCARA_C2;
  89     SCARA_K2 = this->arm2_length*SCARA_S2;
  90
  91     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
  92     SCARA_psi   = atan2f(SCARA_S2,SCARA_C2);
  93
  94
  95     actuator_mm[ALPHA_STEPPER] = to_degrees(SCARA_theta);             // Multiply by 180/Pi  -  theta is support arm angle
  96     actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_theta + SCARA_psi); // Morgan kinematics (dual arm)
  97     //actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_psi);             // real scara
  98     actuator_mm[GAMMA_STEPPER] = cartesian_mm[Z_AXIS];                // No inverse kinematics on Z - Position to add bed offset?
  99
 100 }
 101
 102 void MorganSCARASolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ) {
 103     // Perform forward kinematics, and place results in cartesian_mm[]
 104
 105     float y1, y2,
 106            actuator_rad[2];
 107
 108     actuator_rad[X_AXIS] = actuator_mm[X_AXIS]/(180.0F/3.14159265359f);
 109     actuator_rad[Y_AXIS] = actuator_mm[Y_AXIS]/(180.0F/3.14159265359f);
 110
 111     y1 = sinf(actuator_rad[X_AXIS])*this->arm1_length;
 112     y2 = sinf(actuator_rad[Y_AXIS])*this->arm2_length + y1;
 113
 114     cartesian_mm[X_AXIS] = cosf(actuator_rad[X_AXIS])*this->arm1_length + cosf(actuator_rad[Y_AXIS])*this->arm2_length + this->morgan_offset_x;
 115     cartesian_mm[Y_AXIS] = y2 + this->morgan_offset_y;
 116     cartesian_mm[Z_AXIS] = actuator_mm[Z_AXIS];
 117
 118     cartesian_mm[0] = ROUND(cartesian_mm[0], 4);
 119     cartesian_mm[1] = ROUND(cartesian_mm[1], 4);
 120     cartesian_mm[2] = ROUND(cartesian_mm[2], 4);
 121 }
 122
 123 bool MorganSCARASolution::set_optional(const arm_options_t& options) {
 124
 125     arm_options_t::const_iterator i;
 126
 127     i= options.find('T');          // Theta arm1 length
 128     if(i != options.end()) {
 129         arm1_length= i->second;
 130
 131     }
 132     i= options.find('P');          // Psi arm2 length
 133     if(i != options.end()) {
 134         arm2_length= i->second;
 135     }
 136     i= options.find('X');          // Home initial position X
 137     if(i != options.end()) {
 138         morgan_offset_x= i->second;
 139     }
 140     i= options.find('Y');          // Home initial position Y
 141     if(i != options.end()) {
 142         morgan_offset_y= i->second;
 143     }
 144
 145     init();
 146     return true;
 147 }
 148
 149 bool MorganSCARASolution::get_optional(arm_options_t& options) {
 150     options['T']= this->arm1_length;
 151     options['P']= this->arm2_length;
 152     options['X']= this->morgan_offset_x;
 153     options['Y']= this->morgan_offset_y;
 154     return true;
 155 };