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(100)->as_number();
  80 }
  81
  82 // we allocate the queue here after config is completed so we do not run out of memory during config
  83 void Conveyor::start(uint8_t n)
  84 {
  85     Block::n_actuators= n; // set the number of motors which determines how big the tick info vector is
  86     queue.resize(queue_size);
  87     running = true;
  88 }
  89
  90 void Conveyor::on_halt(void* argument)
  91 {
  92     if(argument == nullptr) {
  93         halted = true;
  94         flush_queue();
  95     } else {
  96         halted = false;
  97     }
  98 }
  99
 100 void Conveyor::on_idle(void*)
 101 {
 102     if (running) {
 103         check_queue();
 104     }
 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();
 113             //block->debug();
 114             block->clear();
 115             queue.consume_tail();
 116         }
 117     }
 118 }
 119
 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 {
 125     if(queue.is_empty()) {
 126         for(auto &a : THEROBOT->actuators) {
 127             if(a->is_moving()) return false;
 128         }
 129         return true;
 130     }
 131
 132     return false;
 133 }
 134
 135 // Wait for the queue to be empty and for all the jobs to finish in step ticker
 136 void Conveyor::wait_for_idle()
 137 {
 138     // wait for the job queue to empty, this means cycling everything on the block queue into the job queue
 139     // forcing them to be jobs
 140     running = false; // stops on_idle calling check_queue
 141     while (!queue.is_empty()) {
 142         check_queue(true); // forces queue to be made available to stepticker
 143         THEKERNEL->call_event(ON_IDLE, this);
 144     }
 145
 146     // now we wait for all motors to stop moving
 147     while(!is_idle()) {
 148         THEKERNEL->call_event(ON_IDLE, this);
 149     }
 150     running = true;
 151     // returning now means that everything has totally finished
 152 }
 153
 154 /*
 155  * push the pre-prepared head block onto the queue
 156  */
 157 void Conveyor::queue_head_block()
 158 {
 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();
 163         return;
 164     }
 165
 166     // upstream caller will block on this until there is room in the queue
 167     while (queue.is_full()) {
 168         //check_queue();
 169         THEKERNEL->call_event(ON_IDLE, this); // will call check_queue();
 170     }
 171
 172     queue.produce_head();
 173
 174     // not sure if this is the correct place but we need to turn on the motors if they were not already on
 175     THEKERNEL->call_event(ON_ENABLE, (void*)1); // turn all enable pins on
 176 }
 177
 178 void Conveyor::check_queue(bool force)
 179 {
 180     static uint32_t last_time_check = us_ticker_read();
 181
 182     if(queue.is_empty()) {
 183         allow_fetch = false;
 184         last_time_check = us_ticker_read(); // reset timeout
 185         return;
 186     }
 187
 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)) {
 191         last_time_check = us_ticker_read(); // reset timeout
 192         allow_fetch = true;
 193         return;
 194     }
 195 }
 196
 197 // called from step ticker ISR
 198 bool Conveyor::get_next_block(Block **block)
 199 {
 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     }
 206
 207     if(queue.isr_tail_i == queue.head_i) return false; // we do not have anything to give
 208
 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
 214     if(!b->locked) {
 215         if(!b->is_ready) __debugbreak(); // should never happen
 216
 217         b->is_ticking= true;
 218         b->recalculate_flag= false;
 219
 220         *block= b;
 221         return true;
 222     }
 223
 224     return false;
 225 }
 226
 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
 234 /*
 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
 240 */
 241 void Conveyor::flush_queue()
 242 {
 243     allow_fetch = false;
 244     flush= true;
 245
 246     // TODO force deceleration of last block
 247
 248     // now wait until the job queue has finished and all motors are idle too
 249     wait_for_idle();
 250     flush= false;
 251 }
 252
 253 // Debug function
 254 void Conveyor::dump_queue()
 255 {
 256     for (unsigned int index = queue.tail_i, i = 0; true; index = queue.next(index), i++ ) {
 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
 265 // feels hacky, but apparently the way to do it
 266 #include "HeapRing.cpp"
 267 template class HeapRing<Block>;