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 "PublicDataRequest.h"
  21 #include "arm_solutions/BaseSolution.h"
  22 #include "arm_solutions/CartesianSolution.h"
  23 #include "arm_solutions/RotatableCartesianSolution.h"
  24 #include "arm_solutions/RostockSolution.h"
  25 #include "arm_solutions/JohannKosselSolution.h"
  26 #include "arm_solutions/HBotSolution.h"
  27
  28 // The Robot converts GCodes into actual movements, and then adds them to the Planner, which passes them to the Conveyor so they can be added to the queue
  29 // It takes care of cutting arcs into segments, same thing for line that are too long
  30
  31 Robot::Robot(){
  32     this->inch_mode = false;
  33     this->absolute_mode = true;
  34     this->motion_mode =  MOTION_MODE_SEEK;
  35     this->select_plane(X_AXIS, Y_AXIS, Z_AXIS);
  36     clear_vector(this->current_position);
  37     clear_vector(this->last_milestone);
  38     this->arm_solution = NULL;
  39     seconds_per_minute = 60.0;
  40 }
  41
  42 //Called when the module has just been loaded
  43 void Robot::on_module_loaded() {
  44     register_for_event(ON_CONFIG_RELOAD);
  45     this->register_for_event(ON_GCODE_RECEIVED);
  46     this->register_for_event(ON_GET_PUBLIC_DATA);
  47     this->register_for_event(ON_SET_PUBLIC_DATA);
  48
  49     // Configuration
  50     this->on_config_reload(this);
  51
  52     // Make our 3 StepperMotors
  53     this->alpha_stepper_motor  = this->kernel->step_ticker->add_stepper_motor( new StepperMotor(&alpha_step_pin,&alpha_dir_pin,&alpha_en_pin) );
  54     this->beta_stepper_motor   = this->kernel->step_ticker->add_stepper_motor( new StepperMotor(&beta_step_pin, &beta_dir_pin, &beta_en_pin ) );
  55     this->gamma_stepper_motor  = this->kernel->step_ticker->add_stepper_motor( new StepperMotor(&gamma_step_pin,&gamma_dir_pin,&gamma_en_pin) );
  56
  57 }
  58
  59 void Robot::on_config_reload(void* argument){
  60
  61     // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
  62     // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
  63     // To make adding those solution easier, they have their own, separate object.
  64     // Here we read the config to find out which arm solution to use
  65     if (this->arm_solution) delete this->arm_solution;
  66     int solution_checksum = get_checksum(this->kernel->config->value(arm_solution_checksum)->by_default("cartesian")->as_string());
  67     // Note checksums are not const expressions when in debug mode, so don't use switch
  68     if(solution_checksum == hbot_checksum || solution_checksum == corexy_checksum) {
  69         this->arm_solution = new HBotSolution(this->kernel->config);
  70
  71     }else if(solution_checksum == rostock_checksum) {
  72         this->arm_solution = new RostockSolution(this->kernel->config);
  73
  74     }else if(solution_checksum == kossel_checksum) {
  75         this->arm_solution = new JohannKosselSolution(this->kernel->config);
  76
  77     }else if(solution_checksum ==  delta_checksum) {
  78         // place holder for now
  79         this->arm_solution = new RostockSolution(this->kernel->config);
  80
  81     }else if(solution_checksum == rotatable_cartesian_checksum) {
  82         this->arm_solution = new RotatableCartesianSolution(this->kernel->config);
  83
  84     }else if(solution_checksum == cartesian_checksum) {
  85         this->arm_solution = new CartesianSolution(this->kernel->config);
  86
  87     }else{
  88         this->arm_solution = new CartesianSolution(this->kernel->config);
  89     }
  90
  91
  92     this->feed_rate           = this->kernel->config->value(default_feed_rate_checksum   )->by_default(100    )->as_number() / 60;
  93     this->seek_rate           = this->kernel->config->value(default_seek_rate_checksum   )->by_default(100    )->as_number() / 60;
  94     this->mm_per_line_segment = this->kernel->config->value(mm_per_line_segment_checksum )->by_default(0.0    )->as_number();
  95     this->delta_segments_per_second = this->kernel->config->value(delta_segments_per_second_checksum )->by_default(0.0    )->as_number();
  96     this->mm_per_arc_segment  = this->kernel->config->value(mm_per_arc_segment_checksum  )->by_default(0.5    )->as_number();
  97     this->arc_correction      = this->kernel->config->value(arc_correction_checksum      )->by_default(5      )->as_number();
  98     this->max_speeds[X_AXIS]  = this->kernel->config->value(x_axis_max_speed_checksum    )->by_default(60000  )->as_number();
  99     this->max_speeds[Y_AXIS]  = this->kernel->config->value(y_axis_max_speed_checksum    )->by_default(60000  )->as_number();
 100     this->max_speeds[Z_AXIS]  = this->kernel->config->value(z_axis_max_speed_checksum    )->by_default(300    )->as_number();
 101     this->alpha_step_pin.from_string( this->kernel->config->value(alpha_step_pin_checksum )->by_default("2.0"  )->as_string())->as_output();
 102     this->alpha_dir_pin.from_string(  this->kernel->config->value(alpha_dir_pin_checksum  )->by_default("0.5"  )->as_string())->as_output();
 103     this->alpha_en_pin.from_string(   this->kernel->config->value(alpha_en_pin_checksum   )->by_default("0.4"  )->as_string())->as_output();
 104     this->beta_step_pin.from_string(  this->kernel->config->value(beta_step_pin_checksum  )->by_default("2.1"  )->as_string())->as_output();
 105     this->gamma_step_pin.from_string( this->kernel->config->value(gamma_step_pin_checksum )->by_default("2.2"  )->as_string())->as_output();
 106     this->gamma_dir_pin.from_string(  this->kernel->config->value(gamma_dir_pin_checksum  )->by_default("0.20" )->as_string())->as_output();
 107     this->gamma_en_pin.from_string(   this->kernel->config->value(gamma_en_pin_checksum   )->by_default("0.19" )->as_string())->as_output();
 108     this->beta_dir_pin.from_string(   this->kernel->config->value(beta_dir_pin_checksum   )->by_default("0.11" )->as_string())->as_output();
 109     this->beta_en_pin.from_string(    this->kernel->config->value(beta_en_pin_checksum    )->by_default("0.10" )->as_string())->as_output();
 110
 111 }
 112
 113 void Robot::on_get_public_data(void* argument){
 114     PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
 115
 116     if(!pdr->starts_with(robot_checksum)) return;
 117
 118     if(pdr->second_element_is(speed_override_percent_checksum)) {
 119         static double return_data;
 120         return_data= 100*this->seconds_per_minute/60;
 121         pdr->set_data_ptr(&return_data);
 122         pdr->set_taken();
 123
 124     }else if(pdr->second_element_is(current_position_checksum)) {
 125         static double return_data[3];
 126         return_data[0]= from_millimeters(this->current_position[0]);
 127         return_data[1]= from_millimeters(this->current_position[1]);
 128         return_data[2]= from_millimeters(this->current_position[2]);
 129
 130         pdr->set_data_ptr(&return_data);
 131         pdr->set_taken();
 132     }
 133 }
 134
 135 void Robot::on_set_public_data(void* argument){
 136     PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
 137
 138     if(!pdr->starts_with(robot_checksum)) return;
 139
 140     if(pdr->second_element_is(speed_override_percent_checksum)) {
 141         // NOTE do not use this while printing!
 142         double t= *static_cast<double*>(pdr->get_data_ptr());
 143         // enforce minimum 10% speed
 144         if (t < 10.0) t= 10.0;
 145
 146         this->seconds_per_minute= t * 0.6;
 147         pdr->set_taken();
 148     }
 149 }
 150
 151 //A GCode has been received
 152 //See if the current Gcode line has some orders for us
 153 void Robot::on_gcode_received(void * argument){
 154     Gcode* gcode = static_cast<Gcode*>(argument);
 155
 156     //Temp variables, constant properties are stored in the object
 157     uint8_t next_action = NEXT_ACTION_DEFAULT;
 158     this->motion_mode = -1;
 159
 160    //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
 161     if( gcode->has_g){
 162         switch( gcode->g ){
 163             case 0:  this->motion_mode = MOTION_MODE_SEEK; gcode->mark_as_taken(); break;
 164             case 1:  this->motion_mode = MOTION_MODE_LINEAR; gcode->mark_as_taken();  break;
 165             case 2:  this->motion_mode = MOTION_MODE_CW_ARC; gcode->mark_as_taken();  break;
 166             case 3:  this->motion_mode = MOTION_MODE_CCW_ARC; gcode->mark_as_taken();  break;
 167             case 17: this->select_plane(X_AXIS, Y_AXIS, Z_AXIS); gcode->mark_as_taken();  break;
 168             case 18: this->select_plane(X_AXIS, Z_AXIS, Y_AXIS); gcode->mark_as_taken();  break;
 169             case 19: this->select_plane(Y_AXIS, Z_AXIS, X_AXIS); gcode->mark_as_taken();  break;
 170             case 20: this->inch_mode = true; gcode->mark_as_taken();  break;
 171             case 21: this->inch_mode = false; gcode->mark_as_taken();  break;
 172             case 90: this->absolute_mode = true; gcode->mark_as_taken();  break;
 173             case 91: this->absolute_mode = false; gcode->mark_as_taken();  break;
 174             case 92: {
 175                 if(gcode->get_num_args() == 0){
 176                     clear_vector(this->last_milestone);
 177                 }else{
 178                     for (char letter = 'X'; letter <= 'Z'; letter++){
 179                         if ( gcode->has_letter(letter) )
 180                             this->last_milestone[letter-'X'] = this->to_millimeters(gcode->get_value(letter));
 181                     }
 182                 }
 183                 memcpy(this->current_position, this->last_milestone, sizeof(double)*3); // current_position[] = last_milestone[];
 184                 this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position);
 185                 gcode->mark_as_taken();
 186                 return; // TODO: Wait until queue empty
 187            }
 188        }
 189    }else if( gcode->has_m){
 190      switch( gcode->m ){
 191             case 92: // M92 - set steps per mm
 192                 double steps[3];
 193                 this->arm_solution->get_steps_per_millimeter(steps);
 194                 if (gcode->has_letter('X'))
 195                     steps[0] = this->to_millimeters(gcode->get_value('X'));
 196                 if (gcode->has_letter('Y'))
 197                     steps[1] = this->to_millimeters(gcode->get_value('Y'));
 198                 if (gcode->has_letter('Z'))
 199                     steps[2] = this->to_millimeters(gcode->get_value('Z'));
 200                 if (gcode->has_letter('F'))
 201                     seconds_per_minute = gcode->get_value('F');
 202                 this->arm_solution->set_steps_per_millimeter(steps);
 203                 // update current position in steps
 204                 this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position);
 205                 gcode->stream->printf("X:%g Y:%g Z:%g F:%g ", steps[0], steps[1], steps[2], seconds_per_minute);
 206                 gcode->add_nl = true;
 207                 gcode->mark_as_taken();
 208                 return;
 209             case 114: gcode->stream->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
 210                                                  from_millimeters(this->current_position[0]),
 211                                                  from_millimeters(this->current_position[1]),
 212                                                  from_millimeters(this->current_position[2]));
 213                 gcode->add_nl = true;
 214                 gcode->mark_as_taken();
 215                 return;
 216             // case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
 217             //     gcode->mark_as_taken();
 218             //     if (gcode->has_letter('S'))
 219             //     {
 220             //         double acc= gcode->get_value('S') * 60 * 60; // mm/min^2
 221             //         // enforce minimum
 222             //         if (acc < 1.0)
 223             //             acc = 1.0;
 224             //         this->kernel->planner->acceleration= acc;
 225             //     }
 226             //     break;
 227
 228             case 220: // M220 - speed override percentage
 229                 gcode->mark_as_taken();
 230                 if (gcode->has_letter('S'))
 231                 {
 232                     double factor = gcode->get_value('S');
 233                     // enforce minimum 10% speed
 234                     if (factor < 10.0)
 235                         factor = 10.0;
 236                     seconds_per_minute = factor * 0.6;
 237                 }
 238                 break;
 239
 240             case 665: // M665 set optional arm solution variables based on arm solution
 241                 gcode->mark_as_taken();
 242                 // the parameter args could be any letter so try each one
 243                 for(char c='A';c<='Z';c++) {
 244                     double v;
 245                     bool supported= arm_solution->get_optional(c, &v); // retrieve current value if supported
 246
 247                     if(supported && gcode->has_letter(c)) { // set new value if supported
 248                         v= gcode->get_value(c);
 249                         arm_solution->set_optional(c, v);
 250                     }
 251                     if(supported) { // print all current values of supported options
 252                         gcode->stream->printf("%c %8.3f ", c, v);
 253                         gcode->add_nl = true;
 254                     }
 255                 }
 256                 break;
 257
 258         }
 259    }
 260     if( this->motion_mode < 0)
 261         return;
 262
 263    //Get parameters
 264     double target[3], offset[3];
 265     clear_vector(target); clear_vector(offset);
 266
 267     memcpy(target, this->current_position, sizeof(target));    //default to last target
 268
 269     for(char letter = 'I'; letter <= 'K'; letter++){ if( gcode->has_letter(letter) ){ offset[letter-'I'] = this->to_millimeters(gcode->get_value(letter));                                                    } }
 270     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']);  } }
 271
 272     if( gcode->has_letter('F') )
 273     {
 274         if( this->motion_mode == MOTION_MODE_SEEK )
 275             this->seek_rate = this->to_millimeters( gcode->get_value('F') ) / 60.0;
 276         else
 277             this->feed_rate = this->to_millimeters( gcode->get_value('F') ) / 60.0;
 278     }
 279
 280     //Perform any physical actions
 281     switch( next_action ){
 282         case NEXT_ACTION_DEFAULT:
 283             switch(this->motion_mode){
 284                 case MOTION_MODE_CANCEL: break;
 285                 case MOTION_MODE_SEEK  : this->append_line(gcode, target, this->seek_rate ); break;
 286                 case MOTION_MODE_LINEAR: this->append_line(gcode, target, this->feed_rate ); break;
 287                 case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: this->compute_arc(gcode, offset, target ); break;
 288             }
 289             break;
 290     }
 291
 292     // As far as the parser is concerned, the position is now == target. In reality the
 293     // motion control system might still be processing the action and the real tool position
 294     // in any intermediate location.
 295     memcpy(this->current_position, target, sizeof(double)*3); // this->position[] = target[];
 296
 297 }
 298
 299 // We received a new gcode, and one of the functions
 300 // determined the distance for that given gcode. So now we can attach this gcode to the right block
 301 // and continue
 302 void Robot::distance_in_gcode_is_known(Gcode* gcode){
 303
 304     //If the queue is empty, execute immediatly, otherwise attach to the last added block
 305     if( this->kernel->conveyor->queue.size() == 0 ){
 306         this->kernel->call_event(ON_GCODE_EXECUTE, gcode );
 307     }else{
 308         Block* block = this->kernel->conveyor->queue.get_ref( this->kernel->conveyor->queue.size() - 1 );
 309         block->append_gcode(gcode);
 310     }
 311
 312 }
 313
 314 // Reset the position for all axes ( used in homing and G92 stuff )
 315 void Robot::reset_axis_position(double position, int axis) {
 316     this->last_milestone[axis] = this->current_position[axis] = position;
 317     this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position);
 318 }
 319
 320
 321 // Convert target from millimeters to steps, and append this to the planner
 322 void Robot::append_milestone( double target[], double rate ){
 323     int steps[3]; //Holds the result of the conversion
 324
 325     // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
 326     this->arm_solution->millimeters_to_steps( target, steps );
 327
 328     double deltas[3];
 329     for(int axis=X_AXIS;axis<=Z_AXIS;axis++){deltas[axis]=target[axis]-this->last_milestone[axis];}
 330
 331     // Compute how long this move moves, so we can attach it to the block for later use
 332     double millimeters_of_travel = sqrt( pow( deltas[X_AXIS], 2 ) +  pow( deltas[Y_AXIS], 2 ) +  pow( deltas[Z_AXIS], 2 ) );
 333
 334     // Do not move faster than the configured limits
 335     for(int axis=X_AXIS;axis<=Z_AXIS;axis++){
 336         if( this->max_speeds[axis] > 0 ){
 337             double axis_speed = ( fabs(deltas[axis]) / ( millimeters_of_travel / rate )) * seconds_per_minute;
 338             if( axis_speed > this->max_speeds[axis] ){
 339                 rate = rate * ( this->max_speeds[axis] / axis_speed );
 340             }
 341         }
 342     }
 343
 344     // Append the block to the planner
 345     this->kernel->planner->append_block( steps, rate * seconds_per_minute, millimeters_of_travel, deltas );
 346
 347     // Update the last_milestone to the current target for the next time we use last_milestone
 348     memcpy(this->last_milestone, target, sizeof(double)*3); // this->last_milestone[] = target[];
 349
 350 }
 351
 352 // Append a move to the queue ( cutting it into segments if needed )
 353 void Robot::append_line(Gcode* gcode, double target[], double rate ){
 354
 355     // Find out the distance for this gcode
 356     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 ) );
 357
 358     // We ignore non-moves ( for example, extruder moves are not XYZ moves )
 359     if( gcode->millimeters_of_travel < 0.0001 ){ return; }
 360
 361     // Mark the gcode as having a known distance
 362     this->distance_in_gcode_is_known( gcode );
 363
 364     // We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
 365     // In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
 366     // 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
 367     uint16_t segments;
 368
 369     if(this->delta_segments_per_second > 1.0) {
 370         // enabled if set to something > 1, it is set to 0.0 by default
 371         // segment based on current speed and requested segments per second
 372         // the faster the travel speed the fewer segments needed
 373         // NOTE rate is mm/sec and we take into account any speed override
 374         float seconds = 60.0/seconds_per_minute * gcode->millimeters_of_travel / rate;
 375         segments= max(1, ceil(this->delta_segments_per_second * seconds));
 376         // TODO if we are only moving in Z on a delta we don't really need to segment at all
 377
 378     }else{
 379         if(this->mm_per_line_segment == 0.0){
 380             segments= 1; // don't split it up
 381         }else{
 382             segments = ceil( gcode->millimeters_of_travel/ this->mm_per_line_segment);
 383         }
 384     }
 385
 386     // A vector to keep track of the endpoint of each segment
 387     double temp_target[3];
 388     //Initialize axes
 389     memcpy( temp_target, this->current_position, sizeof(double)*3); // temp_target[] = this->current_position[];
 390
 391     //For each segment
 392     for( int i=0; i<segments-1; i++ ){
 393         for(int axis=X_AXIS; axis <= Z_AXIS; axis++ ){ temp_target[axis] += ( target[axis]-this->current_position[axis] )/segments; }
 394         // Append the end of this segment to the queue
 395         this->append_milestone(temp_target, rate);
 396     }
 397
 398     // Append the end of this full move to the queue
 399     this->append_milestone(target, rate);
 400 }
 401
 402
 403 // Append an arc to the queue ( cutting it into segments as needed )
 404 void Robot::append_arc(Gcode* gcode, double target[], double offset[], double radius, bool is_clockwise ){
 405
 406     // Scary math
 407     double center_axis0 = this->current_position[this->plane_axis_0] + offset[this->plane_axis_0];
 408     double center_axis1 = this->current_position[this->plane_axis_1] + offset[this->plane_axis_1];
 409     double linear_travel = target[this->plane_axis_2] - this->current_position[this->plane_axis_2];
 410     double r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
 411     double r_axis1 = -offset[this->plane_axis_1];
 412     double rt_axis0 = target[this->plane_axis_0] - center_axis0;
 413     double rt_axis1 = target[this->plane_axis_1] - center_axis1;
 414
 415     // CCW angle between position and target from circle center. Only one atan2() trig computation required.
 416     double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
 417     if (angular_travel < 0) { angular_travel += 2*M_PI; }
 418     if (is_clockwise) { angular_travel -= 2*M_PI; }
 419
 420     // Find the distance for this gcode
 421     gcode->millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
 422
 423     // We don't care about non-XYZ moves ( for example the extruder produces some of those )
 424     if( gcode->millimeters_of_travel < 0.0001 ){ return; }
 425
 426     // Mark the gcode as having a known distance
 427     this->distance_in_gcode_is_known( gcode );
 428
 429     // Figure out how many segments for this gcode
 430     uint16_t segments = floor(gcode->millimeters_of_travel/this->mm_per_arc_segment);
 431
 432     double theta_per_segment = angular_travel/segments;
 433     double linear_per_segment = linear_travel/segments;
 434
 435     /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
 436     and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
 437     r_T = [cos(phi) -sin(phi);
 438     sin(phi) cos(phi] * r ;
 439     For arc generation, the center of the circle is the axis of rotation and the radius vector is
 440     defined from the circle center to the initial position. Each line segment is formed by successive
 441     vector rotations. This requires only two cos() and sin() computations to form the rotation
 442     matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
 443     all double numbers are single precision on the Arduino. (True double precision will not have
 444     round off issues for CNC applications.) Single precision error can accumulate to be greater than
 445     tool precision in some cases. Therefore, arc path correction is implemented.
 446
 447     Small angle approximation may be used to reduce computation overhead further. This approximation
 448     holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
 449     theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
 450     to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
 451     numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
 452     issue for CNC machines with the single precision Arduino calculations.
 453     This approximation also allows mc_arc to immediately insert a line segment into the planner
 454     without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
 455     a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
 456     This is important when there are successive arc motions.
 457     */
 458     // Vector rotation matrix values
 459     double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
 460     double sin_T = theta_per_segment;
 461
 462     double arc_target[3];
 463     double sin_Ti;
 464     double cos_Ti;
 465     double r_axisi;
 466     uint16_t i;
 467     int8_t count = 0;
 468
 469     // Initialize the linear axis
 470     arc_target[this->plane_axis_2] = this->current_position[this->plane_axis_2];
 471
 472     for (i = 1; i<segments; i++) { // Increment (segments-1)
 473
 474         if (count < this->arc_correction ) {
 475           // Apply vector rotation matrix
 476           r_axisi = r_axis0*sin_T + r_axis1*cos_T;
 477           r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
 478           r_axis1 = r_axisi;
 479           count++;
 480         } else {
 481           // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
 482           // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
 483           cos_Ti = cos(i*theta_per_segment);
 484           sin_Ti = sin(i*theta_per_segment);
 485           r_axis0 = -offset[this->plane_axis_0]*cos_Ti + offset[this->plane_axis_1]*sin_Ti;
 486           r_axis1 = -offset[this->plane_axis_0]*sin_Ti - offset[this->plane_axis_1]*cos_Ti;
 487           count = 0;
 488         }
 489
 490         // Update arc_target location
 491         arc_target[this->plane_axis_0] = center_axis0 + r_axis0;
 492         arc_target[this->plane_axis_1] = center_axis1 + r_axis1;
 493         arc_target[this->plane_axis_2] += linear_per_segment;
 494
 495         // Append this segment to the queue
 496         this->append_milestone(arc_target, this->feed_rate);
 497
 498     }
 499
 500     // Ensure last segment arrives at target location.
 501     this->append_milestone(target, this->feed_rate);
 502 }
 503
 504 // Do the math for an arc and add it to the queue
 505 void Robot::compute_arc(Gcode* gcode, double offset[], double target[]){
 506
 507     // Find the radius
 508     double radius = hypot(offset[this->plane_axis_0], offset[this->plane_axis_1]);
 509
 510     // Set clockwise/counter-clockwise sign for mc_arc computations
 511     bool is_clockwise = false;
 512     if( this->motion_mode == MOTION_MODE_CW_ARC ){ is_clockwise = true; }
 513
 514     // Append arc
 515     this->append_arc(gcode, target, offset,  radius, is_clockwise );
 516
 517 }
 518
 519
 520 double Robot::theta(double x, double y){
 521     double t = atan(x/fabs(y));
 522     if (y>0) {return(t);} else {if (t>0){return(M_PI-t);} else {return(-M_PI-t);}}
 523 }
 524
 525 void Robot::select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2){
 526     this->plane_axis_0 = axis_0;
 527     this->plane_axis_1 = axis_1;
 528     this->plane_axis_2 = axis_2;
 529 }
 530
 531