src/modules/tools/drillingcycles/Drillingcycles.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).
   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 "Kernel.h"
   9 #include "Drillingcycles.h"
  10 #include "checksumm.h"
  11 #include "Config.h"
  12 #include "ConfigValue.h"
  13 #include "Gcode.h"
  14 #include "Robot.h"
  15 #include "Conveyor.h"
  16 #include "SlowTicker.h"
  17 #include "StepperMotor.h"
  18 #include "StreamOutputPool.h"
  19 #include <math.h> /* fmod */
  20
  21 // axis index
  22 #define X_AXIS 0
  23 #define Y_AXIS 1
  24 #define Z_AXIS 2
  25
  26 // retract modes
  27 #define RETRACT_TO_Z 0
  28 #define RETRACT_TO_R 1
  29
  30 // dwell units
  31 #define DWELL_UNITS_S 0 // seconds
  32 #define DWELL_UNITS_P 1 // millis
  33
  34 // config names
  35 #define drillingcycles_checksum CHECKSUM("drillingcycles")
  36 #define enable_checksum         CHECKSUM("enable")
  37 #define dwell_units_checksum    CHECKSUM("dwell_units")
  38
  39 Drillingcycles::Drillingcycles() {}
  40
  41 void Drillingcycles::on_module_loaded()
  42 {
  43     // if the module is disabled -> do nothing
  44     if(! THEKERNEL->config->value(drillingcycles_checksum, enable_checksum)->by_default(false)->as_bool()) {
  45         // as this module is not needed free up the resource
  46         delete this;
  47         return;
  48     }
  49
  50     // Settings
  51     this->on_config_reload(this);
  52
  53     // events
  54     this->register_for_event(ON_GCODE_RECEIVED);
  55
  56     // reset values
  57     this->cycle_started = false;
  58     this->retract_type  = RETRACT_TO_Z;
  59
  60     this->initial_z = 0;
  61     this->r_plane   = 0;
  62
  63     this->reset_sticky();
  64 }
  65
  66 void Drillingcycles::on_config_reload(void *argument)
  67 {
  68     // take the dwell units configured by user, or select S (seconds) by default
  69     string dwell_units = THEKERNEL->config->value(drillingcycles_checksum, dwell_units_checksum)->by_default("S")->as_string();
  70     this->dwell_units  = (dwell_units == "P") ? DWELL_UNITS_P : DWELL_UNITS_S;
  71 }
  72
  73 /*
  74 The canned cycles have been implemented as described on this page :
  75 http://www.tormach.com/g81_g89_backgroung.html
  76
  77 /!\ This code expects a clean gcode, no fail safe at this time.
  78
  79 Implemented     : G80-83, G98, G99
  80 Absolute mode   : yes
  81 Relative mode   : no
  82 Incremental (L) : no
  83 */
  84
  85 /* reset all sticky values, called before each cycle */
  86 void Drillingcycles::reset_sticky()
  87 {
  88     this->sticky_z = 0; // Z depth
  89     this->sticky_r = 0; // R plane
  90     this->sticky_f = 0; // feedrate
  91     this->sticky_q = 0; // peck drilling increment
  92     this->sticky_p = 0; // dwell in seconds
  93 }
  94
  95 /* update all sticky values, called before each hole */
  96 void Drillingcycles::update_sticky(Gcode *gcode)
  97 {
  98     if (gcode->has_letter('Z')) this->sticky_z = gcode->get_value('Z');
  99     if (gcode->has_letter('R')) this->sticky_r = gcode->get_value('R');
 100     if (gcode->has_letter('F')) this->sticky_f = gcode->get_value('F');
 101     if (gcode->has_letter('Q')) this->sticky_q = gcode->get_value('Q');
 102     if (gcode->has_letter('P')) this->sticky_p = gcode->get_int('P');
 103
 104     // set retract plane
 105     if (this->retract_type == RETRACT_TO_Z)
 106         this->r_plane = this->initial_z;
 107     else
 108         this->r_plane = this->sticky_r;
 109 }
 110
 111 /* send a formatted Gcode line */
 112 int Drillingcycles::send_gcode(const char* format, ...)
 113 {
 114     // handle variable arguments
 115     va_list args;
 116     va_start(args, format);
 117     // make the formatted string
 118     char line[32]; // max length for an gcode line
 119     int n = vsnprintf(line, sizeof(line), format, args);
 120     va_end(args);
 121     // debug, print the gcode sended
 122     //THEKERNEL->streams->printf(">>> %s\r\n", line);
 123     // make gcode object and send it (right way)
 124     Gcode gc(line, &(StreamOutput::NullStream));
 125     THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
 126     // return the gcode srting length
 127     return n;
 128 }
 129
 130 /* G83: peck drilling */
 131 void Drillingcycles::peck_hole()
 132 {
 133     // start values
 134     float depth  = this->sticky_r - this->sticky_z; // travel depth
 135     float cycles = depth / this->sticky_q;          // cycles count
 136     float rest   = fmod(depth, this->sticky_q);     // final pass
 137     float z_pos  = this->sticky_r;                  // current z position
 138
 139     // for each cycle
 140     for (int i = 1; i < cycles; i++) {
 141         // decrement depth
 142         z_pos -= this->sticky_q;
 143         // feed down to depth at feedrate (F and Z)
 144         this->send_gcode("G1 F%1.4f Z%1.4f", this->sticky_f, z_pos);
 145         // rapids to retract position (R)
 146         this->send_gcode("G0 Z%1.4f", this->sticky_r);
 147     }
 148
 149     // final depth not reached
 150     if (rest > 0) {
 151         // feed down to final depth at feedrate (F and Z)
 152         this->send_gcode("G1 F%1.4f Z%1.4f", this->sticky_f, this->sticky_z);
 153     }
 154 }
 155
 156 void Drillingcycles::make_hole(Gcode *gcode)
 157 {
 158     // compile X and Y values
 159     char x[16] = "";
 160     char y[16] = "";
 161     if (gcode->has_letter('X'))
 162         snprintf(x, sizeof(x), " X%1.4f", gcode->get_value('X'));
 163     if (gcode->has_letter('Y'))
 164         snprintf(y, sizeof(y), " Y%1.4f", gcode->get_value('Y'));
 165
 166     // rapids to X/Y
 167     this->send_gcode("G0%s%s", x, y);
 168     // rapids to retract position (R)
 169     this->send_gcode("G0 Z%1.4f", this->sticky_r);
 170
 171     // if peck drilling
 172     if (this->sticky_q > 0)
 173         this->peck_hole();
 174     else
 175         // feed down to depth at feedrate (F and Z)
 176         this->send_gcode("G1 F%1.4f Z%1.4f", this->sticky_f, this->sticky_z);
 177
 178     // if dwell, wait for x seconds
 179     if (this->sticky_p > 0) {
 180         // dwell exprimed in seconds
 181         if (this->dwell_units == DWELL_UNITS_S)
 182             this->send_gcode("G4 S%u", this->sticky_p);
 183         // dwell exprimed in milliseconds
 184         else
 185             this->send_gcode("G4 P%u", this->sticky_p);
 186     }
 187
 188     // rapids retract at R-Plane (Initial-Z or R)
 189     this->send_gcode("G0 Z%1.4f", this->r_plane);
 190 }
 191
 192 void Drillingcycles::on_gcode_received(void* argument)
 193 {
 194     // received gcode
 195     Gcode *gcode = static_cast<Gcode *>(argument);
 196
 197     // no "G" in gcode, exit...
 198     if (! gcode->has_g)
 199         return;
 200
 201     // "G" value
 202     int code = gcode->g;
 203
 204     // cycle start
 205     if (code == 98 || code == 99) {
 206         // wait for any moves left and current position is update
 207         THEKERNEL->conveyor->wait_for_empty_queue();
 208         // get actual position from robot
 209         float pos[3];
 210         THEKERNEL->robot->get_axis_position(pos);
 211         // backup Z position as Initial-Z value
 212         this->initial_z = pos[Z_AXIS];
 213         // set retract type
 214         this->retract_type = (code == 98) ? RETRACT_TO_Z : RETRACT_TO_R;
 215         // reset sticky values
 216         this->reset_sticky();
 217         // mark cycle started and gcode taken
 218         this->cycle_started = true;
 219     }
 220     // cycle end
 221     else if (code == 80) {
 222         // mark cycle endded and gcode taken
 223         this->cycle_started = false;
 224
 225         // if retract position is R-Plane
 226         if (this->retract_type == RETRACT_TO_R) {
 227             // rapids retract at Initial-Z to avoid futur collisions
 228             this->send_gcode("G0 Z%1.4f", this->initial_z);
 229         }
 230     }
 231     // in cycle
 232     else if (this->cycle_started) {
 233         // relative mode not supported for now...
 234         if (THEKERNEL->robot->absolute_mode == false) {
 235             gcode->stream->printf("Drillingcycles: relative mode not supported.\r\n");
 236             gcode->stream->printf("Drillingcycles: skip hole...\r\n");
 237             // exit
 238             return;
 239         }
 240         // implemented cycles
 241         if (code == 81 || code == 82 || code == 83) {
 242             this->update_sticky(gcode);
 243             this->make_hole(gcode);
 244         }
 245     }
 246 }