Commit | Line | Data |
---|---|---|
ca037905 | 1 | /* |
cd011f58 AW |
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. | |
ca037905 | 5 | You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. |
cd011f58 AW |
6 | */ |
7 | ||
feb204be | 8 | /* |
4cff3ded AW |
9 | #include "libs/Module.h" |
10 | #include "libs/Kernel.h" | |
13e4a3f9 | 11 | #include "modules/robot/Player.h" |
4cff3ded AW |
12 | #include "modules/robot/Block.h" |
13 | #include "modules/tools/extruder/Extruder.h" | |
14 | ||
ca037905 MM |
15 | #define extruder_step_pin_checksum 40763 |
16 | #define extruder_dir_pin_checksum 57277 | |
17 | #define extruder_en_pin_checksum 8017 | |
18 | ||
19 | Extruder::Extruder() { | |
436a2cd1 | 20 | this->absolute_mode = true; |
0eb11a06 | 21 | this->direction = 1; |
ded56b35 | 22 | this->acceleration_lock = false; |
d9ebc974 AW |
23 | this->step_counter = 0; |
24 | this->counter_increment = 0; | |
81b547a1 | 25 | this->paused = false; |
436a2cd1 | 26 | } |
4cff3ded AW |
27 | |
28 | void Extruder::on_module_loaded() { | |
f5598f5b | 29 | |
ded56b35 | 30 | // Do not do anything if not enabledd |
ca037905 | 31 | if( this->kernel->config->value( extruder_module_enable_checksum )->by_default(false)->as_bool() == false ){ return; } |
f5598f5b | 32 | |
4cff3ded | 33 | // Settings |
da24d6ae | 34 | this->on_config_reload(this); |
4cff3ded AW |
35 | |
36 | // We work on the same Block as Stepper, so we need to know when it gets a new one and drops one | |
37 | this->register_for_event(ON_BLOCK_BEGIN); | |
38 | this->register_for_event(ON_BLOCK_END); | |
436a2cd1 | 39 | this->register_for_event(ON_GCODE_EXECUTE); |
81b547a1 AW |
40 | this->register_for_event(ON_PLAY); |
41 | this->register_for_event(ON_PAUSE); | |
ca037905 | 42 | |
ded56b35 | 43 | // Start values |
4cff3ded AW |
44 | this->start_position = 0; |
45 | this->target_position = 0; | |
46 | this->current_position = 0; | |
47 | this->current_block = NULL; | |
ded56b35 | 48 | this->mode = OFF; |
ca037905 | 49 | |
ded56b35 AW |
50 | // Update speed every *acceleration_ticks_per_second* |
51 | // TODO: Make this an independent setting | |
d9ebc974 | 52 | this->kernel->slow_ticker->attach( this->kernel->stepper->acceleration_ticks_per_second , this, &Extruder::acceleration_tick ); |
ded56b35 | 53 | |
3b1e82d2 | 54 | // Initiate main_interrupt timer and step reset timer |
ca037905 | 55 | this->kernel->step_ticker->attach( this, &Extruder::stepping_tick ); |
3b1e82d2 AW |
56 | this->kernel->step_ticker->reset_attach( this, &Extruder::reset_step_pin ); |
57 | ||
4cff3ded AW |
58 | } |
59 | ||
2bb8b390 | 60 | // Get config |
da24d6ae | 61 | void Extruder::on_config_reload(void* argument){ |
8c309ca9 | 62 | this->microseconds_per_step_pulse = this->kernel->config->value(microseconds_per_step_pulse_checksum)->by_default(5)->as_number(); |
46455fd5 | 63 | this->steps_per_millimeter = this->kernel->config->value(extruder_steps_per_mm_checksum )->by_default(1)->as_number(); |
ca037905 MM |
64 | this->feed_rate = this->kernel->config->value(default_feed_rate_checksum )->by_default(1)->as_number(); |
65 | this->acceleration = this->kernel->config->value(acceleration_checksum )->by_default(1)->as_number(); | |
66 | ||
67 | this->step_pin = this->kernel->config->value(extruder_step_pin_checksum )->by_default("1.22" )->as_pin()->as_output(); | |
68 | this->dir_pin = this->kernel->config->value(extruder_dir_pin_checksum )->by_default("1.19" )->as_pin()->as_output(); | |
69 | this->en_pin = this->kernel->config->value(extruder_en_pin_checksum )->by_default("0.19" )->as_pin()->as_output(); | |
436a2cd1 AW |
70 | } |
71 | ||
81b547a1 AW |
72 | |
73 | // When the play/pause button is set to pause, or a module calls the ON_PAUSE event | |
74 | void Extruder::on_pause(void* argument){ | |
75 | this->paused = true; | |
76 | } | |
77 | ||
78 | // When the play/pause button is set to play, or a module calls the ON_PLAY event | |
79 | void Extruder::on_play(void* argument){ | |
80 | this->paused = false; | |
81 | } | |
82 | ||
83 | ||
84 | ||
ded56b35 | 85 | // Compute extrusion speed based on parameters and gcode distance of travel |
436a2cd1 AW |
86 | void Extruder::on_gcode_execute(void* argument){ |
87 | Gcode* gcode = static_cast<Gcode*>(argument); | |
ca037905 | 88 | |
436a2cd1 AW |
89 | // Absolute/relative mode |
90 | if( gcode->has_letter('M')){ | |
ca037905 | 91 | int code = (int) gcode->get_value('M'); |
abc7d730 MM |
92 | if( code == 82 ){ this->absolute_mode = true; } |
93 | if( code == 83 ){ this->absolute_mode = false; } | |
94 | if( code == 84 ){ this->en_pin->set(0); } | |
ca037905 MM |
95 | } |
96 | ||
97 | // The mode is OFF by default, and SOLO or FOLLOW only if we need to extrude | |
ded56b35 | 98 | this->mode = OFF; |
ca037905 | 99 | |
1a2d88eb | 100 | if( gcode->has_letter('G') ){ |
ded56b35 | 101 | // G92: Reset extruder position |
1a2d88eb | 102 | if( gcode->get_value('G') == 92 ){ |
1a2d88eb | 103 | if( gcode->has_letter('E') ){ |
e2b4a32b | 104 | this->current_position = gcode->get_value('E'); |
1a2d88eb | 105 | this->target_position = this->current_position; |
ca037905 MM |
106 | this->start_position = this->current_position; |
107 | } | |
436a2cd1 | 108 | }else{ |
ca037905 | 109 | // Extrusion length from 'G' Gcode |
1a2d88eb | 110 | if( gcode->has_letter('E' )){ |
ca037905 | 111 | // Get relative extrusion distance depending on mode ( in absolute mode we must substract target_position ) |
ded56b35 AW |
112 | double relative_extrusion_distance = gcode->get_value('E'); |
113 | if( this->absolute_mode == true ){ relative_extrusion_distance = relative_extrusion_distance - this->target_position; } | |
ca037905 | 114 | |
ded56b35 AW |
115 | // If the robot is moving, we follow it's movement, otherwise, we move alone |
116 | if( fabs(gcode->millimeters_of_travel) < 0.0001 ){ // With floating numbers, we can have 0 != 0 ... beeeh | |
117 | this->mode = SOLO; | |
118 | this->travel_distance = relative_extrusion_distance; | |
119 | if( gcode->has_letter('F') ){ this->feed_rate = gcode->get_value('F'); } | |
1a2d88eb | 120 | }else{ |
ded56b35 | 121 | this->mode = FOLLOW; |
ca037905 MM |
122 | // We move proportionally to the robot's movement |
123 | this->travel_ratio = relative_extrusion_distance / gcode->millimeters_of_travel; | |
124 | } | |
125 | ||
126 | this->en_pin->set(1); | |
127 | } | |
1a2d88eb | 128 | } |
ca037905 MM |
129 | } |
130 | ||
da24d6ae AW |
131 | } |
132 | ||
ded56b35 | 133 | // When a new block begins, either follow the robot, or step by ourselves ( or stay back and do nothing ) |
4cff3ded AW |
134 | void Extruder::on_block_begin(void* argument){ |
135 | Block* block = static_cast<Block*>(argument); | |
ded56b35 AW |
136 | if( this->mode == SOLO ){ |
137 | // In solo mode we take the block so we can move even if the stepper has nothing to do | |
ca037905 MM |
138 | block->take(); |
139 | this->current_block = block; | |
e2b4a32b | 140 | this->start_position = this->target_position; |
0eb11a06 | 141 | this->target_position = this->start_position + this->travel_distance ; |
ded56b35 AW |
142 | this->travel_ratio = 0.2; // TODO : Make a real acceleration thing |
143 | if( this->target_position > this->current_position ){ this->direction = 1; }else if( this->target_position < this->current_position ){ this->direction = -1; } | |
144 | this->set_speed(int(floor((this->feed_rate/60)*this->steps_per_millimeter)));//Speed in steps per second | |
145 | }else if( this->mode == FOLLOW ){ | |
1a2d88eb | 146 | // In non-solo mode, we just follow the stepper module |
ca037905 | 147 | this->current_block = block; |
e2b4a32b AW |
148 | this->start_position = this->target_position; |
149 | this->target_position = this->start_position + ( this->current_block->millimeters * this->travel_ratio ); | |
ded56b35 | 150 | if( this->target_position > this->current_position ){ this->direction = 1; }else if( this->target_position < this->current_position ){ this->direction = -1; } |
8b8b3339 | 151 | this->acceleration_tick(0); |
ca037905 | 152 | } |
e2b4a32b | 153 | |
4cff3ded AW |
154 | } |
155 | ||
ded56b35 | 156 | // When a block ends, pause the stepping interrupt |
4cff3ded AW |
157 | void Extruder::on_block_end(void* argument){ |
158 | Block* block = static_cast<Block*>(argument); | |
ca037905 MM |
159 | this->current_block = NULL; |
160 | } | |
4cff3ded | 161 | |
ded56b35 | 162 | // Called periodically to change the speed to match acceleration or to match the speed of the robot |
8b8b3339 | 163 | uint32_t Extruder::acceleration_tick(uint32_t dummy){ |
1a2d88eb | 164 | |
ca037905 | 165 | // Avoid trying to work when we really shouldn't ( between blocks or re-entry ) |
8b8b3339 | 166 | if( this->current_block == NULL || this->acceleration_lock || this->paused ){ return 0; } |
ded56b35 | 167 | this->acceleration_lock = true; |
ca037905 | 168 | |
ded56b35 AW |
169 | // In solo mode, we mode independently from the robot |
170 | if( this->mode == SOLO ){ | |
ca037905 | 171 | // TODO : Do real acceleration here |
ded56b35 AW |
172 | this->travel_ratio += 0.03; |
173 | if( this->travel_ratio > 1 ){ this->travel_ratio = 1; } | |
174 | this->set_speed( int(floor(((this->feed_rate/60)*this->steps_per_millimeter)*this->travel_ratio)) ); // Speed in steps per second | |
ca037905 MM |
175 | |
176 | // In follow mode we match the speed of the robot, + eventually advance | |
ded56b35 AW |
177 | }else if( this->mode == FOLLOW ){ |
178 | Stepper* stepper = this->kernel->stepper; // Just for convenience | |
ca037905 MM |
179 | |
180 | // Strategy : | |
ded56b35 AW |
181 | // * Find where in the block will the stepper be at the next tick ( if the block will have ended then, don't change speed ) |
182 | // * Find what position this is for us | |
183 | // * Find what speed we must go at to be at that position for the next acceleration tick | |
184 | // TODO : This works, but PLEASE PLEASE PLEASE if you know a better way to do it, do it better, I don't find this elegant at all, it's just the best I could think of | |
ca037905 MM |
185 | // UPDATE: Yes, this sucks, I have ideas on how to do it better. If this is really bugging you, open a ticket and I'll make it a priority |
186 | ||
187 | int ticks_forward = 3; | |
188 | // We need to take those values here, and then use those instead of the live values, because using the live values inside the loop can break things ( infinite loops etc ... ) | |
ded56b35 | 189 | double next_stepper_rate = stepper->trapezoid_adjusted_rate; |
ca037905 | 190 | double step_events_completed = (double(double(stepper->step_events_completed)/double(1<<16))); |
ded56b35 | 191 | double position = ( this->current_position - this->start_position ) * this->direction ; |
ca037905 MM |
192 | double length = fabs( this->start_position - this->target_position ); |
193 | double last_ratio = -1; | |
ded56b35 AW |
194 | |
195 | // Do the startegy above, but if it does not work, look a bit further and try again, and again ... | |
ca037905 | 196 | while(1){ |
ded56b35 | 197 | |
ca037905 | 198 | // Find the position where we should be at the next tick |
ded56b35 | 199 | double next_ratio = double( step_events_completed + ( next_stepper_rate / 60 / ((double(stepper->acceleration_ticks_per_second)/ticks_forward)) ) ) / double( this->current_block->steps_event_count ); |
ca037905 MM |
200 | double next_relative_position = ( length * next_ratio ); |
201 | ||
202 | // Advance | |
ded56b35 | 203 | // TODO: Proper advance configuration |
d9ebc974 | 204 | double advance = double(next_stepper_rate) * ( 0.00001 * 0.15 ) * 0.4 ; |
ca037905 MM |
205 | //double advance = 0; |
206 | next_relative_position += ( advance ); | |
207 | ||
ded56b35 | 208 | // TODO : all of those "if->return" is very hacky, we should do the math in a way where most of those don't happen, but that requires doing tons of drawing ... |
ca037905 | 209 | if( last_ratio == next_ratio ){ this->acceleration_lock = false; return 0; }else{ last_ratio = next_ratio; } |
8b8b3339 AW |
210 | if( next_ratio == 0 || next_ratio > 1 ){ this->acceleration_lock = false; return 0; } |
211 | if( ticks_forward > 1000 ){ this->acceleration_lock = false; return 0; } // This is very ugly | |
ded56b35 AW |
212 | |
213 | // Hack : We have not looked far enough, we compute how far ahead we must look to get a relevant value | |
ca037905 MM |
214 | if( position > next_relative_position ){ |
215 | double far_back = position - next_relative_position; | |
ded56b35 AW |
216 | double far_back_ratio = far_back / length; |
217 | double move_duration = double( this->current_block->steps_event_count ) / ( double(next_stepper_rate) / 60 ) ; | |
ca037905 MM |
218 | double ticks_in_a_move = round( stepper->acceleration_ticks_per_second * move_duration ); |
219 | double ratio_per_tick = 1 / ticks_in_a_move; | |
ded56b35 AW |
220 | double ticks_to_equilibrium = ceil(far_back_ratio / ratio_per_tick) + 1; |
221 | ticks_forward += ticks_to_equilibrium; | |
222 | // Because this is a loop, and we can be interrupted by the stepping interrupt, if that interrupt changes block, the new block may not be solo, and we may get trapped into an infinite loop | |
8b8b3339 | 223 | if( this->mode != FOLLOW ){ this->acceleration_lock = false; return 0; } |
ca037905 | 224 | continue; |
ded56b35 | 225 | } |
ca037905 MM |
226 | |
227 | // Finally, compute the speed to get to that next position | |
ded56b35 AW |
228 | double next_absolute_position = this->start_position + ( this->direction * next_relative_position ); |
229 | double steps_to_next_tick = ( next_relative_position - position ) * this->steps_per_millimeter; | |
230 | double speed_to_next_tick = steps_to_next_tick / ( 1 / double(double(this->kernel->stepper->acceleration_ticks_per_second) / ticks_forward) ); | |
ca037905 MM |
231 | |
232 | // Change stepping speed | |
ded56b35 AW |
233 | this->set_speed( speed_to_next_tick ); |
234 | ||
235 | this->acceleration_lock = false; | |
8b8b3339 | 236 | return 0; |
ecc402eb | 237 | } |
0eb11a06 | 238 | } |
1a2d88eb | 239 | |
ded56b35 | 240 | this->acceleration_lock = false; |
ca037905 | 241 | return 0; |
ded56b35 | 242 | } |
1a2d88eb | 243 | |
ded56b35 AW |
244 | // Convenience function to set stepping speed |
245 | void Extruder::set_speed( int steps_per_second ){ | |
ca037905 | 246 | |
cd011f58 | 247 | if( steps_per_second < 10 ){ steps_per_second = 10; } |
ca037905 MM |
248 | |
249 | // TODO : Proper limit config value | |
250 | if( steps_per_second > (this->feed_rate*double(this->steps_per_millimeter))/60 ){ | |
251 | steps_per_second = (this->feed_rate*double(this->steps_per_millimeter))/60; | |
0eb11a06 | 252 | } |
1a2d88eb | 253 | |
3b1e82d2 | 254 | this->counter_increment = int(floor(double(1<<16)/double(this->kernel->stepper->base_stepping_frequency / steps_per_second))); |
ca037905 | 255 | |
4cff3ded AW |
256 | } |
257 | ||
8b8b3339 AW |
258 | inline uint32_t Extruder::stepping_tick(uint32_t dummy){ |
259 | if( this->paused ){ return 0; } | |
4cff3ded | 260 | |
3b1e82d2 AW |
261 | this->step_counter += this->counter_increment; |
262 | if( this->step_counter > 1<<16 ){ | |
263 | this->step_counter -= 1<<16; | |
4cff3ded | 264 | |
ca037905 MM |
265 | // If we still have steps to do |
266 | // TODO: Step using the same timer as the robot, and count steps instead of absolute float position | |
267 | if( ( this->current_position < this->target_position && this->direction == 1 ) || ( this->current_position > this->target_position && this->direction == -1 ) ){ | |
3b1e82d2 | 268 | this->current_position += (double(double(1)/double(this->steps_per_millimeter)))*double(this->direction); |
ca037905 MM |
269 | this->dir_pin->set((this->direction > 0) ? 1 : 0); |
270 | this->step_pin->set(1); | |
3b1e82d2 AW |
271 | }else{ |
272 | // Move finished | |
273 | if( this->mode == SOLO && this->current_block != NULL ){ | |
274 | // In follow mode, the robot takes and releases the block, in solo mode we do | |
ca037905 MM |
275 | this->current_block->release(); |
276 | } | |
3b1e82d2 AW |
277 | } |
278 | } | |
ca037905 | 279 | return 0; |
3b1e82d2 | 280 | } |
4cff3ded | 281 | |
8b8b3339 | 282 | uint32_t Extruder::reset_step_pin(uint32_t dummy){ |
ca037905 MM |
283 | this->step_pin->set(0); |
284 | return 0; | |
3b1e82d2 | 285 | } |
feb204be AW |
286 | |
287 | */ | |
288 | ||
289 | ||
290 |