X-Git-Url: http://git.hcoop.net/clinton/Smoothieware.git/blobdiff_plain/1146e870f536078f4c06dffa74010f05c0d4b6b3..49de901734414b04c54cd1890f3a6bd2a03e4d49:/src/modules/robot/Block.cpp diff --git a/src/modules/robot/Block.cpp b/src/modules/robot/Block.cpp index 1b016e58..9f38d5f5 100644 --- a/src/modules/robot/Block.cpp +++ b/src/modules/robot/Block.cpp @@ -8,7 +8,7 @@ #include "libs/Module.h" #include "libs/Kernel.h" #include "libs/nuts_bolts.h" -#include +#include #include #include "Block.h" #include "Planner.h" @@ -16,14 +16,17 @@ #include "Gcode.h" #include "libs/StreamOutputPool.h" #include "StepTicker.h" +#include "platform_memory.h" #include "mri.h" +#include using std::string; -#include #define STEP_TICKER_FREQUENCY THEKERNEL->step_ticker->get_frequency() -#define STEP_TICKER_FREQUENCY_2 (STEP_TICKER_FREQUENCY*STEP_TICKER_FREQUENCY) + +uint8_t Block::n_actuators= 0; +double Block::fp_scale= 0; // A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves. // It's stacked on a queue, and that queue is then executed in order, to move the motors. @@ -32,15 +35,19 @@ using std::string; Block::Block() { + tick_info= nullptr; clear(); } +void Block::init(uint8_t n) +{ + n_actuators= n; + fp_scale= (double)STEPTICKER_FPSCALE / pow((double)STEP_TICKER_FREQUENCY, 2.0); // we scale up by fixed point offset first to avoid tiny values +} + void Block::clear() { - //commands.clear(); - //travel_distances.clear(); - //gcodes.clear(); - //std::vector().swap(gcodes); // this resizes the vector releasing its memory + is_ready = false; this->steps.fill(0); @@ -49,8 +56,8 @@ void Block::clear() nominal_speed = 0.0F; millimeters = 0.0F; entry_speed = 0.0F; - //exit_speed = 0.0F; - acceleration = 100.0F; // we don't want to get devide by zeroes if this is not set + exit_speed = 0.0F; + acceleration = 100.0F; // we don't want to get divide by zeroes if this is not set initial_rate = 0.0F; accelerate_until = 0; decelerate_after = 0; @@ -58,30 +65,57 @@ void Block::clear() recalculate_flag = false; nominal_length_flag = false; max_entry_speed = 0.0F; - is_ready = false; + is_ticking = false; + is_g123 = false; + locked = false; + s_value = 0.0F; - acceleration_per_tick= 0; - deceleration_per_tick= 0; total_move_ticks= 0; + if(tick_info == nullptr) { + // we create this once for this block + tick_info= new tickinfo_t[n_actuators]; //(tickinfo_t *)malloc(sizeof(tickinfo_t) * n_actuators); + if(tick_info == nullptr) { + // if we ran out of memory in AHB0 just stop here + __debugbreak(); + } + } + + for(int i = 0; i < n_actuators; ++i) { + tick_info[i].steps_per_tick= 0; + tick_info[i].counter= 0; + tick_info[i].acceleration_change= 0; + tick_info[i].deceleration_change= 0; + tick_info[i].plateau_rate= 0; + tick_info[i].steps_to_move= 0; + tick_info[i].step_count= 0; + tick_info[i].next_accel_event= 0; + } } void Block::debug() const { - THEKERNEL->streams->printf("%p: steps:X%04lu Y%04lu Z%04lu(max:%4lu) nominal:r%6.1f/s%6.1f mm:%9.6f acc:%5lu dec:%5lu rates:%10.4f entry/max: %10.4f/%10.4f ready:%d recalc:%d nomlen:%d time:%f\r\n", - this, - this->steps[0], - this->steps[1], - this->steps[2], + THEKERNEL->streams->printf("%p: steps-X:%lu Y:%lu Z:%lu ", this, this->steps[0], this->steps[1], this->steps[2]); + for (size_t i = E_AXIS; i < n_actuators; ++i) { + THEKERNEL->streams->printf("%c:%lu ", 'A' + i-E_AXIS, this->steps[i]); + } + THEKERNEL->streams->printf("(max:%lu) nominal:r%1.4f/s%1.4f mm:%1.4f acc:%1.2f accu:%lu decu:%lu ticks:%lu rates:%1.4f/%1.4f entry/max:%1.4f/%1.4f exit:%1.4f primary:%d ready:%d locked:%d ticking:%d recalc:%d nomlen:%d time:%f\r\n", this->steps_event_count, this->nominal_rate, this->nominal_speed, this->millimeters, + this->acceleration, this->accelerate_until, this->decelerate_after, + this->total_move_ticks, this->initial_rate, + this->maximum_rate, this->entry_speed, this->max_entry_speed, + this->exit_speed, + this->primary_axis, this->is_ready, + this->locked, + this->is_ticking, recalculate_flag ? 1 : 0, nominal_length_flag ? 1 : 0, total_move_ticks/STEP_TICKER_FREQUENCY @@ -100,6 +134,9 @@ void Block::debug() const */ void Block::calculate_trapezoid( float entryspeed, float exitspeed ) { + // if block is currently executing, don't touch anything! + if (is_ticking) return; + float initial_rate = this->nominal_rate * (entryspeed / this->nominal_speed); // steps/sec float final_rate = this->nominal_rate * (exitspeed / this->nominal_speed); //printf("Initial rate: %f, final_rate: %f\n", initial_rate, final_rate); @@ -167,26 +204,24 @@ void Block::calculate_trapezoid( float entryspeed, float exitspeed ) float acceleration_in_steps = (acceleration_time > 0.0F ) ? ( this->maximum_rate - initial_rate ) / acceleration_time : 0; float deceleration_in_steps = (deceleration_time > 0.0F ) ? ( this->maximum_rate - final_rate ) / deceleration_time : 0; + // we have a potential race condition here as we could get interrupted anywhere in the middle of this call, we need to lock + // the updates to the blocks to get around it + this->locked= true; // Now figure out the two acceleration ramp change events in ticks this->accelerate_until = acceleration_ticks; this->decelerate_after = total_move_ticks - deceleration_ticks; - // Now figure out the acceleration PER TICK, this should ideally be held as a float, even a double if possible as it's very critical to the block timing - // steps/tick^2 - - this->acceleration_per_tick = acceleration_in_steps / STEP_TICKER_FREQUENCY_2; - this->deceleration_per_tick = deceleration_in_steps / STEP_TICKER_FREQUENCY_2; - // We now have everything we need for this block to call a Steppermotor->move method !!!! // Theorically, if accel is done per tick, the speed curve should be perfect. - - // We need this to call move() this->total_move_ticks = total_move_ticks; - //puts "accelerate_until: #{this->accelerate_until}, decelerate_after: #{this->decelerate_after}, acceleration_per_tick: #{this->acceleration_per_tick}, total_move_ticks: #{this->total_move_ticks}" - this->initial_rate = initial_rate; - //this->exit_speed = exitspeed; + this->exit_speed = exitspeed; + + // prepare the block for stepticker + this->prepare(acceleration_in_steps, deceleration_in_steps); + + this->locked= false; } // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the @@ -251,9 +286,8 @@ float Block::max_exit_speed() { // if block is currently executing, return cached exit speed from calculate_trapezoid // this ensures that a block following a currently executing block will have correct entry speed - // FIXME - // if (times_taken) - // return exit_speed; + if(is_ticking) + return this->exit_speed; // if nominal_length_flag is asserted // we are guaranteed to reach nominal speed regardless of entry speed @@ -267,30 +301,72 @@ float Block::max_exit_speed() return min(max, nominal_speed); } -// Gcodes are attached to their respective blocks so that on_gcode_execute can be called with it -// void Block::append_gcode(Gcode* gcode) -// { -// Gcode new_gcode = *gcode; -// new_gcode.strip_parameters(); // optimization to save memory we strip off the XYZIJK parameters from the saved command -// gcodes.push_back(new_gcode); -// } - -// void Block::begin() -// { -// // can no longer be used in planning -// recalculate_flag = false; - -// // TODO probably should remove this -// if (!is_ready) -// __debugbreak(); - -// } - -// Mark the block as finished -//void Block::release() -//{ - // if (is_ready) { - // is_ready = false; - // THEKERNEL->conveyor->on_block_end(this); - // } -//} +// prepare block for the step ticker, called everytime the block changes +// this is done during planning so does not delay tick generation and step ticker can simply grab the next block during the interrupt +void Block::prepare(float acceleration_in_steps, float deceleration_in_steps) +{ + + float inv = 1.0F / this->steps_event_count; + + // Now figure out the acceleration PER TICK, this should ideally be held as a double as it's very critical to the block timing + // steps/tick^2 + // was.... + // float acceleration_per_tick = acceleration_in_steps / STEP_TICKER_FREQUENCY_2; // that is 100,000² too big for a float + // float deceleration_per_tick = deceleration_in_steps / STEP_TICKER_FREQUENCY_2; + double acceleration_per_tick = acceleration_in_steps * fp_scale; // this is now scaled to fit a 2.30 fixed point number + double deceleration_per_tick = deceleration_in_steps * fp_scale; + + for (uint8_t m = 0; m < n_actuators; m++) { + uint32_t steps = this->steps[m]; + this->tick_info[m].steps_to_move = steps; + if(steps == 0) continue; + + float aratio = inv * steps; + + this->tick_info[m].steps_per_tick = (int64_t)round((((double)this->initial_rate * aratio) / STEP_TICKER_FREQUENCY) * STEPTICKER_FPSCALE); // steps/sec / tick frequency to get steps per tick in 2.62 fixed point + this->tick_info[m].counter = 0; // 2.62 fixed point + this->tick_info[m].step_count = 0; + this->tick_info[m].next_accel_event = this->total_move_ticks + 1; + + double acceleration_change = 0; + if(this->accelerate_until != 0) { // If the next accel event is the end of accel + this->tick_info[m].next_accel_event = this->accelerate_until; + acceleration_change = acceleration_per_tick; + + } else if(this->decelerate_after == 0 /*&& this->accelerate_until == 0*/) { + // we start off decelerating + acceleration_change = -deceleration_per_tick; + + } else if(this->decelerate_after != this->total_move_ticks /*&& this->accelerate_until == 0*/) { + // If the next event is the start of decel ( don't set this if the next accel event is accel end ) + this->tick_info[m].next_accel_event = this->decelerate_after; + } + + // already converted to fixed point just needs scaling by ratio + //#define STEPTICKER_TOFP(x) ((int64_t)round((double)(x)*STEPTICKER_FPSCALE)) + this->tick_info[m].acceleration_change= (int64_t)round(acceleration_change * aratio); + this->tick_info[m].deceleration_change= -(int64_t)round(deceleration_per_tick * aratio); + this->tick_info[m].plateau_rate= (int64_t)round(((this->maximum_rate * aratio) / STEP_TICKER_FREQUENCY) * STEPTICKER_FPSCALE); + + #if 0 + THEKERNEL->streams->printf("spt: %08lX %08lX, ac: %08lX %08lX, dc: %08lX %08lX, pr: %08lX %08lX\n", + (uint32_t)(this->tick_info[m].steps_per_tick>>32), // 2.62 fixed point + (uint32_t)(this->tick_info[m].steps_per_tick&0xFFFFFFFF), // 2.62 fixed point + (uint32_t)(this->tick_info[m].acceleration_change>>32), // 2.62 fixed point signed + (uint32_t)(this->tick_info[m].acceleration_change&0xFFFFFFFF), // 2.62 fixed point signed + (uint32_t)(this->tick_info[m].deceleration_change>>32), // 2.62 fixed point + (uint32_t)(this->tick_info[m].deceleration_change&0xFFFFFFFF), // 2.62 fixed point + (uint32_t)(this->tick_info[m].plateau_rate>>32), // 2.62 fixed point + (uint32_t)(this->tick_info[m].plateau_rate&0xFFFFFFFF) // 2.62 fixed point + ); + #endif + } +} + +// returns current rate (steps/sec) for the given actuator +float Block::get_trapezoid_rate(int i) const +{ + // convert steps per tick from fixed point to float and convert to steps/sec + // FIXME steps_per_tick can change at any time, potential race condition if it changes while being read here + return STEPTICKER_FROMFP(tick_info[i].steps_per_tick) * STEP_TICKER_FREQUENCY; +}