Stéphane Cachat / Mbed 2 deprecated Smoothie

smoothie port to mbed online compiler (smoothieware.org)

Dependencies:   mbed

For documentation, license, ..., please check http://smoothieware.org/

This version has been tested with a 3 axis machine

modules/robot/Robot.cpp@0:31e91bb0ef3c, 2012-07-31 (annotated)

Committer:
scachat
Date:
Tue Jul 31 21:11:18 2012 +0000
Revision:
0:31e91bb0ef3c
smoothie port to mbed online compiler

Who changed what in which revision?

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