src/modules/robot/Conveyor.cpp

   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
   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"
  18 #include "mri.h"
  19 #include "checksumm.h"
  20 #include "Config.h"
  21 #include "libs/StreamOutputPool.h"
  22 #include "ConfigValue.h"
  23 #include "StepTicker.h"
  24 #include "Robot.h"
  25 #include "StepperMotor.h"
  26
  27 #include <functional>
  28 #include <vector>
  29
  30 #include "mbed.h"
  31
  32 #define planner_queue_size_checksum CHECKSUM("planner_queue_size")
  33 #define queue_delay_time_ms_checksum CHECKSUM("queue_delay_time_ms")
  34
  35 /*
  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  */
  61
  62
  63 Conveyor::Conveyor()
  64 {
  65     running = false;
  66     halted = false;
  67     allow_fetch = false;
  68     flush= false;
  69 }
  70
  71 void Conveyor::on_module_loaded()
  72 {
  73     register_for_event(ON_IDLE);
  74     register_for_event(ON_HALT);
  75
  76     // Attach to the end_of_move stepper event
  77     //THEKERNEL->step_ticker->finished_fnc = std::bind( &Conveyor::all_moves_finished, this);
  78     queue_size = THEKERNEL->config->value(planner_queue_size_checksum)->by_default(32)->as_number();
  79     queue_delay_time_ms = THEKERNEL->config->value(queue_delay_time_ms_checksum)->by_default(400)->as_number();
  80 }
  81
  82 // we allocate the queue here after cpnfig is completed so we do not run out of memory during config
  83 void Conveyor::start()
  84 {
  85     queue.resize(queue_size);
  86     running = true;
  87 }
  88
  89 void Conveyor::on_halt(void* argument)
  90 {
  91     if(argument == nullptr) {
  92         halted = true;
  93         flush_queue();
  94     } else {
  95         halted = false;
  96     }
  97 }
  98
  99 void Conveyor::on_idle(void*)
 100 {
 101     if (running) {
 102         check_queue();
 103     }
 104
 105     // we can garbage collect the block queue here
 106     if (queue.tail_i != queue.isr_tail_i) {
 107         if (queue.is_empty()) {
 108             __debugbreak();
 109         } else {
 110             // Cleanly delete block
 111             Block* block = queue.tail_ref();
 112             //block->debug();
 113             block->clear();
 114             queue.consume_tail();
 115         }
 116     }
 117 }
 118
 119 // see if we are idle
 120 // this checks the block queue is empty, and that the step queue is empty and
 121 // checks that all motors are no longer moving
 122 bool Conveyor::is_idle() const
 123 {
 124     if(queue.is_empty()) {
 125         for(auto &a : THEROBOT->actuators) {
 126             if(a->is_moving()) return false;
 127         }
 128         return true;
 129     }
 130
 131     return false;
 132 }
 133
 134 // Wait for the queue to be empty and for all the jobs to finish in step ticker
 135 void Conveyor::wait_for_empty_queue()
 136 {
 137     // wait for the job queue to empty, this means cycling everything on the block queue into the job queue
 138     // forcing them to be jobs
 139     running = false; // stops on_idle calling check_queue
 140     while (!queue.is_empty()) {
 141         check_queue(true); // forces queue to be made available to stepticker
 142         THEKERNEL->call_event(ON_IDLE, this);
 143     }
 144
 145     // now we wait for all motors to stop moving
 146     while(!is_idle()) {
 147         THEKERNEL->call_event(ON_IDLE, this);
 148     }
 149     running = true;
 150     // returning now means that everything has totally finished
 151 }
 152
 153 /*
 154  * push the pre-prepared head block onto the queue
 155  */
 156 void Conveyor::queue_head_block()
 157 {
 158     if(halted) {
 159         // we do not want to stick more stuff on the queue if we are in halt state
 160         // clear and release the block on the head
 161         queue.head_ref()->clear();
 162         return;
 163     }
 164
 165     // upstream caller will block on this until there is room in the queue
 166     while (queue.is_full()) {
 167         //check_queue();
 168         THEKERNEL->call_event(ON_IDLE, this); // will call check_queue();
 169     }
 170
 171     queue.produce_head();
 172
 173     // not sure if this is the correct place but we need to turn on the motors if they were not already on
 174     THEKERNEL->call_event(ON_ENABLE, (void*)1); // turn all enable pins on
 175 }
 176
 177 void Conveyor::check_queue(bool force)
 178 {
 179     static uint32_t last_time_check = us_ticker_read();
 180
 181     if(queue.is_empty()) {
 182         allow_fetch = false;
 183         last_time_check = us_ticker_read(); // reset timeout
 184         return;
 185     }
 186
 187     // 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
 188     // we do this to allow an idle system to pre load the queue a bit so the first few blocks run smoothly.
 189     if(force || queue.is_full() || (us_ticker_read() - last_time_check) >= (queue_delay_time_ms * 1000)) {
 190         last_time_check = us_ticker_read(); // reset timeout
 191         allow_fetch = true;
 192         return;
 193     }
 194 }
 195
 196 // called from step ticker ISR
 197 bool Conveyor::get_next_block(Block **block)
 198 {
 199     // mark entire queue for GC if flush flag is asserted
 200     if (flush){
 201         while (queue.isr_tail_i != queue.head_i) {
 202             queue.isr_tail_i = queue.next(queue.isr_tail_i);
 203         }
 204     }
 205
 206     if(queue.isr_tail_i == queue.head_i) return false; // we do not have anything to give
 207
 208     // wait for queue to fill up, optimizes planning
 209     if(!allow_fetch) return false;
 210
 211     Block *b= queue.item_ref(queue.isr_tail_i);
 212     // we cannot use this now if it is being updated
 213     if(!b->locked) {
 214         if(!b->is_ready) __debugbreak(); // should never happen
 215
 216         b->is_ticking= true;
 217         b->recalculate_flag= false;
 218
 219         *block= b;
 220         return true;
 221     }
 222
 223     return false;
 224 }
 225
 226 // called from step ticker ISR when block is finished, do not do anything slow here
 227 void Conveyor::block_finished()
 228 {
 229     // we increment the isr_tail_i so we can get the next block
 230     queue.isr_tail_i= queue.next(queue.isr_tail_i);
 231 }
 232
 233 /*
 234     In most cases this will not totally flush the queue, as when streaming
 235     gcode there is one stalled waiting for space in the queue, in
 236     queue_head_block() so after this flush, once main_loop runs again one more
 237     gcode gets stuck in the queue, this is bad. Current work around is to call
 238     this when the queue in not full and streaming has stopped
 239 */
 240 void Conveyor::flush_queue()
 241 {
 242     allow_fetch = false;
 243     flush= true;
 244
 245     // TODO force deceleration of last block
 246
 247     // now wait until the job queue has finished and all motors are idle too
 248     wait_for_empty_queue();
 249     flush= false;
 250 }
 251
 252 // Debug function
 253 void Conveyor::dump_queue()
 254 {
 255     for (unsigned int index = queue.tail_i, i = 0; true; index = queue.next(index), i++ ) {
 256         THEKERNEL->streams->printf("block %03d > ", i);
 257         queue.item_ref(index)->debug();
 258
 259         if (index == queue.head_i)
 260             break;
 261     }
 262 }
 263
 264 // feels hacky, but apparently the way to do it
 265 #include "HeapRing.cpp"
 266 template class HeapRing<Block>;