src/modules/robot/Robot.cpp

   1 /*
   2       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl) with additions from Sungeun K. Jeon (https://github.com/chamnit/grbl)
   3       Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
   4       Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
   5       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
   6 */
   7
   8 #include "libs/Module.h"
   9 #include "libs/Kernel.h"
  10 #include <string>
  11 using std::string;
  12 #include <math.h>
  13 #include "Planner.h"
  14 #include "Conveyor.h"
  15 #include "Robot.h"
  16 #include "libs/nuts_bolts.h"
  17 #include "libs/Pin.h"
  18 #include "libs/StepperMotor.h"
  19 #include "../communication/utils/Gcode.h"
  20 #include "arm_solutions/BaseSolution.h"
  21 #include "arm_solutions/CartesianSolution.h"
  22 #include "arm_solutions/RotatableCartesianSolution.h"
  23 #include "arm_solutions/RostockSolution.h"
  24
  25 Robot::Robot(){
  26     this->inch_mode = false;
  27     this->absolute_mode = true;
  28     this->motion_mode =  MOTION_MODE_SEEK;
  29     this->select_plane(X_AXIS, Y_AXIS, Z_AXIS);
  30     clear_vector(this->current_position);
  31     clear_vector(this->last_milestone);
  32     this->arm_solution = NULL;
  33     seconds_per_minute = 60.0;
  34 }
  35
  36 //Called when the module has just been loaded
  37 void Robot::on_module_loaded() {
  38     register_for_event(ON_CONFIG_RELOAD);
  39     this->register_for_event(ON_GCODE_RECEIVED);
  40
  41     // Configuration
  42     this->on_config_reload(this);
  43
  44     // Make our 3 StepperMotors
  45     this->alpha_stepper_motor  = this->kernel->step_ticker->add_stepper_motor( new StepperMotor(&alpha_step_pin,&alpha_dir_pin,&alpha_en_pin) );
  46     this->beta_stepper_motor   = this->kernel->step_ticker->add_stepper_motor( new StepperMotor(&beta_step_pin, &beta_dir_pin, &beta_en_pin ) );
  47     this->gamma_stepper_motor  = this->kernel->step_ticker->add_stepper_motor( new StepperMotor(&gamma_step_pin,&gamma_dir_pin,&gamma_en_pin) );
  48
  49 }
  50
  51 void Robot::on_config_reload(void* argument){
  52     if (this->arm_solution) delete this->arm_solution;
  53     int solution_checksum = get_checksum(this->kernel->config->value(arm_solution_checksum)->by_default("cartesian")->as_string());
  54
  55     // Note checksums are not const expressions when in debug mode, so don't use switch
  56     if(solution_checksum == rostock_checksum) {
  57         this->arm_solution = new RostockSolution(this->kernel->config);
  58
  59     }else if(solution_checksum ==  delta_checksum) {
  60         // place holder for now
  61         this->arm_solution = new RostockSolution(this->kernel->config);
  62
  63     }else if(solution_checksum == rotatable_cartesian_checksum) {
  64         this->arm_solution = new RotatableCartesianSolution(this->kernel->config);
  65
  66     }else if(solution_checksum == cartesian_checksum) {
  67         this->arm_solution = new CartesianSolution(this->kernel->config);
  68
  69     }else{
  70         this->arm_solution = new CartesianSolution(this->kernel->config);
  71     }
  72
  73
  74     this->feed_rate           = this->kernel->config->value(default_feed_rate_checksum   )->by_default(100    )->as_number() / 60;
  75     this->seek_rate           = this->kernel->config->value(default_seek_rate_checksum   )->by_default(100    )->as_number() / 60;
  76     this->mm_per_line_segment = this->kernel->config->value(mm_per_line_segment_checksum )->by_default(0.0    )->as_number();
  77     this->delta_segments_per_second = this->kernel->config->value(delta_segments_per_second_checksum )->by_default(0.0    )->as_number();
  78     this->mm_per_arc_segment  = this->kernel->config->value(mm_per_arc_segment_checksum  )->by_default(0.5    )->as_number();
  79     this->arc_correction      = this->kernel->config->value(arc_correction_checksum      )->by_default(5      )->as_number();
  80     this->max_speeds[X_AXIS]  = this->kernel->config->value(x_axis_max_speed_checksum    )->by_default(60000  )->as_number();
  81     this->max_speeds[Y_AXIS]  = this->kernel->config->value(y_axis_max_speed_checksum    )->by_default(60000  )->as_number();
  82     this->max_speeds[Z_AXIS]  = this->kernel->config->value(z_axis_max_speed_checksum    )->by_default(300    )->as_number();
  83     this->alpha_step_pin.from_string( this->kernel->config->value(alpha_step_pin_checksum )->by_default("2.0"  )->as_string())->as_output();
  84     this->alpha_dir_pin.from_string(  this->kernel->config->value(alpha_dir_pin_checksum  )->by_default("0.5"  )->as_string())->as_output();
  85     this->alpha_en_pin.from_string(   this->kernel->config->value(alpha_en_pin_checksum   )->by_default("0.4"  )->as_string())->as_output()->as_open_drain();
  86     this->beta_step_pin.from_string(  this->kernel->config->value(beta_step_pin_checksum  )->by_default("2.1"  )->as_string())->as_output();
  87     this->gamma_step_pin.from_string( this->kernel->config->value(gamma_step_pin_checksum )->by_default("2.2"  )->as_string())->as_output();
  88     this->gamma_dir_pin.from_string(  this->kernel->config->value(gamma_dir_pin_checksum  )->by_default("0.20" )->as_string())->as_output();
  89     this->gamma_en_pin.from_string(   this->kernel->config->value(gamma_en_pin_checksum   )->by_default("0.19" )->as_string())->as_output()->as_open_drain();
  90     this->beta_dir_pin.from_string(   this->kernel->config->value(beta_dir_pin_checksum   )->by_default("0.11" )->as_string())->as_output();
  91     this->beta_en_pin.from_string(    this->kernel->config->value(beta_en_pin_checksum    )->by_default("0.10" )->as_string())->as_output()->as_open_drain();
  92
  93 }
  94
  95 //A GCode has been received
  96 void Robot::on_gcode_received(void * argument){
  97     Gcode* gcode = static_cast<Gcode*>(argument);
  98     gcode->call_on_gcode_execute_event_immediatly = false;
  99     gcode->on_gcode_execute_event_called = false;
 100     //If the queue is empty, execute immediatly, otherwise attach to the last added block
 101     if( this->kernel->conveyor->queue.size() == 0 ){
 102         gcode->call_on_gcode_execute_event_immediatly = true;
 103         this->execute_gcode(gcode);
 104         if( gcode->on_gcode_execute_event_called == false ){
 105             //printf("GCODE A: %s \r\n", gcode->command.c_str() );
 106             this->kernel->call_event(ON_GCODE_EXECUTE, gcode );
 107         }
 108     }else{
 109         Block* block = this->kernel->conveyor->queue.get_ref( this->kernel->conveyor->queue.size() - 1 );
 110         this->execute_gcode(gcode);
 111         block->append_gcode(gcode);
 112         gcode->queued++;
 113     }
 114 }
 115
 116
 117 void Robot::reset_axis_position(double position, int axis) {
 118     this->last_milestone[axis] = this->current_position[axis] = position;
 119     this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position);
 120 }
 121
 122
 123 //See if the current Gcode line has some orders for us
 124 void Robot::execute_gcode(Gcode* gcode){
 125
 126     //Temp variables, constant properties are stored in the object
 127     uint8_t next_action = NEXT_ACTION_DEFAULT;
 128     this->motion_mode = -1;
 129
 130    //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
 131     if( gcode->has_letter('G')){
 132         switch( (int) gcode->get_value('G') ){
 133             case 0:  this->motion_mode = MOTION_MODE_SEEK; break;
 134             case 1:  this->motion_mode = MOTION_MODE_LINEAR; break;
 135             case 2:  this->motion_mode = MOTION_MODE_CW_ARC; break;
 136             case 3:  this->motion_mode = MOTION_MODE_CCW_ARC; break;
 137             case 17: this->select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
 138             case 18: this->select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
 139             case 19: this->select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
 140             case 20: this->inch_mode = true; break;
 141             case 21: this->inch_mode = false; break;
 142             case 90: this->absolute_mode = true; break;
 143             case 91: this->absolute_mode = false; break;
 144             case 92: {
 145                 if(gcode->get_num_args() == 0){
 146                     clear_vector(this->last_milestone);
 147                 }else{
 148                     for (char letter = 'X'; letter <= 'Z'; letter++){
 149                         if ( gcode->has_letter(letter) )
 150                             this->last_milestone[letter-'X'] = this->to_millimeters(gcode->get_value(letter));
 151                     }
 152                 }
 153                 memcpy(this->current_position, this->last_milestone, sizeof(double)*3); // current_position[] = last_milestone[];
 154                 this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position);
 155                 return; // TODO: Wait until queue empty
 156            }
 157        }
 158    }else if( gcode->has_letter('M')){
 159      switch( (int) gcode->get_value('M') ){
 160             case 92: // M92 - set steps per mm
 161                 double steps[3];
 162                 this->arm_solution->get_steps_per_millimeter(steps);
 163                 if (gcode->has_letter('X'))
 164                     steps[0] = this->to_millimeters(gcode->get_value('X'));
 165                 if (gcode->has_letter('Y'))
 166                     steps[1] = this->to_millimeters(gcode->get_value('Y'));
 167                 if (gcode->has_letter('Z'))
 168                     steps[2] = this->to_millimeters(gcode->get_value('Z'));
 169                 if (gcode->has_letter('F'))
 170                     seconds_per_minute = gcode->get_value('F');
 171                 this->arm_solution->set_steps_per_millimeter(steps);
 172                 // update current position in steps
 173                 this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position);
 174                 gcode->stream->printf("X:%g Y:%g Z:%g F:%g ", steps[0], steps[1], steps[2], seconds_per_minute);
 175                 gcode->add_nl = true;
 176                 return;
 177             case 114: gcode->stream->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
 178                                                  this->current_position[0],
 179                                                  this->current_position[1],
 180                                                  this->current_position[2]);
 181                 gcode->add_nl = true;
 182                 return;
 183             case 220: // M220 - speed override percentage
 184                 if (gcode->has_letter('S'))
 185                 {
 186                     double factor = gcode->get_value('S');
 187                     // enforce minimum 1% speed
 188                     if (factor < 1.0)
 189                         factor = 1.0;
 190                     seconds_per_minute = factor * 0.6;
 191                 }
 192         }
 193    }
 194     if( this->motion_mode < 0)
 195         return;
 196
 197    //Get parameters
 198     double target[3], offset[3];
 199     clear_vector(target); clear_vector(offset);
 200
 201     memcpy(target, this->current_position, sizeof(target));    //default to last target
 202
 203     for(char letter = 'I'; letter <= 'K'; letter++){ if( gcode->has_letter(letter) ){ offset[letter-'I'] = this->to_millimeters(gcode->get_value(letter));                                                    } }
 204     for(char letter = 'X'; letter <= 'Z'; letter++){ if( gcode->has_letter(letter) ){ target[letter-'X'] = this->to_millimeters(gcode->get_value(letter)) + ( this->absolute_mode ? 0 : target[letter-'X']);  } }
 205
 206     if( gcode->has_letter('F') )
 207     {
 208         if( this->motion_mode == MOTION_MODE_SEEK )
 209             this->seek_rate = this->to_millimeters( gcode->get_value('F') ) / 60.0;
 210         else
 211             this->feed_rate = this->to_millimeters( gcode->get_value('F') ) / 60.0;
 212     }
 213
 214     //Perform any physical actions
 215     switch( next_action ){
 216         case NEXT_ACTION_DEFAULT:
 217             switch(this->motion_mode){
 218                 case MOTION_MODE_CANCEL: break;
 219                 case MOTION_MODE_SEEK  : this->append_line(gcode, target, this->seek_rate ); break;
 220                 case MOTION_MODE_LINEAR: this->append_line(gcode, target, this->feed_rate ); break;
 221                 case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: this->compute_arc(gcode, offset, target ); break;
 222             }
 223             break;
 224     }
 225
 226     // As far as the parser is concerned, the position is now == target. In reality the
 227     // motion control system might still be processing the action and the real tool position
 228     // in any intermediate location.
 229     memcpy(this->current_position, target, sizeof(double)*3); // this->position[] = target[];
 230
 231 }
 232
 233 // Convert target from millimeters to steps, and append this to the planner
 234 void Robot::append_milestone( double target[], double rate ){
 235     int steps[3]; //Holds the result of the conversion
 236
 237     this->arm_solution->millimeters_to_steps( target, steps );
 238
 239     double deltas[3];
 240     for(int axis=X_AXIS;axis<=Z_AXIS;axis++){deltas[axis]=target[axis]-this->last_milestone[axis];}
 241
 242
 243     double millimeters_of_travel = sqrt( pow( deltas[X_AXIS], 2 ) +  pow( deltas[Y_AXIS], 2 ) +  pow( deltas[Z_AXIS], 2 ) );
 244
 245     for(int axis=X_AXIS;axis<=Z_AXIS;axis++){
 246         if( this->max_speeds[axis] > 0 ){
 247             double axis_speed = ( fabs(deltas[axis]) / ( millimeters_of_travel / rate )) * seconds_per_minute;
 248             if( axis_speed > this->max_speeds[axis] ){
 249                 rate = rate * ( this->max_speeds[axis] / axis_speed );
 250             }
 251         }
 252     }
 253
 254     this->kernel->planner->append_block( steps, rate * seconds_per_minute, millimeters_of_travel, deltas );
 255
 256     memcpy(this->last_milestone, target, sizeof(double)*3); // this->last_milestone[] = target[];
 257
 258 }
 259
 260 void Robot::append_line(Gcode* gcode, double target[], double rate ){
 261
 262     gcode->millimeters_of_travel = sqrt( pow( target[X_AXIS]-this->current_position[X_AXIS], 2 ) +  pow( target[Y_AXIS]-this->current_position[Y_AXIS], 2 ) +  pow( target[Z_AXIS]-this->current_position[Z_AXIS], 2 ) );
 263
 264     if( gcode->call_on_gcode_execute_event_immediatly == true ){
 265             //printf("GCODE B: %s \r\n", gcode->command.c_str() );
 266             this->kernel->call_event(ON_GCODE_EXECUTE, gcode );
 267             gcode->on_gcode_execute_event_called = true;
 268     }
 269
 270     if (gcode->millimeters_of_travel == 0.0) {
 271         this->append_milestone(this->current_position, 0.0);
 272         return;
 273     }
 274
 275     // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
 276     // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
 277     // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second The latter is more efficient and avoids splitting fast long lines into very small segments, like initial z move to 0, it is what Johanns Marlin delta port does
 278
 279     uint16_t segments;
 280
 281     if(this->delta_segments_per_second > 1.0) {
 282         // enabled if set to something > 1, it is set to 0.0 by default
 283         // segment based on current speed and requested segments per second
 284         // the faster the travel speed the fewer segments needed
 285         // NOTE rate is mm/sec and we take into account any speed override
 286         float seconds = 60.0/seconds_per_minute * gcode->millimeters_of_travel / rate;
 287         segments= max(1, ceil(this->delta_segments_per_second * seconds));
 288         // TODO if we are only moving in Z on a delta we don't really need to segment at all
 289
 290     }else{
 291         if(this->mm_per_line_segment == 0.0){
 292             segments= 1; // don't split it up
 293         }else{
 294             segments = ceil( gcode->millimeters_of_travel/ this->mm_per_line_segment);
 295         }
 296     }
 297
 298     // A vector to keep track of the endpoint of each segment
 299     double temp_target[3];
 300     //Initialize axes
 301     memcpy( temp_target, this->current_position, sizeof(double)*3); // temp_target[] = this->current_position[];
 302
 303     //For each segment
 304     for( int i=0; i<segments-1; i++ ){
 305         for(int axis=X_AXIS; axis <= Z_AXIS; axis++ ){ temp_target[axis] += ( target[axis]-this->current_position[axis] )/segments; }
 306         this->append_milestone(temp_target, rate);
 307     }
 308     this->append_milestone(target, rate);
 309 }
 310
 311
 312 void Robot::append_arc(Gcode* gcode, double target[], double offset[], double radius, bool is_clockwise ){
 313
 314     double center_axis0 = this->current_position[this->plane_axis_0] + offset[this->plane_axis_0];
 315     double center_axis1 = this->current_position[this->plane_axis_1] + offset[this->plane_axis_1];
 316     double linear_travel = target[this->plane_axis_2] - this->current_position[this->plane_axis_2];
 317     double r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
 318     double r_axis1 = -offset[this->plane_axis_1];
 319     double rt_axis0 = target[this->plane_axis_0] - center_axis0;
 320     double rt_axis1 = target[this->plane_axis_1] - center_axis1;
 321
 322     // CCW angle between position and target from circle center. Only one atan2() trig computation required.
 323     double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
 324     if (angular_travel < 0) { angular_travel += 2*M_PI; }
 325     if (is_clockwise) { angular_travel -= 2*M_PI; }
 326
 327     gcode->millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
 328
 329     if( gcode->call_on_gcode_execute_event_immediatly == true ){
 330             //printf("GCODE C: %s \r\n", gcode->command.c_str() );
 331             this->kernel->call_event(ON_GCODE_EXECUTE, gcode );
 332             gcode->on_gcode_execute_event_called = true;
 333     }
 334
 335     if (gcode->millimeters_of_travel == 0.0) {
 336         this->append_milestone(this->current_position, 0.0);
 337         return;
 338     }
 339
 340     uint16_t segments = floor(gcode->millimeters_of_travel/this->mm_per_arc_segment);
 341
 342     double theta_per_segment = angular_travel/segments;
 343     double linear_per_segment = linear_travel/segments;
 344
 345     /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
 346     and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
 347     r_T = [cos(phi) -sin(phi);
 348     sin(phi) cos(phi] * r ;
 349     For arc generation, the center of the circle is the axis of rotation and the radius vector is
 350     defined from the circle center to the initial position. Each line segment is formed by successive
 351     vector rotations. This requires only two cos() and sin() computations to form the rotation
 352     matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
 353     all double numbers are single precision on the Arduino. (True double precision will not have
 354     round off issues for CNC applications.) Single precision error can accumulate to be greater than
 355     tool precision in some cases. Therefore, arc path correction is implemented.
 356
 357     Small angle approximation may be used to reduce computation overhead further. This approximation
 358     holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
 359     theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
 360     to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
 361     numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
 362     issue for CNC machines with the single precision Arduino calculations.
 363     This approximation also allows mc_arc to immediately insert a line segment into the planner
 364     without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
 365     a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
 366     This is important when there are successive arc motions.
 367     */
 368     // Vector rotation matrix values
 369     double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
 370     double sin_T = theta_per_segment;
 371
 372     double arc_target[3];
 373     double sin_Ti;
 374     double cos_Ti;
 375     double r_axisi;
 376     uint16_t i;
 377     int8_t count = 0;
 378
 379     // Initialize the linear axis
 380     arc_target[this->plane_axis_2] = this->current_position[this->plane_axis_2];
 381
 382     for (i = 1; i<segments; i++) { // Increment (segments-1)
 383
 384         if (count < this->arc_correction ) {
 385           // Apply vector rotation matrix
 386           r_axisi = r_axis0*sin_T + r_axis1*cos_T;
 387           r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
 388           r_axis1 = r_axisi;
 389           count++;
 390         } else {
 391           // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
 392           // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
 393           cos_Ti = cos(i*theta_per_segment);
 394           sin_Ti = sin(i*theta_per_segment);
 395           r_axis0 = -offset[this->plane_axis_0]*cos_Ti + offset[this->plane_axis_1]*sin_Ti;
 396           r_axis1 = -offset[this->plane_axis_0]*sin_Ti - offset[this->plane_axis_1]*cos_Ti;
 397           count = 0;
 398         }
 399
 400         // Update arc_target location
 401         arc_target[this->plane_axis_0] = center_axis0 + r_axis0;
 402         arc_target[this->plane_axis_1] = center_axis1 + r_axis1;
 403         arc_target[this->plane_axis_2] += linear_per_segment;
 404         this->append_milestone(arc_target, this->feed_rate);
 405
 406     }
 407     // Ensure last segment arrives at target location.
 408     this->append_milestone(target, this->feed_rate);
 409 }
 410
 411
 412 void Robot::compute_arc(Gcode* gcode, double offset[], double target[]){
 413
 414     // Find the radius
 415     double radius = hypot(offset[this->plane_axis_0], offset[this->plane_axis_1]);
 416
 417     // Set clockwise/counter-clockwise sign for mc_arc computations
 418     bool is_clockwise = false;
 419     if( this->motion_mode == MOTION_MODE_CW_ARC ){ is_clockwise = true; }
 420
 421     // Append arc
 422     this->append_arc(gcode, target, offset,  radius, is_clockwise );
 423
 424 }
 425
 426
 427 // Convert from inches to millimeters ( our internal storage unit ) if needed
 428 inline double Robot::to_millimeters( double value ){
 429         return this->inch_mode ? value/25.4 : value;
 430 }
 431
 432 double Robot::theta(double x, double y){
 433     double t = atan(x/fabs(y));
 434     if (y>0) {return(t);} else {if (t>0){return(M_PI-t);} else {return(-M_PI-t);}}
 435 }
 436
 437 void Robot::select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2){
 438     this->plane_axis_0 = axis_0;
 439     this->plane_axis_1 = axis_1;
 440     this->plane_axis_2 = axis_2;
 441 }
 442
 443