src/modules/robot/arm_solutions/RotatableDeltaSolution.cpp

   1 #include "RotatableDeltaSolution.h"
   2 #include "checksumm.h"
   3 #include "ConfigValue.h"
   4 #include "ConfigCache.h"
   5 #include "libs/Kernel.h"
   6 #include "libs/nuts_bolts.h"
   7 #include "libs/Config.h"
   8 #include "libs/utils.h"
   9 #include "StreamOutputPool.h"
  10 #include <fastmath.h>
  11
  12 #define delta_e_checksum                        CHECKSUM("delta_e_checksum")
  13 #define delta_f_checksum                        CHECKSUM("delta_f_checksum")
  14 #define delta_re_checksum                       CHECKSUM("delta_re_checksum")
  15 #define delta_rf_checksum                       CHECKSUM("delta_rf_checksum")
  16 #define delta_z_offset_checksum                 CHECKSUM("delta_z_offset_checksum")
  17
  18 #define delta_ee_offs_checksum                  CHECKSUM("delta_ee_offs_checksum")
  19 #define tool_offset_checksum                    CHECKSUM("tool_offset_checksum")
  20
  21 #define z_home_angle_checksum                   CHECKSUM("z_home_angle_checksum")
  22
  23 #define delta_printable_radius_checksum         CHECKSUM("delta_printable_radius_checksum")
  24
  25 #define xyz_full_steps_per_rotation_checksum    CHECKSUM("xyz_full_steps_per_rotation_checksum")
  26 #define xyz_microsteps_checksum                 CHECKSUM("xyz_microsteps_checksum")
  27 #define small_pulley_teeth_checksum             CHECKSUM("small_pulley_teeth_checksum")
  28 #define big_pulley_teeth_checksum               CHECKSUM("big_pulley_teeth_checksum")
  29
  30 #ifndef alpha_steps_per_mm_checksum
  31   #define alpha_steps_per_mm_checksum           CHECKSUM("alpha_steps_per_mm_checksum");
  32 #endif
  33
  34 #ifndef beta_steps_per_mm_checksum
  35   #define beta_steps_per_mm_checksum            CHECKSUM("beta_steps_per_mm_checksum");
  36 #endif
  37
  38 #ifndef gamma_steps_per_mm_checksum
  39   #define gamma_steps_per_mm_checksum           CHECKSUM("gamma_steps_per_mm_checksum");
  40 #endif
  41
  42 const float pi     = 3.14159265358979323846;    // PI
  43 const float two_pi = 2 * pi;
  44 const float sin120 = 0.86602540378443864676372317075294; //sqrt3/2.0
  45 const float cos120 = -0.5;
  46 const float tan60  = 1.7320508075688772935274463415059; //sqrt3;
  47 const float sin30  = 0.5;
  48 const float tan30  = 0.57735026918962576450914878050196; //1/sqrt3
  49
  50 RotatableDeltaSolution::RotatableDeltaSolution(Config* config)
  51 {
  52         // End effector length
  53         delta_e = config->value((unsigned int)delta_e_checksum)->by_default(131.636F)->as_number();
  54
  55         // Base length
  56         delta_f = config->value((unsigned int)delta_f_checksum)->by_default(190.526F)->as_number();
  57
  58         // Carbon rod length
  59         delta_re = config->value((unsigned int)delta_re_checksum)->by_default(270.000F)->as_number();
  60
  61         // Servo horn length
  62         delta_rf = config->value((unsigned int)delta_rf_checksum)->by_default(90.000F)->as_number();
  63
  64         // Distance from delta 8mm rod/pulley to table/bed,
  65         // NOTE: For OpenPnP, set the zero to be about 25mm above the bed..
  66         delta_z_offset = config->value((unsigned int)delta_z_offset_checksum)->by_default(290.700F)->as_number();
  67
  68         // Ball joint plane to bottom of end effector surface
  69         delta_ee_offs = config->value((unsigned int)delta_ee_offs_checksum)->by_default(15.000F)->as_number();
  70
  71         // Distance between end effector ball joint plane and tip of tool (PnP)
  72         tool_offset = config->value((unsigned int)tool_offset_checksum)->by_default(30.500F)->as_number();
  73
  74         // This is the angle where the arms hit the endstop sensor
  75         z_home_angle = config->value((unsigned int)z_home_angle_checksum)->by_default(-60.000F)->as_number();
  76
  77         // Print surface diameter/2 minus unreachable space (avoid collisions with vertical towers).
  78         delta_printable_radius = config->value((unsigned int)delta_printable_radius_checksum)->by_default(150.0F)->as_number();
  79
  80         init();
  81 }
  82
  83 // inverse kinematics
  84 // helper functions, calculates angle theta1 (for YZ-pane)
  85 int RotatableDeltaSolution::delta_calcAngleYZ(float x0, float y0, float z0, float &theta)
  86 {
  87     float y1 = -0.5F * tan30 * delta_f; // f/2 * tan 30
  88     y0      -=  0.5F * tan30 * delta_e; // shift center to edge
  89     // z = a + b*y
  90     float a = (x0*x0 + y0*y0 + z0*z0 + delta_rf*delta_rf - delta_re*delta_re - y1*y1) / (2.0F*z0);
  91     float b = (y1-y0)/z0;
  92
  93     float d = -(a+b*y1)*(a+b*y1) + delta_rf*(b*b*delta_rf+delta_rf); // discriminant
  94     if (d < 0.0F) return -1;                                            // non-existing point
  95
  96     float yj = (y1 - a*b - sqrtf(d))/(b*b + 1.0F);                     // choosing outer point
  97     float zj = a + b*yj;
  98
  99     theta = 180.0F*atanf(-zj/(y1 - yj))/pi + ((yj>y1)?180.0F:0.0F);
 100     return 0;
 101 }
 102
 103 // forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0)
 104 // returned status: 0=OK, -1=non-existing position
 105 int RotatableDeltaSolution::delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0)
 106 {
 107      float t = (delta_f-delta_e)*tan30/2.0F;
 108      float degrees_to_radians = pi/180.0F;
 109
 110      theta1 *= degrees_to_radians;
 111      theta2 *= degrees_to_radians;
 112      theta3 *= degrees_to_radians;
 113
 114      float y1 = -(t + delta_rf*cosf(theta1));
 115      float z1 = -delta_rf*sinf(theta1);
 116
 117      float y2 = (t + delta_rf*cosf(theta2))*sin30;
 118      float x2 = y2*tan60;
 119      float z2 = -delta_rf*sinf(theta2);
 120
 121      float y3 = (t + delta_rf*cosf(theta3))*sin30;
 122      float x3 = -y3*tan60;
 123      float z3 = -delta_rf*sinf(theta3);
 124
 125      float dnm = (y2-y1)*x3-(y3-y1)*x2;
 126
 127      float w1 = y1*y1 + z1*z1;
 128      float w2 = x2*x2 + y2*y2 + z2*z2;
 129      float w3 = x3*x3 + y3*y3 + z3*z3;
 130
 131      // x = (a1*z + b1)/dnm
 132      float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
 133      float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0F;
 134
 135      // y = (a2*z + b2)/dnm;
 136      float a2 = -(z2-z1)*x3+(z3-z1)*x2;
 137      float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0F;
 138
 139      // a*z^2 + b*z + c = 0
 140      float a = a1*a1 + a2*a2 + dnm*dnm;
 141      float b = 2.0F*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
 142      float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - delta_re*delta_re);
 143
 144      // discriminant
 145      float d = b*b - (float)4.0F*a*c;
 146      if (d < 0.0F) return -1; // non-existing point
 147
 148      z0 = -(float)0.5F*(b+sqrtf(d))/a;
 149      x0 = (a1*z0 + b1)/dnm;
 150      y0 = (a2*z0 + b2)/dnm;
 151
 152      z0 += z_calc_offset; //nj
 153      return 0;
 154 }
 155
 156
 157 void RotatableDeltaSolution::init() {
 158
 159         z_calc_offset  = (delta_z_offset - tool_offset - delta_ee_offs)*-1.0F;
 160
 161     // This is calculated from the Z_HOME_ANGLE angles specified in the config (file), after applying forward
 162     // kinematics, and adding the Z calc offset to it.
 163         z_home_offs    = (((delta_z_offset - tool_offset - delta_ee_offs) - 182.002F) - 0.5F);
 164
 165 }
 166
 167 void RotatableDeltaSolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] )
 168 {
 169         //We need to translate the Cartesian coordinates in mm to the actuator position required in mm so the stepper motor  functions
 170         float alpha_theta = 0.0F;
 171         float beta_theta  = 0.0F;
 172         float gamma_theta = 0.0F;
 173
 174         //Code from Trossen Robotics tutorial, note we put the X axis at the back and not the front of the robot.
 175
 176         float x0 = cartesian_mm[X_AXIS];
 177         float y0 = cartesian_mm[Y_AXIS];
 178         float z_with_offset = cartesian_mm[Z_AXIS] + z_calc_offset; //The delta calculation below places zero at the top.  Subtract the Z offset to make zero at the bottom.
 179
 180         int status =              delta_calcAngleYZ(x0,                    y0,                  z_with_offset, alpha_theta);
 181         if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z_with_offset, beta_theta);  // rotate co-ordinates to +120 deg
 182         if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z_with_offset, gamma_theta);  // rotate co-ordinates to -120 deg
 183
 184         if (status == -1)  //something went wrong
 185         {
 186             THEKERNEL->streams->printf("ERROR: Delta calculation fail!  Unable to move to:\n");
 187             THEKERNEL->streams->printf("    x= %f\n",cartesian_mm[X_AXIS]);
 188             THEKERNEL->streams->printf("    y= %f\n",cartesian_mm[Y_AXIS]);
 189             THEKERNEL->streams->printf("    z= %f\n",cartesian_mm[Z_AXIS]);
 190             THEKERNEL->streams->printf(" CalcZ= %f\n",z_calc_offset);
 191             THEKERNEL->streams->printf(" Offz= %f\n",z_with_offset);
 192           }
 193           else
 194           {
 195             actuator_mm[ALPHA_STEPPER] = alpha_theta;
 196             actuator_mm[BETA_STEPPER ] = beta_theta;
 197             actuator_mm[GAMMA_STEPPER] = gamma_theta;
 198
 199 //                THEKERNEL->streams->printf("cartesian x= %f\n\r",cartesian_mm[X_AXIS]);
 200 //                THEKERNEL->streams->printf(" y= %f\n\r",cartesian_mm[Y_AXIS]);
 201 //                THEKERNEL->streams->printf(" z= %f\n\r",cartesian_mm[Z_AXIS]);
 202 //                THEKERNEL->streams->printf(" Offz= %f\n\r",z_with_offset);
 203 //                THEKERNEL->streams->printf(" delta x= %f\n\r",delta[X_AXIS]);
 204 //                THEKERNEL->streams->printf(" y= %f\n\r",delta[Y_AXIS]);
 205 //                THEKERNEL->streams->printf(" z= %f\n\r",delta[Z_AXIS]);
 206           }
 207
 208 }
 209
 210 void RotatableDeltaSolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] )
 211 {
 212     //Use forward kinematics
 213     delta_calcForward(actuator_mm[ALPHA_STEPPER], actuator_mm[BETA_STEPPER ], actuator_mm[GAMMA_STEPPER], cartesian_mm[X_AXIS], cartesian_mm[Y_AXIS], cartesian_mm[Z_AXIS]);
 214 }
 215
 216 bool RotatableDeltaSolution::set_optional(const arm_options_t& options) {
 217
 218     for(auto &i : options) {
 219         switch(i.first) {
 220             case 'A': delta_e                   = i.second; break;
 221             case 'B': delta_f                   = i.second; break;
 222             case 'C': delta_re                  = i.second; break;
 223             case 'D': delta_rf                  = i.second; break;
 224             case 'E': delta_z_offset            = i.second; break;
 225             case 'F': delta_ee_offs             = i.second; break;
 226             case 'G': tool_offset               = i.second; break;
 227             case 'H': z_home_angle              = i.second; break;
 228             case 'I': delta_printable_radius    = i.second; break;
 229         }
 230     }
 231     init();
 232     return true;
 233 }
 234
 235 bool RotatableDeltaSolution::get_optional(arm_options_t& options) {
 236
 237     // don't report these if none of them are set
 238     if(this->delta_e     != 0.0F || this->delta_f        != 0.0F || this->delta_re               != 0.0F ||
 239        this->delta_rf    != 0.0F || this->delta_z_offset != 0.0F || this->delta_ee_offs          != 0.0F ||
 240        this->tool_offset != 0.0F || this->z_home_angle   != 0.0F || this->delta_printable_radius != 0.0F) {
 241
 242         options['A'] = this->delta_e;
 243         options['B'] = this->delta_f;
 244         options['C'] = this->delta_re;
 245         options['D'] = this->delta_rf;
 246         options['E'] = this->delta_z_offset;
 247         options['F'] = this->delta_ee_offs;
 248         options['G'] = this->tool_offset;
 249         options['H'] = this->z_home_angle;
 250         options['I'] = this->delta_printable_radius;
 251     }
 252
 253     return true;
 254 };
 255