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(); | |
4de83d43 | 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 |
6c0d8cf7 | 136 | void Conveyor::wait_for_idle(bool wait_for_motors) |
c501670b | 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 | ||
6c0d8cf7 JM |
146 | if(wait_for_motors) { |
147 | // now we wait for all motors to stop moving | |
148 | while(!is_idle()) { | |
149 | THEKERNEL->call_event(ON_IDLE, this); | |
150 | } | |
2134bcf2 | 151 | } |
6c0d8cf7 | 152 | |
f6542ad9 | 153 | running = true; |
d1d120e1 | 154 | // returning now means that everything has totally finished |
17c68379 BG |
155 | } |
156 | ||
8698e81a MM |
157 | /* |
158 | * push the pre-prepared head block onto the queue | |
159 | */ | |
2134bcf2 MM |
160 | void Conveyor::queue_head_block() |
161 | { | |
5dd280fc JM |
162 | // upstream caller will block on this until there is room in the queue |
163 | while (queue.is_full() && !halted) { | |
164 | //check_queue(); | |
165 | THEKERNEL->call_event(ON_IDLE, this); // will call check_queue(); | |
166 | } | |
167 | ||
728477c4 JM |
168 | if(halted) { |
169 | // we do not want to stick more stuff on the queue if we are in halt state | |
170 | // clear and release the block on the head | |
171 | queue.head_ref()->clear(); | |
5dd280fc | 172 | return; // if we got a halt then we are done here |
728477c4 | 173 | } |
d1d120e1 | 174 | |
d1d120e1 | 175 | queue.produce_head(); |
b5708347 | 176 | |
121844b7 | 177 | // not sure if this is the correct place but we need to turn on the motors if they were not already on |
b5708347 | 178 | THEKERNEL->call_event(ON_ENABLE, (void*)1); // turn all enable pins on |
2134bcf2 MM |
179 | } |
180 | ||
d1d120e1 | 181 | void Conveyor::check_queue(bool force) |
2134bcf2 | 182 | { |
d1d120e1 JM |
183 | static uint32_t last_time_check = us_ticker_read(); |
184 | ||
f6542ad9 JM |
185 | if(queue.is_empty()) { |
186 | allow_fetch = false; | |
e518eb86 JM |
187 | last_time_check = us_ticker_read(); // reset timeout |
188 | return; | |
189 | } | |
d1d120e1 | 190 | |
f6542ad9 JM |
191 | // 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 |
192 | // we do this to allow an idle system to pre load the queue a bit so the first few blocks run smoothly. | |
193 | if(force || queue.is_full() || (us_ticker_read() - last_time_check) >= (queue_delay_time_ms * 1000)) { | |
121844b7 | 194 | last_time_check = us_ticker_read(); // reset timeout |
6c0d8cf7 | 195 | if(!flush) allow_fetch = true; |
b5708347 | 196 | return; |
e518eb86 | 197 | } |
f6542ad9 | 198 | } |
d1d120e1 | 199 | |
f6542ad9 JM |
200 | // called from step ticker ISR |
201 | bool Conveyor::get_next_block(Block **block) | |
202 | { | |
a19a873f JM |
203 | // mark entire queue for GC if flush flag is asserted |
204 | if (flush){ | |
205 | while (queue.isr_tail_i != queue.head_i) { | |
206 | queue.isr_tail_i = queue.next(queue.isr_tail_i); | |
207 | } | |
208 | } | |
f6542ad9 | 209 | |
12f08b7b | 210 | // default the feerate to zero if there is no block available |
97152643 JM |
211 | this->current_feedrate= 0; |
212 | ||
6c0d8cf7 | 213 | if(halted || queue.isr_tail_i == queue.head_i) return false; // we do not have anything to give |
f6542ad9 | 214 | |
a19a873f JM |
215 | // wait for queue to fill up, optimizes planning |
216 | if(!allow_fetch) return false; | |
217 | ||
218 | Block *b= queue.item_ref(queue.isr_tail_i); | |
219 | // we cannot use this now if it is being updated | |
f6542ad9 | 220 | if(!b->locked) { |
a19a873f JM |
221 | if(!b->is_ready) __debugbreak(); // should never happen |
222 | ||
f6542ad9 | 223 | b->is_ticking= true; |
a19a873f | 224 | b->recalculate_flag= false; |
97152643 | 225 | this->current_feedrate= b->nominal_speed; |
f6542ad9 | 226 | *block= b; |
f6542ad9 | 227 | return true; |
2134bcf2 | 228 | } |
f6542ad9 JM |
229 | |
230 | return false; | |
2134bcf2 | 231 | } |
c501670b | 232 | |
a19a873f JM |
233 | // called from step ticker ISR when block is finished, do not do anything slow here |
234 | void Conveyor::block_finished() | |
235 | { | |
236 | // we increment the isr_tail_i so we can get the next block | |
237 | queue.isr_tail_i= queue.next(queue.isr_tail_i); | |
238 | } | |
239 | ||
728477c4 | 240 | /* |
728477c4 JM |
241 | In most cases this will not totally flush the queue, as when streaming |
242 | gcode there is one stalled waiting for space in the queue, in | |
243 | queue_head_block() so after this flush, once main_loop runs again one more | |
244 | gcode gets stuck in the queue, this is bad. Current work around is to call | |
245 | this when the queue in not full and streaming has stopped | |
728477c4 | 246 | */ |
b375ba1d JM |
247 | void Conveyor::flush_queue() |
248 | { | |
f6542ad9 | 249 | allow_fetch = false; |
a19a873f | 250 | flush= true; |
f6542ad9 | 251 | |
d1d120e1 JM |
252 | // TODO force deceleration of last block |
253 | ||
6c0d8cf7 JM |
254 | // now wait until the block queue has been flushed |
255 | wait_for_idle(false); | |
c34e7f52 | 256 | |
a19a873f | 257 | flush= false; |
b375ba1d JM |
258 | } |
259 | ||
a617ac35 MM |
260 | // Debug function |
261 | void Conveyor::dump_queue() | |
262 | { | |
9e6014a6 | 263 | for (unsigned int index = queue.tail_i, i = 0; true; index = queue.next(index), i++ ) { |
a617ac35 MM |
264 | THEKERNEL->streams->printf("block %03d > ", i); |
265 | queue.item_ref(index)->debug(); | |
266 | ||
267 | if (index == queue.head_i) | |
268 | break; | |
269 | } | |
270 | } | |
271 | ||
c501670b MM |
272 | // feels hacky, but apparently the way to do it |
273 | #include "HeapRing.cpp" | |
274 | template class HeapRing<Block>; |