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 | ||
5673fe39 MM |
10 | #include "BaseSolution.h" |
11 | ||
61134a65 JM |
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 | ||
3779fd74 | 27 | void Touchprobe::on_module_loaded() { |
4a76020e | 28 | // if the module is disabled -> do nothing |
314ab8f7 | 29 | this->enabled = THEKERNEL->config->value( touchprobe_enable_checksum )->by_default(false)->as_bool(); |
98761c28 JM |
30 | if( !(this->enabled) ){ |
31 | // as this module is not needed free up the resource | |
32 | delete this; | |
33 | return; | |
34 | } | |
6de8ab5b | 35 | this->probe_rate = 5; |
3779fd74 SK |
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); | |
a63da33c | 41 | register_for_event(ON_IDLE); |
3779fd74 SK |
42 | } |
43 | ||
44 | void Touchprobe::on_config_reload(void* argument){ | |
314ab8f7 MM |
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(); | |
3779fd74 | 47 | |
314ab8f7 MM |
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; | |
3779fd74 | 51 | |
314ab8f7 | 52 | this->should_log = this->enabled = THEKERNEL->config->value( touchprobe_log_enable_checksum )->by_default(false)->as_bool(); |
6de8ab5b | 53 | if( this->should_log){ |
314ab8f7 MM |
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(); | |
a63da33c | 56 | this->logfile = NULL; |
6de8ab5b | 57 | } |
3779fd74 SK |
58 | } |
59 | ||
6de8ab5b | 60 | void Touchprobe::wait_for_touch(int distance[]){ |
3779fd74 | 61 | unsigned int debounce = 0; |
a63da33c | 62 | while(true){ |
314ab8f7 | 63 | THEKERNEL->call_event(ON_IDLE); |
a63da33c SK |
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() ){ | |
3779fd74 SK |
70 | //...increase debounce counter... |
71 | if( debounce < debounce_count) { | |
72 | // ...but only if the counter hasn't reached the max. value | |
73 | debounce++; | |
3779fd74 | 74 | } else { |
bb4ebb9b | 75 | // ...otherwise stop the steppers, return its remaining steps |
3779fd74 | 76 | for( int i=0; i<3; i++ ){ |
6de8ab5b | 77 | distance[i] = 0; |
3779fd74 | 78 | if ( this->steppers[i]->moving ){ |
6de8ab5b | 79 | distance[i] = this->steppers[i]->stepped; |
9c5fa39a | 80 | distance[i] *= this->steppers[i]->direction ? -1 : 1; |
bb4ebb9b | 81 | this->steppers[i]->move(0,0); |
3779fd74 SK |
82 | } |
83 | } | |
a63da33c | 84 | return; |
3779fd74 SK |
85 | } |
86 | }else{ | |
a63da33c | 87 | // The probe was not hit yet, reset debounce counter |
3779fd74 SK |
88 | debounce = 0; |
89 | } | |
90 | } | |
3779fd74 SK |
91 | } |
92 | ||
a63da33c | 93 | |
f015eaee SK |
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 | |
a63da33c SK |
102 | void Touchprobe::on_idle(void* argument){ |
103 | if( this->logfile == NULL) { | |
f015eaee | 104 | // NOTE: File creation is buggy, a file may appear but writing to it will fail |
a63da33c SK |
105 | this->logfile = fopen( filename.c_str(), "a"); |
106 | } | |
107 | } | |
108 | ||
3779fd74 SK |
109 | void Touchprobe::on_gcode_received(void* argument) |
110 | { | |
111 | Gcode* gcode = static_cast<Gcode*>(argument); | |
314ab8f7 | 112 | Robot* robot = THEKERNEL->robot; |
3779fd74 | 113 | |
4a76020e SK |
114 | if( gcode->has_g) { |
115 | if( gcode->g == 31 ) { | |
78d0e16a MM |
116 | float tmp[3], pos[3], mm[3]; |
117 | int steps[3]; | |
6de8ab5b | 118 | // first wait for an empty queue i.e. no moves left |
314ab8f7 | 119 | THEKERNEL->conveyor->wait_for_empty_queue(); |
6de8ab5b | 120 | |
a63da33c SK |
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 | } | |
6de8ab5b | 129 | if( gcode->has_letter('F') ) { |
da947c62 | 130 | this->probe_rate = robot->to_millimeters( gcode->get_value('F') ) / robot->seconds_per_minute; |
6de8ab5b | 131 | } |
78d0e16a MM |
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; | |
6de8ab5b SK |
135 | |
136 | if( ((abs(steps[0]) > 0 ? 1:0) + (abs(steps[1]) > 0 ? 1:0) + (abs(steps[2]) > 0 ? 1:0)) != 1 ){ | |
a63da33c | 137 | return; //TODO coordinated movement not supported yet |
6de8ab5b SK |
138 | } |
139 | ||
140 | // Enable the motors | |
314ab8f7 | 141 | THEKERNEL->stepper->turn_enable_pins_on(); |
6de8ab5b | 142 | // move |
6de8ab5b | 143 | for(char c='X'; c<='Z'; c++){ |
78d0e16a | 144 | tmp[c - 'X'] = robot->actuators[c - 'X']->steps_per_mm; |
6de8ab5b SK |
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 | |
78d0e16a | 150 | this->steppers[c-'X']->set_speed(this->probe_rate * robot->actuators[c]->steps_per_mm); |
6de8ab5b SK |
151 | this->steppers[c-'X']->move(dir,abs(steps[c-'X'])); |
152 | } | |
153 | ||
78d0e16a | 154 | wait_for_touch(steps); |
a63da33c | 155 | // calculate new position |
6de8ab5b | 156 | for(char c='X'; c<='Z'; c++){ |
78d0e16a | 157 | robot->reset_axis_position(pos[c-'X'] + mm[c-'X'], c-'X'); |
6de8ab5b | 158 | } |
3779fd74 | 159 | |
6de8ab5b | 160 | if( this->should_log ){ |
6de8ab5b | 161 | robot->get_axis_position(pos); |
f015eaee SK |
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(); | |
3779fd74 SK |
164 | } |
165 | } | |
a63da33c | 166 | }else if(gcode->has_m) { |
f015eaee SK |
167 | // log rotation |
168 | // for now this only writes a separator | |
169 | // TODO do a actual log rotation | |
a63da33c SK |
170 | if( this->mcode != 0 && this->should_log && gcode->m == this->mcode){ |
171 | string name; | |
f015eaee SK |
172 | fputs("--\n",logfile); |
173 | flush_log(); | |
a63da33c | 174 | } |
3779fd74 SK |
175 | } |
176 | } | |
f015eaee | 177 |