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 | ||
4762e5ac JM |
8 | #include "nuts_bolts.h" |
9 | #include "Gcode.h" | |
10 | #include "Module.h" | |
11 | #include "Kernel.h" | |
f80d18b9 L |
12 | #include "Timer.h" // mbed.h lib |
13 | #include "wait_api.h" // mbed.h lib | |
14 | #include "Block.h" | |
15 | #include "Conveyor.h" | |
16 | #include "Planner.h" | |
55456577 | 17 | #include "mri.h" |
61134a65 JM |
18 | #include "checksumm.h" |
19 | #include "Config.h" | |
4762e5ac | 20 | #include "StreamOutputPool.h" |
8d54c34c | 21 | #include "ConfigValue.h" |
9e6014a6 | 22 | #include "StepTicker.h" |
e518eb86 | 23 | #include "Robot.h" |
b5708347 | 24 | #include "StepperMotor.h" |
9e6014a6 JM |
25 | |
26 | #include <functional> | |
f80d18b9 | 27 | |
d1d120e1 | 28 | #include "mbed.h" |
edac9072 | 29 | |
d1d120e1 | 30 | #define planner_queue_size_checksum CHECKSUM("planner_queue_size") |
121844b7 | 31 | #define queue_delay_time_ms_checksum CHECKSUM("queue_delay_time_ms") |
9e6014a6 JM |
32 | |
33 | /* | |
f6542ad9 JM |
34 | * The conveyor holds the queue of blocks, takes care of creating them, and starting the executing chain of blocks |
35 | * | |
36 | * The Queue is implemented as a ringbuffer- with a twist | |
37 | * | |
38 | * Since delete() is not thread-safe, we must marshall deletable items out of ISR context | |
39 | * | |
40 | * To do this, we have implmented a *double* ringbuffer- two ringbuffers sharing the same ring, and one index pointer | |
41 | * | |
42 | * 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. | |
43 | * When the block is fully prepared, we increment the head pointer, and from that point we must not touch it anymore. | |
44 | * | |
45 | * also, as in regular ringbuffers, we can 'use' the TAIL block, and increment tail pointer when we're finished with it | |
46 | * | |
47 | * Both of these are implemented here- see queue_head_block() (where head is pushed) and on_idle() (where tail is consumed) | |
48 | * | |
49 | * The double ring is implemented by adding a third index pointer that lives in between head and tail. We call it isr_tail_i. | |
50 | * | |
51 | * in ISR context, we use HEAD as the head pointer, and isr_tail_i as the tail pointer. | |
52 | * 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 | |
53 | * | |
54 | * in IDLE context, we use isr_tail_i as the head pointer, and TAIL as the tail pointer. | |
55 | * 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 | |
56 | * | |
57 | * 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. | |
58 | */ | |
9e6014a6 | 59 | |
9e6014a6 JM |
60 | |
61 | Conveyor::Conveyor() | |
62 | { | |
121844b7 | 63 | running = false; |
f6542ad9 | 64 | allow_fetch = false; |
a19a873f | 65 | flush= false; |
702023f3 MM |
66 | } |
67 | ||
9e6014a6 JM |
68 | void Conveyor::on_module_loaded() |
69 | { | |
b5708347 | 70 | register_for_event(ON_IDLE); |
b375ba1d | 71 | register_for_event(ON_HALT); |
0b3e628f | 72 | |
9e6014a6 | 73 | // Attach to the end_of_move stepper event |
d1d120e1 | 74 | //THEKERNEL->step_ticker->finished_fnc = std::bind( &Conveyor::all_moves_finished, this); |
f6542ad9 | 75 | queue_size = THEKERNEL->config->value(planner_queue_size_checksum)->by_default(32)->as_number(); |
98e30679 | 76 | queue_delay_time_ms = THEKERNEL->config->value(queue_delay_time_ms_checksum)->by_default(100)->as_number(); |
121844b7 JM |
77 | } |
78 | ||
216ef701 | 79 | // we allocate the queue here after config is completed so we do not run out of memory during config |
8a9f9313 | 80 | void Conveyor::start(uint8_t n) |
121844b7 | 81 | { |
c0f10f7d | 82 | Block::init(n); // set the number of motors which determines how big the tick info vector is |
121844b7 | 83 | queue.resize(queue_size); |
f6542ad9 | 84 | running = true; |
702023f3 MM |
85 | } |
86 | ||
9e6014a6 JM |
87 | void Conveyor::on_halt(void* argument) |
88 | { | |
728477c4 | 89 | if(argument == nullptr) { |
728477c4 | 90 | flush_queue(); |
728477c4 | 91 | } |
b375ba1d JM |
92 | } |
93 | ||
b5708347 | 94 | void Conveyor::on_idle(void*) |
01b69353 | 95 | { |
9e6014a6 | 96 | if (running) { |
d1d120e1 | 97 | check_queue(); |
9e6014a6 | 98 | } |
f6542ad9 JM |
99 | |
100 | // we can garbage collect the block queue here | |
101 | if (queue.tail_i != queue.isr_tail_i) { | |
102 | if (queue.is_empty()) { | |
103 | __debugbreak(); | |
104 | } else { | |
105 | // Cleanly delete block | |
106 | Block* block = queue.tail_ref(); | |
7671f21c | 107 | //block->debug(); |
f6542ad9 JM |
108 | block->clear(); |
109 | queue.consume_tail(); | |
110 | } | |
111 | } | |
f80d18b9 L |
112 | } |
113 | ||
b5708347 JM |
114 | // see if we are idle |
115 | // this checks the block queue is empty, and that the step queue is empty and | |
116 | // checks that all motors are no longer moving | |
117 | bool Conveyor::is_idle() const | |
118 | { | |
f6542ad9 | 119 | if(queue.is_empty()) { |
c8bac202 | 120 | for(auto &a : THEROBOT->actuators) { |
b5708347 JM |
121 | if(a->is_moving()) return false; |
122 | } | |
123 | return true; | |
124 | } | |
125 | ||
126 | return false; | |
127 | } | |
128 | ||
d1d120e1 | 129 | // Wait for the queue to be empty and for all the jobs to finish in step ticker |
6c0d8cf7 | 130 | void Conveyor::wait_for_idle(bool wait_for_motors) |
c501670b | 131 | { |
d1d120e1 | 132 | // wait for the job queue to empty, this means cycling everything on the block queue into the job queue |
e518eb86 | 133 | // forcing them to be jobs |
f6542ad9 | 134 | running = false; // stops on_idle calling check_queue |
728477c4 | 135 | while (!queue.is_empty()) { |
f6542ad9 | 136 | check_queue(true); // forces queue to be made available to stepticker |
d1d120e1 JM |
137 | THEKERNEL->call_event(ON_IDLE, this); |
138 | } | |
139 | ||
6c0d8cf7 JM |
140 | if(wait_for_motors) { |
141 | // now we wait for all motors to stop moving | |
142 | while(!is_idle()) { | |
143 | THEKERNEL->call_event(ON_IDLE, this); | |
144 | } | |
2134bcf2 | 145 | } |
6c0d8cf7 | 146 | |
f6542ad9 | 147 | running = true; |
d1d120e1 | 148 | // returning now means that everything has totally finished |
17c68379 BG |
149 | } |
150 | ||
8698e81a MM |
151 | /* |
152 | * push the pre-prepared head block onto the queue | |
153 | */ | |
2134bcf2 MM |
154 | void Conveyor::queue_head_block() |
155 | { | |
5dd280fc | 156 | // upstream caller will block on this until there is room in the queue |
39c0196b | 157 | while (queue.is_full() && !THEKERNEL->is_halted()) { |
5dd280fc JM |
158 | //check_queue(); |
159 | THEKERNEL->call_event(ON_IDLE, this); // will call check_queue(); | |
160 | } | |
161 | ||
39c0196b | 162 | if(THEKERNEL->is_halted()) { |
728477c4 JM |
163 | // we do not want to stick more stuff on the queue if we are in halt state |
164 | // clear and release the block on the head | |
165 | queue.head_ref()->clear(); | |
5dd280fc | 166 | return; // if we got a halt then we are done here |
728477c4 | 167 | } |
d1d120e1 | 168 | |
d1d120e1 | 169 | queue.produce_head(); |
b5708347 | 170 | |
121844b7 | 171 | // not sure if this is the correct place but we need to turn on the motors if they were not already on |
b5708347 | 172 | THEKERNEL->call_event(ON_ENABLE, (void*)1); // turn all enable pins on |
2134bcf2 MM |
173 | } |
174 | ||
d1d120e1 | 175 | void Conveyor::check_queue(bool force) |
2134bcf2 | 176 | { |
d1d120e1 JM |
177 | static uint32_t last_time_check = us_ticker_read(); |
178 | ||
f6542ad9 JM |
179 | if(queue.is_empty()) { |
180 | allow_fetch = false; | |
e518eb86 JM |
181 | last_time_check = us_ticker_read(); // reset timeout |
182 | return; | |
183 | } | |
d1d120e1 | 184 | |
f6542ad9 JM |
185 | // 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 |
186 | // we do this to allow an idle system to pre load the queue a bit so the first few blocks run smoothly. | |
187 | if(force || queue.is_full() || (us_ticker_read() - last_time_check) >= (queue_delay_time_ms * 1000)) { | |
121844b7 | 188 | last_time_check = us_ticker_read(); // reset timeout |
6c0d8cf7 | 189 | if(!flush) allow_fetch = true; |
b5708347 | 190 | return; |
e518eb86 | 191 | } |
f6542ad9 | 192 | } |
d1d120e1 | 193 | |
f6542ad9 JM |
194 | // called from step ticker ISR |
195 | bool Conveyor::get_next_block(Block **block) | |
196 | { | |
a19a873f JM |
197 | // mark entire queue for GC if flush flag is asserted |
198 | if (flush){ | |
199 | while (queue.isr_tail_i != queue.head_i) { | |
200 | queue.isr_tail_i = queue.next(queue.isr_tail_i); | |
201 | } | |
202 | } | |
f6542ad9 | 203 | |
12f08b7b | 204 | // default the feerate to zero if there is no block available |
97152643 JM |
205 | this->current_feedrate= 0; |
206 | ||
39c0196b | 207 | if(THEKERNEL->is_halted() || queue.isr_tail_i == queue.head_i) return false; // we do not have anything to give |
f6542ad9 | 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 | 218 | b->recalculate_flag= false; |
97152643 | 219 | this->current_feedrate= b->nominal_speed; |
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 | ||
6c0d8cf7 JM |
248 | // now wait until the block queue has been flushed |
249 | wait_for_idle(false); | |
c34e7f52 | 250 | |
a19a873f | 251 | flush= false; |
b375ba1d JM |
252 | } |
253 | ||
a617ac35 MM |
254 | // Debug function |
255 | void Conveyor::dump_queue() | |
256 | { | |
9e6014a6 | 257 | for (unsigned int index = queue.tail_i, i = 0; true; index = queue.next(index), i++ ) { |
a617ac35 MM |
258 | THEKERNEL->streams->printf("block %03d > ", i); |
259 | queue.item_ref(index)->debug(); | |
260 | ||
261 | if (index == queue.head_i) | |
262 | break; | |
263 | } | |
264 | } |