Commit | Line | Data |
---|---|---|
3779fd74 SK |
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() { | |
4a76020e | 11 | // if the module is disabled -> do nothing |
6de8ab5b | 12 | this->enabled = this->kernel->config->value( touchprobe_enable_checksum )->by_default(false)->as_bool(); |
98761c28 JM |
13 | if( !(this->enabled) ){ |
14 | // as this module is not needed free up the resource | |
15 | delete this; | |
16 | return; | |
17 | } | |
6de8ab5b | 18 | this->probe_rate = 5; |
3779fd74 SK |
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); | |
a63da33c | 24 | register_for_event(ON_IDLE); |
3779fd74 SK |
25 | } |
26 | ||
27 | void Touchprobe::on_config_reload(void* argument){ | |
28 | this->pin.from_string( this->kernel->config->value(touchprobe_pin_checksum)->by_default("nc" )->as_string())->as_input(); | |
f29b0272 | 29 | this->debounce_count = this->kernel->config->value(touchprobe_debounce_count_checksum)->by_default(100 )->as_number(); |
3779fd74 SK |
30 | |
31 | this->steppers[0] = this->kernel->robot->alpha_stepper_motor; | |
32 | this->steppers[1] = this->kernel->robot->beta_stepper_motor; | |
33 | this->steppers[2] = this->kernel->robot->gamma_stepper_motor; | |
34 | ||
6de8ab5b SK |
35 | this->should_log = this->enabled = this->kernel->config->value( touchprobe_log_enable_checksum )->by_default(false)->as_bool(); |
36 | if( this->should_log){ | |
37 | this->filename = this->kernel->config->value(touchprobe_logfile_name_checksum)->by_default("/sd/probe_log.csv")->as_string(); | |
a63da33c SK |
38 | this->mcode = this->kernel->config->value(touchprobe_log_rotate_mcode_checksum)->by_default(0)->as_int(); |
39 | this->logfile = NULL; | |
6de8ab5b | 40 | } |
3779fd74 SK |
41 | } |
42 | ||
6de8ab5b | 43 | void Touchprobe::wait_for_touch(int distance[]){ |
3779fd74 | 44 | unsigned int debounce = 0; |
a63da33c | 45 | while(true){ |
3779fd74 | 46 | this->kernel->call_event(ON_IDLE); |
a63da33c SK |
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() ){ | |
3779fd74 SK |
53 | //...increase debounce counter... |
54 | if( debounce < debounce_count) { | |
55 | // ...but only if the counter hasn't reached the max. value | |
56 | debounce++; | |
3779fd74 | 57 | } else { |
bb4ebb9b | 58 | // ...otherwise stop the steppers, return its remaining steps |
3779fd74 | 59 | for( int i=0; i<3; i++ ){ |
6de8ab5b | 60 | distance[i] = 0; |
3779fd74 | 61 | if ( this->steppers[i]->moving ){ |
6de8ab5b SK |
62 | distance[i] = this->steppers[i]->stepped; |
63 | distance[i] *= this->steppers[i]->dir_pin->get() ? -1 : 1; | |
bb4ebb9b | 64 | this->steppers[i]->move(0,0); |
3779fd74 SK |
65 | } |
66 | } | |
a63da33c | 67 | return; |
3779fd74 SK |
68 | } |
69 | }else{ | |
a63da33c | 70 | // The probe was not hit yet, reset debounce counter |
3779fd74 SK |
71 | debounce = 0; |
72 | } | |
73 | } | |
3779fd74 SK |
74 | } |
75 | ||
a63da33c | 76 | |
f015eaee SK |
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 | |
a63da33c SK |
85 | void Touchprobe::on_idle(void* argument){ |
86 | if( this->logfile == NULL) { | |
f015eaee | 87 | // NOTE: File creation is buggy, a file may appear but writing to it will fail |
a63da33c SK |
88 | this->logfile = fopen( filename.c_str(), "a"); |
89 | } | |
90 | } | |
91 | ||
3779fd74 SK |
92 | void Touchprobe::on_gcode_received(void* argument) |
93 | { | |
94 | Gcode* gcode = static_cast<Gcode*>(argument); | |
4a76020e | 95 | Robot* robot = this->kernel->robot; |
3779fd74 | 96 | |
4a76020e SK |
97 | if( gcode->has_g) { |
98 | if( gcode->g == 31 ) { | |
6de8ab5b SK |
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 | this->kernel->conveyor->wait_for_empty_queue(); | |
103 | ||
a63da33c SK |
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 | } | |
6de8ab5b | 112 | if( gcode->has_letter('F') ) { |
a63da33c | 113 | this->probe_rate = robot->to_millimeters( gcode->get_value('F') ) / 60.0; |
6de8ab5b | 114 | } |
6de8ab5b | 115 | robot->arm_solution->millimeters_to_steps(tmp,steps); |
a63da33c | 116 | robot->arm_solution->millimeters_to_steps(tmp,distance); //default to full move |
6de8ab5b SK |
117 | |
118 | if( ((abs(steps[0]) > 0 ? 1:0) + (abs(steps[1]) > 0 ? 1:0) + (abs(steps[2]) > 0 ? 1:0)) != 1 ){ | |
a63da33c | 119 | return; //TODO coordinated movement not supported yet |
6de8ab5b SK |
120 | } |
121 | ||
122 | // Enable the motors | |
123 | this->kernel->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); | |
a63da33c | 137 | // calculate new position |
6de8ab5b SK |
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 | } | |
3779fd74 | 141 | |
6de8ab5b | 142 | if( this->should_log ){ |
6de8ab5b | 143 | robot->get_axis_position(pos); |
f015eaee SK |
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(); | |
3779fd74 SK |
146 | } |
147 | } | |
a63da33c | 148 | }else if(gcode->has_m) { |
f015eaee SK |
149 | // log rotation |
150 | // for now this only writes a separator | |
151 | // TODO do a actual log rotation | |
a63da33c SK |
152 | if( this->mcode != 0 && this->should_log && gcode->m == this->mcode){ |
153 | string name; | |
f015eaee SK |
154 | fputs("--\n",logfile); |
155 | flush_log(); | |
a63da33c | 156 | } |
3779fd74 SK |
157 | } |
158 | } | |
f015eaee | 159 |