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