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