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/>.
8 #include "nuts_bolts.h"
12 #include "Timer.h" // mbed.h lib
13 #include "wait_api.h" // mbed.h lib
18 #include "checksumm.h"
20 #include "StreamOutputPool.h"
21 #include "ConfigValue.h"
22 #include "StepTicker.h"
24 #include "StepperMotor.h"
30 #define planner_queue_size_checksum CHECKSUM("planner_queue_size")
31 #define queue_delay_time_ms_checksum CHECKSUM("queue_delay_time_ms")
34 * The conveyor holds the queue of blocks, takes care of creating them, and starting the executing chain of blocks
36 * The Queue is implemented as a ringbuffer- with a twist
38 * Since delete() is not thread-safe, we must marshall deletable items out of ISR context
40 * To do this, we have implmented a *double* ringbuffer- two ringbuffers sharing the same ring, and one index pointer
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.
45 * also, as in regular ringbuffers, we can 'use' the TAIL block, and increment tail pointer when we're finished with it
47 * Both of these are implemented here- see queue_head_block() (where head is pushed) and on_idle() (where tail is consumed)
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.
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
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
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.
68 void Conveyor::on_module_loaded()
70 register_for_event(ON_IDLE
);
71 register_for_event(ON_HALT
);
73 // Attach to the end_of_move stepper event
74 //THEKERNEL->step_ticker->finished_fnc = std::bind( &Conveyor::all_moves_finished, this);
75 queue_size
= THEKERNEL
->config
->value(planner_queue_size_checksum
)->by_default(32)->as_number();
76 queue_delay_time_ms
= THEKERNEL
->config
->value(queue_delay_time_ms_checksum
)->by_default(100)->as_number();
79 // we allocate the queue here after config is completed so we do not run out of memory during config
80 void Conveyor::start(uint8_t n
)
82 Block::init(n
); // set the number of motors which determines how big the tick info vector is
83 queue
.resize(queue_size
);
87 void Conveyor::on_halt(void* argument
)
89 if(argument
== nullptr) {
94 void Conveyor::on_idle(void*)
100 // we can garbage collect the block queue here
101 if (queue
.tail_i
!= queue
.isr_tail_i
) {
102 if (queue
.is_empty()) {
105 // Cleanly delete block
106 Block
* block
= queue
.tail_ref();
109 queue
.consume_tail();
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
119 if(queue
.is_empty()) {
120 for(auto &a
: THEROBOT
->actuators
) {
121 if(a
->is_moving()) return false;
129 // Wait for the queue to be empty and for all the jobs to finish in step ticker
130 void Conveyor::wait_for_idle(bool wait_for_motors
)
132 // wait for the job queue to empty, this means cycling everything on the block queue into the job queue
133 // forcing them to be jobs
134 running
= false; // stops on_idle calling check_queue
135 while (!queue
.is_empty()) {
136 check_queue(true); // forces queue to be made available to stepticker
137 THEKERNEL
->call_event(ON_IDLE
, this);
140 if(wait_for_motors
) {
141 // now we wait for all motors to stop moving
143 THEKERNEL
->call_event(ON_IDLE
, this);
148 // returning now means that everything has totally finished
152 * push the pre-prepared head block onto the queue
154 void Conveyor::queue_head_block()
156 // upstream caller will block on this until there is room in the queue
157 while (queue
.is_full() && !THEKERNEL
->is_halted()) {
159 THEKERNEL
->call_event(ON_IDLE
, this); // will call check_queue();
162 if(THEKERNEL
->is_halted()) {
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();
166 return; // if we got a halt then we are done here
169 queue
.produce_head();
171 // not sure if this is the correct place but we need to turn on the motors if they were not already on
172 THEKERNEL
->call_event(ON_ENABLE
, (void*)1); // turn all enable pins on
175 void Conveyor::check_queue(bool force
)
177 static uint32_t last_time_check
= us_ticker_read();
179 if(queue
.is_empty()) {
181 last_time_check
= us_ticker_read(); // reset timeout
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)) {
188 last_time_check
= us_ticker_read(); // reset timeout
189 if(!flush
) allow_fetch
= true;
194 // called from step ticker ISR
195 bool Conveyor::get_next_block(Block
**block
)
197 // mark entire queue for GC if flush flag is asserted
199 while (queue
.isr_tail_i
!= queue
.head_i
) {
200 queue
.isr_tail_i
= queue
.next(queue
.isr_tail_i
);
204 // default the feerate to zero if there is no block available
205 this->current_feedrate
= 0;
207 if(THEKERNEL
->is_halted() || queue
.isr_tail_i
== queue
.head_i
) return false; // we do not have anything to give
209 // wait for queue to fill up, optimizes planning
210 if(!allow_fetch
) return false;
212 Block
*b
= queue
.item_ref(queue
.isr_tail_i
);
213 // we cannot use this now if it is being updated
215 if(!b
->is_ready
) __debugbreak(); // should never happen
218 b
->recalculate_flag
= false;
219 this->current_feedrate
= b
->nominal_speed
;
227 // called from step ticker ISR when block is finished, do not do anything slow here
228 void Conveyor::block_finished()
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
);
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
241 void Conveyor::flush_queue()
246 // TODO force deceleration of last block
248 // now wait until the block queue has been flushed
249 wait_for_idle(false);
255 void Conveyor::dump_queue()
257 for (unsigned int index
= queue
.tail_i
, i
= 0; true; index
= queue
.next(index
), i
++ ) {
258 THEKERNEL
->streams
->printf("block %03d > ", i
);
259 queue
.item_ref(index
)->debug();
261 if (index
== queue
.head_i
)