#include "libs/Module.h"
#include "libs/Kernel.h"
#include "libs/nuts_bolts.h"
-#include <math.h>
+#include <cmath>
#include <string>
#include "Block.h"
#include "Planner.h"
#include "Conveyor.h"
#include "Gcode.h"
#include "libs/StreamOutputPool.h"
-#include "Stepper.h"
#include "StepTicker.h"
+#include "platform_memory.h"
#include "mri.h"
+#include <inttypes.h>
using std::string;
-#include <vector>
#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.
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<Gcode>().swap(gcodes); // this resizes the vector releasing its memory
+ is_ready = false;
this->steps.fill(0);
steps_event_count = 0;
- nominal_rate = 0;
+ nominal_rate = 0.0F;
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
- initial_rate = -1;
+ 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;
direction_bits = 0;
recalculate_flag = false;
nominal_length_flag = false;
max_entry_speed = 0.0F;
- is_ready = false;
- times_taken = 0;
- acceleration_per_tick= 0;
- deceleration_per_tick= 0;
+ is_ticking = false;
+ is_g123 = false;
+ locked = false;
+ s_value = 0.0F;
+
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()
+void Block::debug() const
{
- THEKERNEL->streams->printf("%p: steps:X%04lu Y%04lu Z%04lu(max:%4lu) nominal:r%10lu/s%6.1f mm:%9.6f acc:%5lu dec:%5lu rates:%10lu entry/max: %10.4f/%10.4f taken:%d ready:%d recalc:%d nomlen:%d\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->times_taken,
+ this->exit_speed,
+ this->primary_axis,
this->is_ready,
+ this->locked,
+ this->is_ticking,
recalculate_flag ? 1 : 0,
- nominal_length_flag ? 1 : 0
+ nominal_length_flag ? 1 : 0,
+ total_move_ticks/STEP_TICKER_FREQUENCY
);
}
void Block::calculate_trapezoid( float entryspeed, float exitspeed )
{
// if block is currently executing, don't touch anything!
- if (times_taken)
- return;
+ 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);
// Now this is the maximum rate we'll achieve this move, either because
// it's the higher we can achieve, or because it's the higher we are
// allowed to achieve
- this->maximum_rate = std::min(maximum_possible_rate, (float)this->nominal_rate);
+ this->maximum_rate = std::min(maximum_possible_rate, this->nominal_rate);
// Now figure out how long it takes to accelerate in seconds
float time_to_accelerate = ( this->maximum_rate - initial_rate ) / acceleration_per_second;
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;
- // Note we use this value for acceleration as well as for deceleration, if that doesn't work, we can also as well compute the deceleration value this way :
- // float deceleration(steps/s²) = ( final_rate(steps/s) - maximum_rate(steps/s) ) / acceleration_time(s);
- // and store that in the block and use it for deceleration, which -will- yield better results, but may not be useful. If the moves do not end correctly, try computing this value, adding it to the block, and then using it for deceleration in the step generator
-
+ // 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;
-}
-// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
-// given acceleration:
-float Block::estimate_acceleration_distance(float initialrate, float targetrate, float acceleration)
-{
- return( ((targetrate * targetrate) - (initialrate * initialrate)) / (2.0F * acceleration));
-}
+ // prepare the block for stepticker
+ this->prepare(acceleration_in_steps, deceleration_in_steps);
-// This function gives you the point at which you must start braking (at the rate of -acceleration) if
-// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
-// a total travel of distance. This can be used to compute the intersection point between acceleration and
-// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
-//
-/* + <- some maximum rate we don't care about
- /|\
- / | \
- / | + <- final_rate
- / | |
- initial_rate -> +----+--+
- ^ ^
- | |
- intersection_distance distance */
-float Block::intersection_distance(float initialrate, float finalrate, float acceleration, float distance)
-{
- return((2 * acceleration * distance - initialrate * initialrate + finalrate * finalrate) / (4 * acceleration));
+ this->locked= false;
}
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
return sqrtf(target_velocity * target_velocity - 2.0F * acceleration * distance);
}
-
// Called by Planner::recalculate() when scanning the plan from last to first entry.
float Block::reverse_pass(float 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
- 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
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()
+// 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)
{
- recalculate_flag = false;
-
- if (!is_ready)
- __debugbreak();
-
- times_taken = -1;
-
- // execute all the gcodes related to this block
- for(unsigned int index = 0; index < gcodes.size(); index++)
- THEKERNEL->call_event(ON_GCODE_EXECUTE, &(gcodes[index]));
-
- THEKERNEL->call_event(ON_BLOCK_BEGIN, this);
+ float inv = 1.0F / this->steps_event_count;
- if (times_taken < 0)
- release();
-}
-
-// Signal the conveyor that this block is ready to be injected into the system
-void Block::ready()
-{
- this->is_ready = true;
-}
+ // 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;
+ }
-// Mark the block as taken by one more module
-void Block::take()
-{
- if (times_taken < 0)
- times_taken = 0;
- times_taken++;
+ // 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
+ }
}
-// Mark the block as no longer taken by one module, go to next block if this frees it
-void Block::release()
+// returns current rate (steps/sec) for the given actuator
+float Block::get_trapezoid_rate(int i) const
{
- if (--this->times_taken <= 0) {
- times_taken = 0;
- if (is_ready) {
- is_ready = false;
- THEKERNEL->call_event(ON_BLOCK_END, this);
-
- // ensure conveyor gets called last
- THEKERNEL->conveyor->on_block_end(this);
- }
- }
+ // 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;
}
HCoop / clinton / Smoothieware.git / blobdiff