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 "mbed.h"
  13 #include <math.h>
  14 #include "Planner.h"
  15 #include "Robot.h"
  16 #include "libs/nuts_bolts.h"
  17 #include "../communication/utils/Gcode.h"
  18 #include "arm_solutions/BaseSolution.h"
  19 #include "arm_solutions/CartesianSolution.h"
  20
  21 Robot::Robot(){
  22     this->inch_mode = false;
  23     this->absolute_mode = false;
  24     this->motion_mode =  MOTION_MODE_SEEK;
  25     this->select_plane(X_AXIS, Y_AXIS, Z_AXIS);
  26     clear_vector(this->current_position);
  27     clear_vector(this->last_milestone);
  28 }
  29
  30 //Called when the module has just been loaded
  31 void Robot::on_module_loaded() {
  32     this->arm_solution = new CartesianSolution(this->kernel->config);
  33     this->register_for_event(ON_GCODE_RECEIVED);
  34
  35     // Configuration
  36     this->on_config_reload(this);
  37 }
  38
  39 void Robot::on_config_reload(void* argument){
  40     this->feed_rate =           this->kernel->config->value(default_feed_rate_checksum  )->by_default(100)->as_number()/60;
  41     this->seek_rate =           this->kernel->config->value(default_seek_rate_checksum  )->by_default(100)->as_number()/60;
  42     this->mm_per_line_segment = this->kernel->config->value(mm_per_line_segment_checksum)->by_default(0.1)->as_number();
  43     this->mm_per_arc_segment  = this->kernel->config->value(mm_per_arc_segment_checksum )->by_default(10 )->as_number();
  44     this->arc_correction      = this->kernel->config->value(arc_correction_checksum     )->by_default(5  )->as_number();
  45 }
  46
  47 //A GCode has been received
  48 void Robot::on_gcode_received(void * argument){
  49     Gcode* gcode = static_cast<Gcode*>(argument);
  50     this->kernel->planner->attach_gcode_to_queue(gcode);
  51     this->execute_gcode(gcode);
  52 }
  53
  54 //See if the current Gcode line has some orders for us
  55 void Robot::execute_gcode(Gcode* gcode){
  56
  57     //Temp variables, constant properties are stored in the object
  58     uint8_t next_action = NEXT_ACTION_DEFAULT;
  59     this->motion_mode = -1;
  60
  61    //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
  62    if( gcode->has_letter('G')){
  63        switch( (int) gcode->get_value('G') ){
  64            case 0: this->motion_mode = MOTION_MODE_SEEK; break;
  65            case 1: this->motion_mode = MOTION_MODE_LINEAR; break;
  66            case 2: this->motion_mode = MOTION_MODE_CW_ARC; break;
  67            case 3: this->motion_mode = MOTION_MODE_CCW_ARC; break;
  68            case 17: this->select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
  69            case 18: this->select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
  70            case 19: this->select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
  71            case 20:this->inch_mode = true; break;
  72            case 21:this->inch_mode = false; break;
  73            case 90:this->absolute_mode = true; break;
  74            case 91:this->absolute_mode = false; break;
  75        }
  76     }else{ return; }
  77
  78    //Get parameters
  79     double target[3], offset[3];
  80     clear_vector(target); clear_vector(offset);
  81
  82     memcpy(target, this->current_position, sizeof(target));    //default to last target
  83
  84     for(char letter = 'I'; letter <= 'K'; letter++){ if( gcode->has_letter(letter) ){ offset[letter-'I'] = this->to_millimeters(gcode->get_value(letter));                                                    } }
  85     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']);  } }
  86
  87     if( gcode->has_letter('F') ){ if( this->motion_mode == MOTION_MODE_SEEK ){ this->seek_rate = this->to_millimeters( gcode->get_value('F') ) / 60; }else{ this->feed_rate = this->to_millimeters( gcode->get_value('F') ) / 60; } }
  88
  89     //Perform any physical actions
  90     switch( next_action ){
  91         case NEXT_ACTION_DEFAULT:
  92             switch(this->motion_mode){
  93                 case MOTION_MODE_CANCEL: break;
  94                 case MOTION_MODE_SEEK  : this->append_line(target, this->seek_rate ); break;
  95                 case MOTION_MODE_LINEAR: this->append_line(target, this->feed_rate ); break;
  96                 case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: this->compute_arc(offset, target ); break;
  97             }
  98             break;
  99     }
 100     // As far as the parser is concerned, the position is now == target. In reality the
 101     // motion control system might still be processing the action and the real tool position
 102     // in any intermediate location.
 103     memcpy(this->current_position, target, sizeof(double)*3); // this->position[] = target[];
 104
 105 }
 106
 107 // Convert target from millimeters to steps, and append this to the planner
 108 void Robot::append_milestone( double target[], double rate ){
 109     int steps[3]; //Holds the result of the conversion
 110
 111     this->arm_solution->millimeters_to_steps( target, steps );
 112
 113     double deltas[3];
 114     for(int axis=X_AXIS;axis<=Z_AXIS;axis++){deltas[axis]=target[axis]-this->last_milestone[axis];}
 115
 116
 117     double millimeters_of_travel = sqrt( pow( deltas[X_AXIS], 2 ) +  pow( deltas[Y_AXIS], 2 ) +  pow( deltas[Z_AXIS], 2 ) );
 118     if( millimeters_of_travel < 0.001 ){ return; }
 119     double duration = millimeters_of_travel / rate;
 120     //this->kernel->serial->printf("dur: %f mm: %f rate: %f target_z: %f steps_z: %d deltas_z: %f \r\n", duration, millimeters_of_travel, rate, target[2], steps[2], deltas[2] );
 121
 122     this->kernel->planner->append_block( steps, rate*60, millimeters_of_travel, deltas );
 123
 124     memcpy(this->last_milestone, target, sizeof(double)*3); // this->last_milestone[] = target[];
 125
 126 }
 127
 128 void Robot::append_line(double target[], double rate ){
 129
 130
 131     // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
 132     // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
 133     double 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 ) );
 134     if( millimeters_of_travel == 0.0 ){ return; }
 135
 136     uint16_t segments = ceil( millimeters_of_travel/ this->mm_per_line_segment);
 137     // A vector to keep track of the endpoint of each segment
 138     double temp_target[3];
 139     //Initialize axes
 140     memcpy( temp_target, this->current_position, sizeof(double)*3); // temp_target[] = this->current_position[];
 141
 142     //For each segment
 143     for( int i=0; i<segments-1; i++ ){
 144         for(int axis=X_AXIS; axis <= Z_AXIS; axis++ ){ temp_target[axis] += ( target[axis]-this->current_position[axis] )/segments; }
 145         this->append_milestone(temp_target, rate);
 146     }
 147     this->append_milestone(target, rate);
 148 }
 149
 150
 151 void Robot::append_arc( double target[], double offset[], double radius, bool is_clockwise ){
 152
 153     double center_axis0 = this->current_position[this->plane_axis_0] + offset[this->plane_axis_0];
 154     double center_axis1 = this->current_position[this->plane_axis_1] + offset[this->plane_axis_1];
 155     double linear_travel = target[this->plane_axis_2] - this->current_position[this->plane_axis_2];
 156     double r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
 157     double r_axis1 = -offset[this->plane_axis_1];
 158     double rt_axis0 = target[this->plane_axis_0] - center_axis0;
 159     double rt_axis1 = target[this->plane_axis_1] - center_axis1;
 160
 161     // CCW angle between position and target from circle center. Only one atan2() trig computation required.
 162     double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
 163     if (angular_travel < 0) { angular_travel += 2*M_PI; }
 164     if (is_clockwise) { angular_travel -= 2*M_PI; }
 165
 166     double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
 167     if (millimeters_of_travel == 0.0) { return; }
 168     uint16_t segments = floor(millimeters_of_travel/this->mm_per_arc_segment);
 169
 170     double theta_per_segment = angular_travel/segments;
 171     double linear_per_segment = linear_travel/segments;
 172
 173     /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
 174     and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
 175     r_T = [cos(phi) -sin(phi);
 176     sin(phi) cos(phi] * r ;
 177     For arc generation, the center of the circle is the axis of rotation and the radius vector is
 178     defined from the circle center to the initial position. Each line segment is formed by successive
 179     vector rotations. This requires only two cos() and sin() computations to form the rotation
 180     matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
 181     all double numbers are single precision on the Arduino. (True double precision will not have
 182     round off issues for CNC applications.) Single precision error can accumulate to be greater than
 183     tool precision in some cases. Therefore, arc path correction is implemented.
 184
 185     Small angle approximation may be used to reduce computation overhead further. This approximation
 186     holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
 187     theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
 188     to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
 189     numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
 190     issue for CNC machines with the single precision Arduino calculations.
 191     This approximation also allows mc_arc to immediately insert a line segment into the planner
 192     without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
 193     a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
 194     This is important when there are successive arc motions.
 195     */
 196     // Vector rotation matrix values
 197     double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
 198     double sin_T = theta_per_segment;
 199
 200     double arc_target[3];
 201     double sin_Ti;
 202     double cos_Ti;
 203     double r_axisi;
 204     uint16_t i;
 205     int8_t count = 0;
 206
 207     // Initialize the linear axis
 208     arc_target[this->plane_axis_2] = this->current_position[this->plane_axis_2];
 209
 210     for (i = 1; i<segments; i++) { // Increment (segments-1)
 211
 212         if (count < this->arc_correction ) {
 213           // Apply vector rotation matrix
 214           r_axisi = r_axis0*sin_T + r_axis1*cos_T;
 215           r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
 216           r_axis1 = r_axisi;
 217           count++;
 218         } else {
 219           // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
 220           // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
 221           cos_Ti = cos(i*theta_per_segment);
 222           sin_Ti = sin(i*theta_per_segment);
 223           r_axis0 = -offset[this->plane_axis_0]*cos_Ti + offset[this->plane_axis_1]*sin_Ti;
 224           r_axis1 = -offset[this->plane_axis_0]*sin_Ti - offset[this->plane_axis_1]*cos_Ti;
 225           count = 0;
 226         }
 227
 228         // Update arc_target location
 229         arc_target[this->plane_axis_0] = center_axis0 + r_axis0;
 230         arc_target[this->plane_axis_1] = center_axis1 + r_axis1;
 231         arc_target[this->plane_axis_2] += linear_per_segment;
 232         this->append_milestone(arc_target, this->feed_rate);
 233
 234     }
 235     // Ensure last segment arrives at target location.
 236     this->append_milestone(target, this->feed_rate);
 237 }
 238
 239
 240 void Robot::compute_arc(double offset[], double target[]){
 241
 242     // Find the radius
 243     double radius = hypot(offset[this->plane_axis_0], offset[this->plane_axis_1]);
 244
 245     // Set clockwise/counter-clockwise sign for mc_arc computations
 246     bool is_clockwise = false;
 247     if( this->motion_mode == MOTION_MODE_CW_ARC ){ is_clockwise = true; }
 248
 249     // Append arc
 250     this->append_arc( target, offset,  radius, is_clockwise );
 251
 252 }
 253
 254
 255 // Convert from inches to millimeters ( our internal storage unit ) if needed
 256 inline double Robot::to_millimeters( double value ){
 257         return this->inch_mode ? value/25.4 : value;
 258 }
 259
 260 double Robot::theta(double x, double y){
 261     double t = atan(x/fabs(y));
 262     if (y>0) {return(t);} else {if (t>0){return(M_PI-t);} else {return(-M_PI-t);}}
 263 }
 264
 265 void Robot::select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2){
 266     this->plane_axis_0 = axis_0;
 267     this->plane_axis_1 = axis_1;
 268     this->plane_axis_2 = axis_2;
 269 }
 270
 271