Touchprobe.cpp

00001 /*
00002       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
00003       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.
00004       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.
00005       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
00006 */
00007 
00008 #include "Touchprobe.h"
00009 
00010 #include "BaseSolution.h"
00011 
00012 void Touchprobe::on_module_loaded() {
00013     // if the module is disabled -> do nothing
00014     this->enabled = THEKERNEL->config->value( touchprobe_enable_checksum )->by_default(false)->as_bool();
00015     if( !(this->enabled) ){
00016         // as this module is not needed free up the resource
00017         delete this;
00018         return;
00019     }
00020     this->probe_rate = 5;
00021     // load settings
00022     this->on_config_reload(this);
00023     // register event-handlers
00024     register_for_event(ON_CONFIG_RELOAD);
00025     register_for_event(ON_GCODE_RECEIVED);
00026     register_for_event(ON_IDLE);
00027 }
00028 
00029 void Touchprobe::on_config_reload(void* argument){
00030     this->pin.from_string(  THEKERNEL->config->value(touchprobe_pin_checksum)->by_default("nc" )->as_string())->as_input();
00031     this->debounce_count =  THEKERNEL->config->value(touchprobe_debounce_count_checksum)->by_default(100  )->as_number();
00032 
00033     this->steppers[0] = THEKERNEL->robot->alpha_stepper_motor;
00034     this->steppers[1] = THEKERNEL->robot->beta_stepper_motor;
00035     this->steppers[2] = THEKERNEL->robot->gamma_stepper_motor;
00036 
00037     this->should_log = this->enabled = THEKERNEL->config->value( touchprobe_log_enable_checksum )->by_default(false)->as_bool();
00038     if( this->should_log){
00039         this->filename = THEKERNEL->config->value(touchprobe_logfile_name_checksum)->by_default("/sd/probe_log.csv")->as_string();
00040         this->mcode = THEKERNEL->config->value(touchprobe_log_rotate_mcode_checksum)->by_default(0)->as_int();
00041         this->logfile = NULL;
00042     }
00043 }
00044 
00045 void Touchprobe::wait_for_touch(int distance[]){
00046     unsigned int debounce = 0;
00047     while(true){
00048         THEKERNEL->call_event(ON_IDLE);
00049         // if no stepper is moving, moves are finished and there was no touch
00050         if( ((this->steppers[0]->moving ? 0:1 ) + (this->steppers[1]->moving ? 0:1 ) + (this->steppers[2]->moving ? 0:1 )) == 3 ){
00051             return;
00052         }
00053         // if the touchprobe is active...
00054         if( this->pin.get() ){
00055             //...increase debounce counter...
00056             if( debounce < debounce_count) {
00057                 // ...but only if the counter hasn't reached the max. value
00058                 debounce++;
00059             } else {
00060                 // ...otherwise stop the steppers, return its remaining steps
00061                 for( int i=0; i<3; i++ ){
00062                     distance[i] = 0;
00063                     if ( this->steppers[i]->moving ){
00064                         distance[i] =  this->steppers[i]->stepped;
00065                         distance[i] *= this->steppers[i]->direction ? -1 : 1;
00066                         this->steppers[i]->move(0,0);
00067                     }
00068                 }
00069                 return;
00070             }
00071         }else{
00072             // The probe was not hit yet, reset debounce counter
00073             debounce = 0;
00074         }
00075     }
00076 }
00077 
00078 
00079 void Touchprobe::flush_log(){
00080     //FIXME *sigh* fflush doesn't work as expected, see: http://mbed.org/forum/mbed/topic/3234/ or http://mbed.org/search/?type=&q=fflush
00081     //fflush(logfile);
00082     fclose(logfile);
00083     //can't reopen the file here -> crash
00084     logfile = NULL;
00085 }
00086 // Workaround for the close<->reopen crash, which itself is a workaround for wrong (or unimplemented) fflush behaviour
00087 void Touchprobe::on_idle(void* argument){
00088     if( this->logfile == NULL) {
00089         // NOTE: File creation is buggy, a file may appear but writing to it will fail
00090         this->logfile = fopen( filename.c_str(), "a");
00091     }
00092 }
00093 
00094 void Touchprobe::on_gcode_received(void* argument)
00095 {
00096     Gcode* gcode = static_cast<Gcode*>(argument);
00097     Robot* robot = THEKERNEL->robot;
00098 
00099     if( gcode->has_g) {
00100         if( gcode->g == 31 ) {
00101             float tmp[3], pos[3], mm[3];
00102             int steps[3];
00103             // first wait for an empty queue i.e. no moves left
00104             THEKERNEL->conveyor->wait_for_empty_queue();
00105 
00106             robot->get_axis_position(pos);
00107             for(char c = 'X'; c <= 'Z'; c++){
00108                 if( gcode->has_letter(c) ){
00109                     tmp[c-'X'] = robot->to_millimeters(gcode->get_value(c)) - ( robot->absolute_mode ? pos[c-'X'] : 0 );
00110                 }else{
00111                     tmp[c-'X'] = 0;
00112                 }
00113             }
00114             if( gcode->has_letter('F') )            {
00115                 this->probe_rate = robot->to_millimeters( gcode->get_value('F') ) / robot->seconds_per_minute;
00116             }
00117             robot->arm_solution->cartesian_to_actuator(tmp, mm);
00118             for (int c = 0; c < 3; c++)
00119                 steps[c] = mm[c] * robot->actuators[c]->steps_per_mm;
00120 
00121             if( ((abs(steps[0]) > 0 ? 1:0) + (abs(steps[1]) > 0 ? 1:0) + (abs(steps[2]) > 0 ? 1:0)) != 1 ){
00122                 return; //TODO coordinated movement not supported yet
00123             }
00124 
00125             // Enable the motors
00126             THEKERNEL->stepper->turn_enable_pins_on();
00127             // move
00128             for(char c='X'; c<='Z'; c++){
00129                 tmp[c - 'X'] = robot->actuators[c - 'X']->steps_per_mm;
00130                 if( steps[c-'X'] == 0 ){
00131                     continue;
00132                 }
00133                 bool dir = steps[c-'X'] < 0;
00134                 // tmp is steps/mm, probe_rate in mm/s -> speed needs steps/s
00135                 this->steppers[c-'X']->set_speed(this->probe_rate * robot->actuators[c]->steps_per_mm);
00136                 this->steppers[c-'X']->move(dir,abs(steps[c-'X']));
00137             }
00138 
00139             wait_for_touch(steps);
00140             // calculate new position
00141             for(char c='X'; c<='Z'; c++){
00142                 robot->reset_axis_position(pos[c-'X'] + mm[c-'X'], c-'X');
00143             }
00144 
00145             if( this->should_log ){
00146                 robot->get_axis_position(pos);
00147                 fprintf(logfile,"%1.3f %1.3f %1.3f\n", robot->from_millimeters(pos[0]), robot->from_millimeters(pos[1]), robot->from_millimeters(pos[2]) );
00148                 flush_log();
00149             }
00150         }
00151     }else if(gcode->has_m) {
00152         // log rotation
00153         // for now this only writes a separator
00154         // TODO do a actual log rotation
00155         if( this->mcode != 0 && this->should_log && gcode->m == this->mcode){
00156             string name;
00157             fputs("--\n",logfile);
00158             flush_log();
00159         }
00160     }
00161 }
00162