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