Stéphane Cachat / Mbed 2 deprecated Smoothie

smoothie port to mbed online compiler (smoothieware.org)

Dependencies:   mbed

For documentation, license, ..., please check http://smoothieware.org/

This version has been tested with a 3 axis machine

modules/robot/Block.cpp@0:31e91bb0ef3c, 2012-07-31 (annotated)

Committer:
scachat
Date:
Tue Jul 31 21:11:18 2012 +0000
Revision:
0:31e91bb0ef3c
smoothie port to mbed online compiler

Who changed what in which revision?

UserRevisionLine numberNew contents of line
scachat 0:31e91bb0ef3c 1 /*
scachat 0:31e91bb0ef3c 2 This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
scachat 0:31e91bb0ef3c 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.
scachat 0:31e91bb0ef3c 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.
scachat 0:31e91bb0ef3c 5 You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
scachat 0:31e91bb0ef3c 6 */
scachat 0:31e91bb0ef3c 7
scachat 0:31e91bb0ef3c 8 #include "libs/Module.h"
scachat 0:31e91bb0ef3c 9 #include "libs/Kernel.h"
scachat 0:31e91bb0ef3c 10 #include "libs/nuts_bolts.h"
scachat 0:31e91bb0ef3c 11 #include <math.h>
scachat 0:31e91bb0ef3c 12 #include <string>
scachat 0:31e91bb0ef3c 13 #include "Block.h"
scachat 0:31e91bb0ef3c 14 #include "Planner.h"
scachat 0:31e91bb0ef3c 15 #include "Player.h"
scachat 0:31e91bb0ef3c 16 using std::string;
scachat 0:31e91bb0ef3c 17 #include <vector>
scachat 0:31e91bb0ef3c 18 #include "../communication/utils/Gcode.h"
scachat 0:31e91bb0ef3c 19
scachat 0:31e91bb0ef3c 20 Block::Block(){
scachat 0:31e91bb0ef3c 21 clear_vector(this->steps);
scachat 0:31e91bb0ef3c 22 this->times_taken = 0; // A block can be "taken" by any number of modules, and the next block is not moved to until all the modules have "released" it. This value serves as a tracker.
scachat 0:31e91bb0ef3c 23 this->is_ready = false;
scachat 0:31e91bb0ef3c 24 this->initial_rate = -1;
scachat 0:31e91bb0ef3c 25 this->final_rate = -1;
scachat 0:31e91bb0ef3c 26 }
scachat 0:31e91bb0ef3c 27
scachat 0:31e91bb0ef3c 28 void Block::debug(Kernel* kernel){
scachat 0:31e91bb0ef3c 29 kernel->serial->printf("%p: steps:%4d|%4d|%4d(max:%4d) nominal:r%10d/s%6.1f mm:%9.6f rdelta:%8d acc:%5d dec:%5d rates:%10d>%10d taken:%d ready:%d \r\n", this, this->steps[0], this->steps[1], this->steps[2], this->steps_event_count, this->nominal_rate, this->nominal_speed, this->millimeters, this->rate_delta, this->accelerate_until, this->decelerate_after, this->initial_rate, this->final_rate, this->times_taken, this->is_ready );
scachat 0:31e91bb0ef3c 30 }
scachat 0:31e91bb0ef3c 31
scachat 0:31e91bb0ef3c 32
scachat 0:31e91bb0ef3c 33 // Calculate a braking factor to reach baseline speed which is max_jerk/2, e.g. the
scachat 0:31e91bb0ef3c 34 // speed under which you cannot exceed max_jerk no matter what you do.
scachat 0:31e91bb0ef3c 35 double Block::compute_factor_for_safe_speed(){
scachat 0:31e91bb0ef3c 36 return( this->planner->max_jerk / this->nominal_speed );
scachat 0:31e91bb0ef3c 37 }
scachat 0:31e91bb0ef3c 38
scachat 0:31e91bb0ef3c 39
scachat 0:31e91bb0ef3c 40 // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
scachat 0:31e91bb0ef3c 41 // The factors represent a factor of braking and must be in the range 0.0-1.0.
scachat 0:31e91bb0ef3c 42 // +--------+ <- nominal_rate
scachat 0:31e91bb0ef3c 43 // / \
scachat 0:31e91bb0ef3c 44 // nominal_rate*entry_factor -> + \
scachat 0:31e91bb0ef3c 45 // | + <- nominal_rate*exit_factor
scachat 0:31e91bb0ef3c 46 // +-------------+
scachat 0:31e91bb0ef3c 47 // time -->
scachat 0:31e91bb0ef3c 48 void Block::calculate_trapezoid( double entryfactor, double exitfactor ){
scachat 0:31e91bb0ef3c 49
scachat 0:31e91bb0ef3c 50 this->initial_rate = ceil(this->nominal_rate * entryfactor); // (step/min)
scachat 0:31e91bb0ef3c 51 this->final_rate = ceil(this->nominal_rate * exitfactor); // (step/min)
scachat 0:31e91bb0ef3c 52 double acceleration_per_minute = this->rate_delta * this->planner->kernel->stepper->acceleration_ticks_per_second * 60.0;
scachat 0:31e91bb0ef3c 53 int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_minute ) );
scachat 0:31e91bb0ef3c 54 int decelerate_steps = ceil( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate, -acceleration_per_minute ) );
scachat 0:31e91bb0ef3c 55
scachat 0:31e91bb0ef3c 56 // Calculate the size of Plateau of Nominal Rate.
scachat 0:31e91bb0ef3c 57 int plateau_steps = this->steps_event_count-accelerate_steps-decelerate_steps;
scachat 0:31e91bb0ef3c 58
scachat 0:31e91bb0ef3c 59 // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
scachat 0:31e91bb0ef3c 60 // have to use intersection_distance() to calculate when to abort acceleration and start braking
scachat 0:31e91bb0ef3c 61 // in order to reach the final_rate exactly at the end of this block.
scachat 0:31e91bb0ef3c 62 if (plateau_steps < 0) {
scachat 0:31e91bb0ef3c 63 accelerate_steps = ceil(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_minute, this->steps_event_count));
scachat 0:31e91bb0ef3c 64 accelerate_steps = max( accelerate_steps, 0 ); // Check limits due to numerical round-off
scachat 0:31e91bb0ef3c 65 accelerate_steps = min( accelerate_steps, int(this->steps_event_count) );
scachat 0:31e91bb0ef3c 66 plateau_steps = 0;
scachat 0:31e91bb0ef3c 67 }
scachat 0:31e91bb0ef3c 68
scachat 0:31e91bb0ef3c 69 this->accelerate_until = accelerate_steps;
scachat 0:31e91bb0ef3c 70 this->decelerate_after = accelerate_steps+plateau_steps;
scachat 0:31e91bb0ef3c 71
scachat 0:31e91bb0ef3c 72 // TODO: FIX THIS: DIRTY HACK so that we don't end too early for blocks with 0 as final_rate. Doing the math right would be better. Probably fixed in latest grbl
scachat 0:31e91bb0ef3c 73 if( this->final_rate < 0.01 ){
scachat 0:31e91bb0ef3c 74 this->decelerate_after += ( this->nominal_rate / 60 / this->planner->kernel->stepper->acceleration_ticks_per_second ) * 3;
scachat 0:31e91bb0ef3c 75 }
scachat 0:31e91bb0ef3c 76
scachat 0:31e91bb0ef3c 77 }
scachat 0:31e91bb0ef3c 78
scachat 0:31e91bb0ef3c 79 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
scachat 0:31e91bb0ef3c 80 // given acceleration:
scachat 0:31e91bb0ef3c 81 double Block::estimate_acceleration_distance(double initialrate, double targetrate, double acceleration) {
scachat 0:31e91bb0ef3c 82 return( (targetrate*targetrate-initialrate*initialrate)/(2L*acceleration));
scachat 0:31e91bb0ef3c 83 }
scachat 0:31e91bb0ef3c 84
scachat 0:31e91bb0ef3c 85 // This function gives you the point at which you must start braking (at the rate of -acceleration) if
scachat 0:31e91bb0ef3c 86 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
scachat 0:31e91bb0ef3c 87 // a total travel of distance. This can be used to compute the intersection point between acceleration and
scachat 0:31e91bb0ef3c 88 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
scachat 0:31e91bb0ef3c 89 //
scachat 0:31e91bb0ef3c 90 /* + <- some maximum rate we don't care about
scachat 0:31e91bb0ef3c 91 /|\
scachat 0:31e91bb0ef3c 92 / | \
scachat 0:31e91bb0ef3c 93 / | + <- final_rate
scachat 0:31e91bb0ef3c 94 / | |
scachat 0:31e91bb0ef3c 95 initial_rate -> +----+--+
scachat 0:31e91bb0ef3c 96 ^ ^
scachat 0:31e91bb0ef3c 97 | |
scachat 0:31e91bb0ef3c 98 intersection_distance distance */
scachat 0:31e91bb0ef3c 99 double Block::intersection_distance(double initialrate, double finalrate, double acceleration, double distance) {
scachat 0:31e91bb0ef3c 100 return((2*acceleration*distance-initialrate*initialrate+finalrate*finalrate)/(4*acceleration));
scachat 0:31e91bb0ef3c 101 }
scachat 0:31e91bb0ef3c 102
scachat 0:31e91bb0ef3c 103 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
scachat 0:31e91bb0ef3c 104 // acceleration within the allotted distance.
scachat 0:31e91bb0ef3c 105 inline double max_allowable_speed(double acceleration, double target_velocity, double distance) {
scachat 0:31e91bb0ef3c 106 return(
scachat 0:31e91bb0ef3c 107 sqrt(target_velocity*target_velocity-2L*acceleration*60*60*distance) //Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes
scachat 0:31e91bb0ef3c 108 );
scachat 0:31e91bb0ef3c 109 }
scachat 0:31e91bb0ef3c 110
scachat 0:31e91bb0ef3c 111
scachat 0:31e91bb0ef3c 112 // Called by Planner::recalculate() when scanning the plan from last to first entry.
scachat 0:31e91bb0ef3c 113 void Block::reverse_pass(Block* next, Block* previous){
scachat 0:31e91bb0ef3c 114
scachat 0:31e91bb0ef3c 115 if (next) {
scachat 0:31e91bb0ef3c 116 // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
scachat 0:31e91bb0ef3c 117 // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
scachat 0:31e91bb0ef3c 118 // check for maximum allowable speed reductions to ensure maximum possible planned speed.
scachat 0:31e91bb0ef3c 119 if (this->entry_speed != this->max_entry_speed) {
scachat 0:31e91bb0ef3c 120
scachat 0:31e91bb0ef3c 121 // If nominal length true, max junction speed is guaranteed to be reached. Only compute
scachat 0:31e91bb0ef3c 122 // for max allowable speed if block is decelerating and nominal length is false.
scachat 0:31e91bb0ef3c 123 if ((!this->nominal_length_flag) && (this->max_entry_speed > next->entry_speed)) {
scachat 0:31e91bb0ef3c 124 this->entry_speed = min( this->max_entry_speed, max_allowable_speed(-this->planner->acceleration,next->entry_speed,this->millimeters));
scachat 0:31e91bb0ef3c 125 } else {
scachat 0:31e91bb0ef3c 126 this->entry_speed = this->max_entry_speed;
scachat 0:31e91bb0ef3c 127 }
scachat 0:31e91bb0ef3c 128 this->recalculate_flag = true;
scachat 0:31e91bb0ef3c 129
scachat 0:31e91bb0ef3c 130 }
scachat 0:31e91bb0ef3c 131 } // Skip last block. Already initialized and set for recalculation.
scachat 0:31e91bb0ef3c 132
scachat 0:31e91bb0ef3c 133 }
scachat 0:31e91bb0ef3c 134
scachat 0:31e91bb0ef3c 135
scachat 0:31e91bb0ef3c 136 // Called by Planner::recalculate() when scanning the plan from first to last entry.
scachat 0:31e91bb0ef3c 137 void Block::forward_pass(Block* previous, Block* next){
scachat 0:31e91bb0ef3c 138
scachat 0:31e91bb0ef3c 139 if(!previous) { return; } // Begin planning after buffer_tail
scachat 0:31e91bb0ef3c 140
scachat 0:31e91bb0ef3c 141 // If the previous block is an acceleration block, but it is not long enough to complete the
scachat 0:31e91bb0ef3c 142 // full speed change within the block, we need to adjust the entry speed accordingly. Entry
scachat 0:31e91bb0ef3c 143 // speeds have already been reset, maximized, and reverse planned by reverse planner.
scachat 0:31e91bb0ef3c 144 // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
scachat 0:31e91bb0ef3c 145 if (!previous->nominal_length_flag) {
scachat 0:31e91bb0ef3c 146 if (previous->entry_speed < this->entry_speed) {
scachat 0:31e91bb0ef3c 147 double entry_speed = min( this->entry_speed,
scachat 0:31e91bb0ef3c 148 max_allowable_speed(-this->planner->acceleration,previous->entry_speed,previous->millimeters) );
scachat 0:31e91bb0ef3c 149
scachat 0:31e91bb0ef3c 150 // Check for junction speed change
scachat 0:31e91bb0ef3c 151 if (this->entry_speed != entry_speed) {
scachat 0:31e91bb0ef3c 152 this->entry_speed = entry_speed;
scachat 0:31e91bb0ef3c 153 this->recalculate_flag = true;
scachat 0:31e91bb0ef3c 154 }
scachat 0:31e91bb0ef3c 155 }
scachat 0:31e91bb0ef3c 156 }
scachat 0:31e91bb0ef3c 157
scachat 0:31e91bb0ef3c 158 }
scachat 0:31e91bb0ef3c 159
scachat 0:31e91bb0ef3c 160
scachat 0:31e91bb0ef3c 161 // Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it
scachat 0:31e91bb0ef3c 162 void Block::append_gcode(Gcode* gcode){
scachat 0:31e91bb0ef3c 163 __disable_irq();
scachat 0:31e91bb0ef3c 164 this->gcodes.push_back(*gcode);
scachat 0:31e91bb0ef3c 165 __enable_irq();
scachat 0:31e91bb0ef3c 166 }
scachat 0:31e91bb0ef3c 167
scachat 0:31e91bb0ef3c 168 // The attached gcodes are then poped and the on_gcode_execute event is called with them as a parameter
scachat 0:31e91bb0ef3c 169 void Block::pop_and_execute_gcode(Kernel* &kernel){
scachat 0:31e91bb0ef3c 170 Block* block = const_cast<Block*>(this);
scachat 0:31e91bb0ef3c 171 for(unsigned short index=0; index<block->gcodes.size(); index++){
scachat 0:31e91bb0ef3c 172 kernel->call_event(ON_GCODE_EXECUTE, &(block->gcodes[index]));
scachat 0:31e91bb0ef3c 173 }
scachat 0:31e91bb0ef3c 174 }
scachat 0:31e91bb0ef3c 175
scachat 0:31e91bb0ef3c 176 // Signal the player that this block is ready to be injected into the system
scachat 0:31e91bb0ef3c 177 void Block::ready(){
scachat 0:31e91bb0ef3c 178 this->is_ready = true;
scachat 0:31e91bb0ef3c 179 this->player->new_block_added();
scachat 0:31e91bb0ef3c 180 }
scachat 0:31e91bb0ef3c 181
scachat 0:31e91bb0ef3c 182 // Mark the block as taken by one more module
scachat 0:31e91bb0ef3c 183 void Block::take(){
scachat 0:31e91bb0ef3c 184 this->times_taken++;
scachat 0:31e91bb0ef3c 185 }
scachat 0:31e91bb0ef3c 186
scachat 0:31e91bb0ef3c 187 // Mark the block as no longer taken by one module, go to next block if this free's it
scachat 0:31e91bb0ef3c 188 void Block::release(){
scachat 0:31e91bb0ef3c 189 this->times_taken--;
scachat 0:31e91bb0ef3c 190 if( this->times_taken < 1 ){
scachat 0:31e91bb0ef3c 191 this->player->kernel->call_event(ON_BLOCK_END, this);
scachat 0:31e91bb0ef3c 192 this->pop_and_execute_gcode(this->player->kernel);
scachat 0:31e91bb0ef3c 193 Player* player = this->player;
scachat 0:31e91bb0ef3c 194
scachat 0:31e91bb0ef3c 195 if( player->queue.size() > 0 ){
scachat 0:31e91bb0ef3c 196 player->queue.delete_first();
scachat 0:31e91bb0ef3c 197 }
scachat 0:31e91bb0ef3c 198
scachat 0:31e91bb0ef3c 199 if( player->looking_for_new_block == false ){
scachat 0:31e91bb0ef3c 200 if( player->queue.size() > 0 ){
scachat 0:31e91bb0ef3c 201 Block* candidate = player->queue.get_ref(0);
scachat 0:31e91bb0ef3c 202 if( candidate->is_ready ){
scachat 0:31e91bb0ef3c 203 player->current_block = candidate;
scachat 0:31e91bb0ef3c 204 player->kernel->call_event(ON_BLOCK_BEGIN, player->current_block);
scachat 0:31e91bb0ef3c 205 if( player->current_block->times_taken < 1 ){
scachat 0:31e91bb0ef3c 206 player->current_block->release();
scachat 0:31e91bb0ef3c 207 }
scachat 0:31e91bb0ef3c 208 }else{
scachat 0:31e91bb0ef3c 209
scachat 0:31e91bb0ef3c 210 player->current_block = NULL;
scachat 0:31e91bb0ef3c 211
scachat 0:31e91bb0ef3c 212 }
scachat 0:31e91bb0ef3c 213 }else{
scachat 0:31e91bb0ef3c 214 player->current_block = NULL;
scachat 0:31e91bb0ef3c 215 }
scachat 0:31e91bb0ef3c 216 }
scachat 0:31e91bb0ef3c 217 }
scachat 0:31e91bb0ef3c 218 }
scachat 0:31e91bb0ef3c 219
scachat 0:31e91bb0ef3c 220
scachat 0:31e91bb0ef3c 221