src/modules/tools/touchprobe/Touchprobe.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 "Touchprobe.h"
   9
  10 #include "BaseSolution.h"
  11
  12 #define touchprobe_enable_checksum           CHECKSUM("touchprobe_enable")
  13 #define touchprobe_log_enable_checksum       CHECKSUM("touchprobe_log_enable")
  14 #define touchprobe_logfile_name_checksum     CHECKSUM("touchprobe_logfile_name")
  15 #define touchprobe_log_rotate_mcode_checksum CHECKSUM("touchprobe_log_rotate_mcode")
  16 #define touchprobe_pin_checksum              CHECKSUM("touchprobe_pin")
  17 #define touchprobe_debounce_count_checksum   CHECKSUM("touchprobe_debounce_count")
  18
  19 #define touchprobe_enable_checksum           CHECKSUM("touchprobe_enable")
  20 #define touchprobe_log_enable_checksum       CHECKSUM("touchprobe_log_enable")
  21 #define touchprobe_logfile_name_checksum     CHECKSUM("touchprobe_logfile_name")
  22 #define touchprobe_log_rotate_mcode_checksum CHECKSUM("touchprobe_log_rotate_mcode")
  23 #define touchprobe_pin_checksum              CHECKSUM("touchprobe_pin")
  24 #define touchprobe_debounce_count_checksum   CHECKSUM("touchprobe_debounce_count")
  25
  26
  27 void Touchprobe::on_module_loaded() {
  28     // if the module is disabled -> do nothing
  29     this->enabled = THEKERNEL->config->value( touchprobe_enable_checksum )->by_default(false)->as_bool();
  30     if( !(this->enabled) ){
  31         // as this module is not needed free up the resource
  32         delete this;
  33         return;
  34     }
  35     this->probe_rate = 5;
  36     // load settings
  37     this->on_config_reload(this);
  38     // register event-handlers
  39     register_for_event(ON_CONFIG_RELOAD);
  40     register_for_event(ON_GCODE_RECEIVED);
  41     register_for_event(ON_IDLE);
  42 }
  43
  44 void Touchprobe::on_config_reload(void* argument){
  45     this->pin.from_string(  THEKERNEL->config->value(touchprobe_pin_checksum)->by_default("nc" )->as_string())->as_input();
  46     this->debounce_count =  THEKERNEL->config->value(touchprobe_debounce_count_checksum)->by_default(100  )->as_number();
  47
  48     this->steppers[0] = THEKERNEL->robot->alpha_stepper_motor;
  49     this->steppers[1] = THEKERNEL->robot->beta_stepper_motor;
  50     this->steppers[2] = THEKERNEL->robot->gamma_stepper_motor;
  51
  52     this->should_log = this->enabled = THEKERNEL->config->value( touchprobe_log_enable_checksum )->by_default(false)->as_bool();
  53     if( this->should_log){
  54         this->filename = THEKERNEL->config->value(touchprobe_logfile_name_checksum)->by_default("/sd/probe_log.csv")->as_string();
  55         this->mcode = THEKERNEL->config->value(touchprobe_log_rotate_mcode_checksum)->by_default(0)->as_int();
  56         this->logfile = NULL;
  57     }
  58 }
  59
  60 void Touchprobe::wait_for_touch(int distance[]){
  61     unsigned int debounce = 0;
  62     while(true){
  63         THEKERNEL->call_event(ON_IDLE);
  64         // if no stepper is moving, moves are finished and there was no touch
  65         if( ((this->steppers[0]->moving ? 0:1 ) + (this->steppers[1]->moving ? 0:1 ) + (this->steppers[2]->moving ? 0:1 )) == 3 ){
  66             return;
  67         }
  68         // if the touchprobe is active...
  69         if( this->pin.get() ){
  70             //...increase debounce counter...
  71             if( debounce < debounce_count) {
  72                 // ...but only if the counter hasn't reached the max. value
  73                 debounce++;
  74             } else {
  75                 // ...otherwise stop the steppers, return its remaining steps
  76                 for( int i=0; i<3; i++ ){
  77                     distance[i] = 0;
  78                     if ( this->steppers[i]->moving ){
  79                         distance[i] =  this->steppers[i]->stepped;
  80                         distance[i] *= this->steppers[i]->direction ? -1 : 1;
  81                         this->steppers[i]->move(0,0);
  82                     }
  83                 }
  84                 return;
  85             }
  86         }else{
  87             // The probe was not hit yet, reset debounce counter
  88             debounce = 0;
  89         }
  90     }
  91 }
  92
  93
  94 void Touchprobe::flush_log(){
  95     //FIXME *sigh* fflush doesn't work as expected, see: http://mbed.org/forum/mbed/topic/3234/ or http://mbed.org/search/?type=&q=fflush
  96     //fflush(logfile);
  97     fclose(logfile);
  98     //can't reopen the file here -> crash
  99     logfile = NULL;
 100 }
 101 // Workaround for the close<->reopen crash, which itself is a workaround for wrong (or unimplemented) fflush behaviour
 102 void Touchprobe::on_idle(void* argument){
 103     if( this->logfile == NULL) {
 104         // NOTE: File creation is buggy, a file may appear but writing to it will fail
 105         this->logfile = fopen( filename.c_str(), "a");
 106     }
 107 }
 108
 109 void Touchprobe::on_gcode_received(void* argument)
 110 {
 111     Gcode* gcode = static_cast<Gcode*>(argument);
 112     Robot* robot = THEKERNEL->robot;
 113
 114     if( gcode->has_g) {
 115         if( gcode->g == 31 ) {
 116             float tmp[3], pos[3], mm[3];
 117             int steps[3];
 118             // first wait for an empty queue i.e. no moves left
 119             THEKERNEL->conveyor->wait_for_empty_queue();
 120
 121             robot->get_axis_position(pos);
 122             for(char c = 'X'; c <= 'Z'; c++){
 123                 if( gcode->has_letter(c) ){
 124                     tmp[c-'X'] = robot->to_millimeters(gcode->get_value(c)) - ( robot->absolute_mode ? pos[c-'X'] : 0 );
 125                 }else{
 126                     tmp[c-'X'] = 0;
 127                 }
 128             }
 129             if( gcode->has_letter('F') )            {
 130                 this->probe_rate = robot->to_millimeters( gcode->get_value('F') ) / robot->seconds_per_minute;
 131             }
 132             robot->arm_solution->cartesian_to_actuator(tmp, mm);
 133             for (int c = 0; c < 3; c++)
 134                 steps[c] = mm[c] * robot->actuators[c]->steps_per_mm;
 135
 136             if( ((abs(steps[0]) > 0 ? 1:0) + (abs(steps[1]) > 0 ? 1:0) + (abs(steps[2]) > 0 ? 1:0)) != 1 ){
 137                 return; //TODO coordinated movement not supported yet
 138             }
 139
 140             // Enable the motors
 141             THEKERNEL->stepper->turn_enable_pins_on();
 142             // move
 143             for(char c='X'; c<='Z'; c++){
 144                 tmp[c - 'X'] = robot->actuators[c - 'X']->steps_per_mm;
 145                 if( steps[c-'X'] == 0 ){
 146                     continue;
 147                 }
 148                 bool dir = steps[c-'X'] < 0;
 149                 // tmp is steps/mm, probe_rate in mm/s -> speed needs steps/s
 150                 this->steppers[c-'X']->set_speed(this->probe_rate * robot->actuators[c]->steps_per_mm);
 151                 this->steppers[c-'X']->move(dir,abs(steps[c-'X']));
 152             }
 153
 154             wait_for_touch(steps);
 155             // calculate new position
 156             for(char c='X'; c<='Z'; c++){
 157                 robot->reset_axis_position(pos[c-'X'] + mm[c-'X'], c-'X');
 158             }
 159
 160             if( this->should_log ){
 161                 robot->get_axis_position(pos);
 162                 fprintf(logfile,"%1.3f %1.3f %1.3f\n", robot->from_millimeters(pos[0]), robot->from_millimeters(pos[1]), robot->from_millimeters(pos[2]) );
 163                 flush_log();
 164             }
 165         }
 166     }else if(gcode->has_m) {
 167         // log rotation
 168         // for now this only writes a separator
 169         // TODO do a actual log rotation
 170         if( this->mcode != 0 && this->should_log && gcode->m == this->mcode){
 171             string name;
 172             fputs("--\n",logfile);
 173             flush_log();
 174         }
 175     }
 176 }
 177