Robot.cpp

00001 /*  
00002       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)
00003       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.
00004       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.
00005       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. 
00006 */
00007 
00008 #include "libs/Module.h"
00009 #include "libs/Kernel.h"
00010 #include <string>
00011 using std::string;
00012 #include <math.h>
00013 #include "Planner.h"
00014 #include "Player.h"
00015 #include "Robot.h"
00016 #include "libs/nuts_bolts.h"
00017 #include "../communication/utils/Gcode.h"
00018 #include "arm_solutions/BaseSolution.h"
00019 #include "arm_solutions/CartesianSolution.h"
00020 
00021 Robot::Robot(){
00022     this->inch_mode = false;
00023     this->absolute_mode = true;
00024     this->motion_mode =  MOTION_MODE_SEEK; 
00025     this->select_plane(X_AXIS, Y_AXIS, Z_AXIS);
00026     clear_vector(this->current_position);
00027     clear_vector(this->last_milestone); 
00028 }
00029 
00030 //Called when the module has just been loaded
00031 void Robot::on_module_loaded() {
00032     this->arm_solution = new CartesianSolution(this->kernel->config);
00033     this->register_for_event(ON_GCODE_RECEIVED);
00034 
00035     // Configuration
00036     this->on_config_reload(this);
00037 }
00038 
00039 void Robot::on_config_reload(void* argument){
00040     this->feed_rate =           this->kernel->config->value(default_feed_rate_checksum  )->by_default(100)->as_number()/60; 
00041     this->seek_rate =           this->kernel->config->value(default_seek_rate_checksum  )->by_default(100)->as_number()/60;
00042     this->mm_per_line_segment = this->kernel->config->value(mm_per_line_segment_checksum)->by_default(0.1)->as_number();
00043     this->mm_per_arc_segment  = this->kernel->config->value(mm_per_arc_segment_checksum )->by_default(10 )->as_number();
00044     this->arc_correction      = this->kernel->config->value(arc_correction_checksum     )->by_default(5  )->as_number();
00045     this->max_speeds[X_AXIS]  = this->kernel->config->value(x_axis_max_speed_checksum   )->by_default(0  )->as_number();
00046     this->max_speeds[Y_AXIS]  = this->kernel->config->value(y_axis_max_speed_checksum   )->by_default(0  )->as_number();
00047     this->max_speeds[Z_AXIS]  = this->kernel->config->value(z_axis_max_speed_checksum   )->by_default(0  )->as_number();
00048 }
00049 
00050 //A GCode has been received
00051 void Robot::on_gcode_received(void * argument){
00052     Gcode* gcode = static_cast<Gcode*>(argument);
00053     gcode->call_on_gcode_execute_event_immediatly = false; 
00054     gcode->on_gcode_execute_event_called = false;
00055     //If the queue is empty, execute immediatly, otherwise attach to the last added block
00056     if( this->kernel->player->queue.size() == 0 ){
00057         gcode->call_on_gcode_execute_event_immediatly = true;
00058         this->execute_gcode(gcode);
00059         if( gcode->on_gcode_execute_event_called == false ){
00060             this->kernel->call_event(ON_GCODE_EXECUTE, gcode ); 
00061         }
00062     }else{
00063         Block* block = this->kernel->player->queue.get_ref( this->kernel->player->queue.size() - 1 );
00064         this->execute_gcode(gcode);
00065         block->append_gcode(gcode);
00066     }
00067     
00068 }
00069 
00070 
00071 //See if the current Gcode line has some orders for us
00072 void Robot::execute_gcode(Gcode* gcode){
00073     
00074     //Temp variables, constant properties are stored in the object
00075     uint8_t next_action = NEXT_ACTION_DEFAULT;
00076     this->motion_mode = -1;
00077 
00078    //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
00079    if( gcode->has_letter('G')){
00080        switch( (int) gcode->get_value('G') ){
00081            case 0: this->motion_mode = MOTION_MODE_SEEK; break;
00082            case 1: this->motion_mode = MOTION_MODE_LINEAR; break;
00083            case 2: this->motion_mode = MOTION_MODE_CW_ARC; break;
00084            case 3: this->motion_mode = MOTION_MODE_CCW_ARC; break;
00085            case 17: this->select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
00086            case 18: this->select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
00087            case 19: this->select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
00088            case 20:this->inch_mode = true; break;
00089            case 21:this->inch_mode = false; break;
00090            case 90:this->absolute_mode = true; break;
00091            case 91:this->absolute_mode = false; break;
00092        } 
00093     }else{ return; }
00094     
00095    //Get parameters
00096     double target[3], offset[3];
00097     clear_vector(target); clear_vector(offset); 
00098     
00099     memcpy(target, this->current_position, sizeof(target));    //default to last target
00100     
00101     for(char letter = 'I'; letter <= 'K'; letter++){ if( gcode->has_letter(letter) ){ offset[letter-'I'] = this->to_millimeters(gcode->get_value(letter));                                                    } }     
00102     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']);  } }     
00103     
00104     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; } }
00105    
00106     //Perform any physical actions
00107     switch( next_action ){
00108         case NEXT_ACTION_DEFAULT:
00109             switch(this->motion_mode){
00110                 case MOTION_MODE_CANCEL: break;
00111                 case MOTION_MODE_SEEK  : this->append_line(gcode, target, this->seek_rate ); break;
00112                 case MOTION_MODE_LINEAR: this->append_line(gcode, target, this->feed_rate ); break;
00113                 case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: this->compute_arc(gcode, offset, target ); break; 
00114             }
00115             break;
00116     }
00117 
00118     // As far as the parser is concerned, the position is now == target. In reality the
00119     // motion control system might still be processing the action and the real tool position
00120     // in any intermediate location.
00121     memcpy(this->current_position, target, sizeof(double)*3); // this->position[] = target[]; 
00122 
00123 }
00124 
00125 // Convert target from millimeters to steps, and append this to the planner
00126 void Robot::append_milestone( double target[], double rate ){
00127     int steps[3]; //Holds the result of the conversion
00128     
00129     this->arm_solution->millimeters_to_steps( target, steps );
00130     
00131     double deltas[3];
00132     for(int axis=X_AXIS;axis<=Z_AXIS;axis++){deltas[axis]=target[axis]-this->last_milestone[axis];}
00133 
00134     
00135     double millimeters_of_travel = sqrt( pow( deltas[X_AXIS], 2 ) +  pow( deltas[Y_AXIS], 2 ) +  pow( deltas[Z_AXIS], 2 ) );      
00136     
00137     double duration = 0;
00138     if( rate > 0 ){ duration = millimeters_of_travel / rate; }
00139 
00140     for(int axis=X_AXIS;axis<=Z_AXIS;axis++){
00141         if( this->max_speeds[axis] > 0 ){ 
00142             double axis_speed = ( fabs(deltas[axis]) / ( millimeters_of_travel / rate )) * 60; 
00143             if( axis_speed > this->max_speeds[axis] ){ 
00144                 rate = rate * ( this->max_speeds[axis] / axis_speed ); 
00145             }
00146         }
00147     }
00148 
00149     this->kernel->planner->append_block( steps, rate*60, millimeters_of_travel, deltas ); 
00150 
00151     memcpy(this->last_milestone, target, sizeof(double)*3); // this->last_milestone[] = target[]; 
00152 
00153 }
00154 
00155 void Robot::append_line(Gcode* gcode, double target[], double rate ){
00156 
00157 
00158     // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes. 
00159     // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
00160     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 ) ); 
00161 
00162     if( gcode->call_on_gcode_execute_event_immediatly == true ){
00163             this->kernel->call_event(ON_GCODE_EXECUTE, gcode ); 
00164             gcode->on_gcode_execute_event_called = true;
00165     }
00166 
00167     if (gcode->millimeters_of_travel == 0.0) { 
00168         this->append_milestone(this->current_position, 0.0);
00169         return; 
00170     }
00171 
00172     uint16_t segments = ceil( gcode->millimeters_of_travel/ this->mm_per_line_segment); 
00173     // A vector to keep track of the endpoint of each segment
00174     double temp_target[3];
00175     //Initialize axes
00176     memcpy( temp_target, this->current_position, sizeof(double)*3); // temp_target[] = this->current_position[];  
00177 
00178     //For each segment
00179     for( int i=0; i<segments-1; i++ ){
00180         for(int axis=X_AXIS; axis <= Z_AXIS; axis++ ){ temp_target[axis] += ( target[axis]-this->current_position[axis] )/segments; }        
00181         this->append_milestone(temp_target, rate); 
00182     }
00183     this->append_milestone(target, rate);
00184 }
00185 
00186 
00187 void Robot::append_arc(Gcode* gcode, double target[], double offset[], double radius, bool is_clockwise ){
00188 
00189     double center_axis0 = this->current_position[this->plane_axis_0] + offset[this->plane_axis_0];
00190     double center_axis1 = this->current_position[this->plane_axis_1] + offset[this->plane_axis_1];
00191     double linear_travel = target[this->plane_axis_2] - this->current_position[this->plane_axis_2];
00192     double r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
00193     double r_axis1 = -offset[this->plane_axis_1];
00194     double rt_axis0 = target[this->plane_axis_0] - center_axis0;
00195     double rt_axis1 = target[this->plane_axis_1] - center_axis1;
00196 
00197     // CCW angle between position and target from circle center. Only one atan2() trig computation required.
00198     double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
00199     if (angular_travel < 0) { angular_travel += 2*M_PI; }
00200     if (is_clockwise) { angular_travel -= 2*M_PI; }
00201 
00202     gcode->millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
00203 
00204     if( gcode->call_on_gcode_execute_event_immediatly == true ){
00205             this->kernel->call_event(ON_GCODE_EXECUTE, gcode ); 
00206             gcode->on_gcode_execute_event_called = true;
00207     }
00208 
00209     if (gcode->millimeters_of_travel == 0.0) { 
00210         this->append_milestone(this->current_position, 0.0);
00211         return; 
00212     }
00213  
00214     uint16_t segments = floor(gcode->millimeters_of_travel/this->mm_per_arc_segment);
00215 
00216     double theta_per_segment = angular_travel/segments;
00217     double linear_per_segment = linear_travel/segments;
00218 
00219     /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
00220     and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
00221     r_T = [cos(phi) -sin(phi);
00222     sin(phi) cos(phi] * r ;
00223     For arc generation, the center of the circle is the axis of rotation and the radius vector is
00224     defined from the circle center to the initial position. Each line segment is formed by successive
00225     vector rotations. This requires only two cos() and sin() computations to form the rotation
00226     matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
00227     all double numbers are single precision on the Arduino. (True double precision will not have
00228     round off issues for CNC applications.) Single precision error can accumulate to be greater than
00229     tool precision in some cases. Therefore, arc path correction is implemented.
00230 
00231     Small angle approximation may be used to reduce computation overhead further. This approximation
00232     holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
00233     theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
00234     to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
00235     numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
00236     issue for CNC machines with the single precision Arduino calculations.
00237     This approximation also allows mc_arc to immediately insert a line segment into the planner
00238     without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
00239     a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
00240     This is important when there are successive arc motions.
00241     */
00242     // Vector rotation matrix values
00243     double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
00244     double sin_T = theta_per_segment;
00245 
00246     double arc_target[3];
00247     double sin_Ti;
00248     double cos_Ti;
00249     double r_axisi;
00250     uint16_t i;
00251     int8_t count = 0;
00252 
00253     // Initialize the linear axis
00254     arc_target[this->plane_axis_2] = this->current_position[this->plane_axis_2];
00255 
00256     for (i = 1; i<segments; i++) { // Increment (segments-1)
00257 
00258         if (count < this->arc_correction ) {
00259           // Apply vector rotation matrix
00260           r_axisi = r_axis0*sin_T + r_axis1*cos_T;
00261           r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
00262           r_axis1 = r_axisi;
00263           count++;
00264         } else {
00265           // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
00266           // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
00267           cos_Ti = cos(i*theta_per_segment);
00268           sin_Ti = sin(i*theta_per_segment);
00269           r_axis0 = -offset[this->plane_axis_0]*cos_Ti + offset[this->plane_axis_1]*sin_Ti;
00270           r_axis1 = -offset[this->plane_axis_0]*sin_Ti - offset[this->plane_axis_1]*cos_Ti;
00271           count = 0;
00272         }
00273 
00274         // Update arc_target location
00275         arc_target[this->plane_axis_0] = center_axis0 + r_axis0;
00276         arc_target[this->plane_axis_1] = center_axis1 + r_axis1;
00277         arc_target[this->plane_axis_2] += linear_per_segment;
00278         this->append_milestone(arc_target, this->feed_rate);
00279 
00280     }
00281     // Ensure last segment arrives at target location.
00282     this->append_milestone(target, this->feed_rate);
00283 }
00284 
00285 
00286 void Robot::compute_arc(Gcode* gcode, double offset[], double target[]){
00287 
00288     // Find the radius
00289     double radius = hypot(offset[this->plane_axis_0], offset[this->plane_axis_1]);
00290 
00291     // Set clockwise/counter-clockwise sign for mc_arc computations
00292     bool is_clockwise = false;
00293     if( this->motion_mode == MOTION_MODE_CW_ARC ){ is_clockwise = true; } 
00294 
00295     // Append arc
00296     this->append_arc(gcode, target, offset,  radius, is_clockwise );
00297 
00298 }
00299 
00300 
00301 // Convert from inches to millimeters ( our internal storage unit ) if needed
00302 inline double Robot::to_millimeters( double value ){
00303         return this->inch_mode ? value/25.4 : value; 
00304 }
00305 
00306 double Robot::theta(double x, double y){
00307     double t = atan(x/fabs(y));
00308     if (y>0) {return(t);} else {if (t>0){return(M_PI-t);} else {return(-M_PI-t);}}
00309 }
00310 
00311 void Robot::select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2){
00312     this->plane_axis_0 = axis_0;
00313     this->plane_axis_1 = axis_1;
00314     this->plane_axis_2 = axis_2;
00315 }
00316 
00317