Commit | Line | Data |
---|---|---|
f80d18b9 L |
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) with additions from Sungeun K. Jeon (https://github.com/chamnit/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 | ||
f80d18b9 L |
8 | #include "libs/nuts_bolts.h" |
9 | #include "libs/RingBuffer.h" | |
10 | #include "../communication/utils/Gcode.h" | |
11 | #include "libs/Module.h" | |
12 | #include "libs/Kernel.h" | |
13 | #include "Timer.h" // mbed.h lib | |
14 | #include "wait_api.h" // mbed.h lib | |
15 | #include "Block.h" | |
16 | #include "Conveyor.h" | |
17 | #include "Planner.h" | |
55456577 | 18 | #include "mri.h" |
61134a65 JM |
19 | #include "checksumm.h" |
20 | #include "Config.h" | |
21 | #include "libs/StreamOutputPool.h" | |
8d54c34c | 22 | #include "ConfigValue.h" |
9e6014a6 | 23 | #include "StepTicker.h" |
e518eb86 | 24 | #include "Robot.h" |
b5708347 | 25 | #include "StepperMotor.h" |
9e6014a6 JM |
26 | |
27 | #include <functional> | |
28 | #include <vector> | |
f80d18b9 | 29 | |
d1d120e1 | 30 | #include "mbed.h" |
edac9072 | 31 | |
d1d120e1 | 32 | #define planner_queue_size_checksum CHECKSUM("planner_queue_size") |
121844b7 | 33 | #define queue_delay_time_ms_checksum CHECKSUM("queue_delay_time_ms") |
9e6014a6 JM |
34 | |
35 | /* | |
f6542ad9 JM |
36 | * The conveyor holds the queue of blocks, takes care of creating them, and starting the executing chain of blocks |
37 | * | |
38 | * The Queue is implemented as a ringbuffer- with a twist | |
39 | * | |
40 | * Since delete() is not thread-safe, we must marshall deletable items out of ISR context | |
41 | * | |
42 | * To do this, we have implmented a *double* ringbuffer- two ringbuffers sharing the same ring, and one index pointer | |
43 | * | |
44 | * as in regular ringbuffers, HEAD always points to a clean, free block. We are free to prepare it as we see fit, at our leisure. | |
45 | * When the block is fully prepared, we increment the head pointer, and from that point we must not touch it anymore. | |
46 | * | |
47 | * also, as in regular ringbuffers, we can 'use' the TAIL block, and increment tail pointer when we're finished with it | |
48 | * | |
49 | * Both of these are implemented here- see queue_head_block() (where head is pushed) and on_idle() (where tail is consumed) | |
50 | * | |
51 | * The double ring is implemented by adding a third index pointer that lives in between head and tail. We call it isr_tail_i. | |
52 | * | |
53 | * in ISR context, we use HEAD as the head pointer, and isr_tail_i as the tail pointer. | |
54 | * As HEAD increments, ISR context can consume the new blocks which appear, and when we're finished with a block, we increment isr_tail_i to signal that they're finished, and ready to be cleaned | |
55 | * | |
56 | * in IDLE context, we use isr_tail_i as the head pointer, and TAIL as the tail pointer. | |
57 | * When isr_tail_i != tail, we clean up the tail block (performing ISR-unsafe delete operations) and consume it (increment tail pointer), returning it to the pool of clean, unused blocks which HEAD is allowed to prepare for queueing | |
58 | * | |
59 | * Thus, our two ringbuffers exist sharing the one ring of blocks, and we safely marshall used blocks from ISR context to IDLE context for safe cleanup. | |
60 | */ | |
9e6014a6 | 61 | |
9e6014a6 JM |
62 | |
63 | Conveyor::Conveyor() | |
64 | { | |
121844b7 | 65 | running = false; |
9e6014a6 | 66 | halted = false; |
f6542ad9 | 67 | allow_fetch = false; |
a19a873f | 68 | flush= false; |
702023f3 MM |
69 | } |
70 | ||
9e6014a6 JM |
71 | void Conveyor::on_module_loaded() |
72 | { | |
b5708347 | 73 | register_for_event(ON_IDLE); |
b375ba1d | 74 | register_for_event(ON_HALT); |
0b3e628f | 75 | |
9e6014a6 | 76 | // Attach to the end_of_move stepper event |
d1d120e1 | 77 | //THEKERNEL->step_ticker->finished_fnc = std::bind( &Conveyor::all_moves_finished, this); |
f6542ad9 | 78 | queue_size = THEKERNEL->config->value(planner_queue_size_checksum)->by_default(32)->as_number(); |
98e30679 | 79 | queue_delay_time_ms = THEKERNEL->config->value(queue_delay_time_ms_checksum)->by_default(100)->as_number(); |
121844b7 JM |
80 | } |
81 | ||
216ef701 | 82 | // we allocate the queue here after config is completed so we do not run out of memory during config |
8a9f9313 | 83 | void Conveyor::start(uint8_t n) |
121844b7 | 84 | { |
8a9f9313 | 85 | Block::n_actuators= n; // set the number of motors which determines how big the tick info vector is |
121844b7 | 86 | queue.resize(queue_size); |
f6542ad9 | 87 | running = true; |
702023f3 MM |
88 | } |
89 | ||
9e6014a6 JM |
90 | void Conveyor::on_halt(void* argument) |
91 | { | |
728477c4 | 92 | if(argument == nullptr) { |
9e6014a6 | 93 | halted = true; |
728477c4 | 94 | flush_queue(); |
9e6014a6 JM |
95 | } else { |
96 | halted = false; | |
728477c4 | 97 | } |
b375ba1d JM |
98 | } |
99 | ||
b5708347 | 100 | void Conveyor::on_idle(void*) |
01b69353 | 101 | { |
9e6014a6 | 102 | if (running) { |
d1d120e1 | 103 | check_queue(); |
9e6014a6 | 104 | } |
f6542ad9 JM |
105 | |
106 | // we can garbage collect the block queue here | |
107 | if (queue.tail_i != queue.isr_tail_i) { | |
108 | if (queue.is_empty()) { | |
109 | __debugbreak(); | |
110 | } else { | |
111 | // Cleanly delete block | |
112 | Block* block = queue.tail_ref(); | |
374d0777 | 113 | block->debug(); |
f6542ad9 JM |
114 | block->clear(); |
115 | queue.consume_tail(); | |
116 | } | |
117 | } | |
f80d18b9 L |
118 | } |
119 | ||
b5708347 JM |
120 | // see if we are idle |
121 | // this checks the block queue is empty, and that the step queue is empty and | |
122 | // checks that all motors are no longer moving | |
123 | bool Conveyor::is_idle() const | |
124 | { | |
f6542ad9 | 125 | if(queue.is_empty()) { |
c8bac202 | 126 | for(auto &a : THEROBOT->actuators) { |
b5708347 JM |
127 | if(a->is_moving()) return false; |
128 | } | |
129 | return true; | |
130 | } | |
131 | ||
132 | return false; | |
133 | } | |
134 | ||
d1d120e1 | 135 | // Wait for the queue to be empty and for all the jobs to finish in step ticker |
c501670b MM |
136 | void Conveyor::wait_for_empty_queue() |
137 | { | |
d1d120e1 | 138 | // wait for the job queue to empty, this means cycling everything on the block queue into the job queue |
e518eb86 | 139 | // forcing them to be jobs |
f6542ad9 | 140 | running = false; // stops on_idle calling check_queue |
728477c4 | 141 | while (!queue.is_empty()) { |
f6542ad9 | 142 | check_queue(true); // forces queue to be made available to stepticker |
d1d120e1 JM |
143 | THEKERNEL->call_event(ON_IDLE, this); |
144 | } | |
145 | ||
e518eb86 | 146 | // now we wait for all motors to stop moving |
b5708347 | 147 | while(!is_idle()) { |
dda52007 | 148 | THEKERNEL->call_event(ON_IDLE, this); |
2134bcf2 | 149 | } |
f6542ad9 | 150 | running = true; |
d1d120e1 | 151 | // returning now means that everything has totally finished |
17c68379 BG |
152 | } |
153 | ||
8698e81a MM |
154 | /* |
155 | * push the pre-prepared head block onto the queue | |
156 | */ | |
2134bcf2 MM |
157 | void Conveyor::queue_head_block() |
158 | { | |
728477c4 JM |
159 | if(halted) { |
160 | // we do not want to stick more stuff on the queue if we are in halt state | |
161 | // clear and release the block on the head | |
162 | queue.head_ref()->clear(); | |
d1d120e1 JM |
163 | return; |
164 | } | |
728477c4 | 165 | |
d1d120e1 JM |
166 | // upstream caller will block on this until there is room in the queue |
167 | while (queue.is_full()) { | |
b5708347 JM |
168 | //check_queue(); |
169 | THEKERNEL->call_event(ON_IDLE, this); // will call check_queue(); | |
728477c4 | 170 | } |
d1d120e1 | 171 | |
d1d120e1 | 172 | queue.produce_head(); |
b5708347 | 173 | |
121844b7 | 174 | // not sure if this is the correct place but we need to turn on the motors if they were not already on |
b5708347 | 175 | THEKERNEL->call_event(ON_ENABLE, (void*)1); // turn all enable pins on |
2134bcf2 MM |
176 | } |
177 | ||
d1d120e1 | 178 | void Conveyor::check_queue(bool force) |
2134bcf2 | 179 | { |
d1d120e1 JM |
180 | static uint32_t last_time_check = us_ticker_read(); |
181 | ||
f6542ad9 JM |
182 | if(queue.is_empty()) { |
183 | allow_fetch = false; | |
e518eb86 JM |
184 | last_time_check = us_ticker_read(); // reset timeout |
185 | return; | |
186 | } | |
d1d120e1 | 187 | |
f6542ad9 JM |
188 | // if we have been waiting for more than the required waiting time and the queue is not empty, or the queue is full, then allow stepticker to get the tail |
189 | // we do this to allow an idle system to pre load the queue a bit so the first few blocks run smoothly. | |
190 | if(force || queue.is_full() || (us_ticker_read() - last_time_check) >= (queue_delay_time_ms * 1000)) { | |
121844b7 | 191 | last_time_check = us_ticker_read(); // reset timeout |
f6542ad9 | 192 | allow_fetch = true; |
b5708347 | 193 | return; |
e518eb86 | 194 | } |
f6542ad9 | 195 | } |
d1d120e1 | 196 | |
f6542ad9 JM |
197 | // called from step ticker ISR |
198 | bool Conveyor::get_next_block(Block **block) | |
199 | { | |
a19a873f JM |
200 | // mark entire queue for GC if flush flag is asserted |
201 | if (flush){ | |
202 | while (queue.isr_tail_i != queue.head_i) { | |
203 | queue.isr_tail_i = queue.next(queue.isr_tail_i); | |
204 | } | |
205 | } | |
f6542ad9 JM |
206 | |
207 | if(queue.isr_tail_i == queue.head_i) return false; // we do not have anything to give | |
208 | ||
a19a873f JM |
209 | // wait for queue to fill up, optimizes planning |
210 | if(!allow_fetch) return false; | |
211 | ||
212 | Block *b= queue.item_ref(queue.isr_tail_i); | |
213 | // we cannot use this now if it is being updated | |
f6542ad9 | 214 | if(!b->locked) { |
a19a873f JM |
215 | if(!b->is_ready) __debugbreak(); // should never happen |
216 | ||
f6542ad9 | 217 | b->is_ticking= true; |
a19a873f JM |
218 | b->recalculate_flag= false; |
219 | ||
f6542ad9 | 220 | *block= b; |
f6542ad9 | 221 | return true; |
2134bcf2 | 222 | } |
f6542ad9 JM |
223 | |
224 | return false; | |
2134bcf2 | 225 | } |
c501670b | 226 | |
a19a873f JM |
227 | // called from step ticker ISR when block is finished, do not do anything slow here |
228 | void Conveyor::block_finished() | |
229 | { | |
230 | // we increment the isr_tail_i so we can get the next block | |
231 | queue.isr_tail_i= queue.next(queue.isr_tail_i); | |
232 | } | |
233 | ||
728477c4 | 234 | /* |
728477c4 JM |
235 | In most cases this will not totally flush the queue, as when streaming |
236 | gcode there is one stalled waiting for space in the queue, in | |
237 | queue_head_block() so after this flush, once main_loop runs again one more | |
238 | gcode gets stuck in the queue, this is bad. Current work around is to call | |
239 | this when the queue in not full and streaming has stopped | |
728477c4 | 240 | */ |
b375ba1d JM |
241 | void Conveyor::flush_queue() |
242 | { | |
f6542ad9 | 243 | allow_fetch = false; |
a19a873f | 244 | flush= true; |
f6542ad9 | 245 | |
d1d120e1 JM |
246 | // TODO force deceleration of last block |
247 | ||
e518eb86 | 248 | // now wait until the job queue has finished and all motors are idle too |
b375ba1d | 249 | wait_for_empty_queue(); |
a19a873f | 250 | flush= false; |
b375ba1d JM |
251 | } |
252 | ||
a617ac35 MM |
253 | // Debug function |
254 | void Conveyor::dump_queue() | |
255 | { | |
9e6014a6 | 256 | for (unsigned int index = queue.tail_i, i = 0; true; index = queue.next(index), i++ ) { |
a617ac35 MM |
257 | THEKERNEL->streams->printf("block %03d > ", i); |
258 | queue.item_ref(index)->debug(); | |
259 | ||
260 | if (index == queue.head_i) | |
261 | break; | |
262 | } | |
263 | } | |
264 | ||
c501670b MM |
265 | // feels hacky, but apparently the way to do it |
266 | #include "HeapRing.cpp" | |
267 | template class HeapRing<Block>; |