return this->serial->getc();
}
+// Does the queue have a given char ?
bool SerialConsole::has_char(char letter){
int index = this->buffer.head;
while( index != this->buffer.tail ){
#include <stdlib.h>
-
+// This is a gcode object. It reprensents a GCode string/command, an caches some important values about that command for the sake of performance.
+// It gets passed around in events, and attached to the queue ( that'll change )
Gcode::Gcode(const string& command, StreamOutput* stream) : command(command), m(0), g(0), add_nl(false), stream(stream) {
prepare_cached_values();
this->millimeters_of_travel = 0L;
return count;
}
+// Cache some of this command's properties, so we don't have to parse the string every time we want to look at them
void Gcode::prepare_cached_values(){
if( this->has_letter('G') ){
this->has_g = true;
#include <vector>
#include "../communication/utils/Gcode.h"
+// 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.
+// Most of the accel math is also done in this class
+// And GCode objects for use in on_gcode_execute are also help in here
+
Block::Block(){
clear_vector(this->steps);
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.
// | + <- nominal_rate*exit_factor
// +-------------+
// time -->
-//*/
+*/
void Block::calculate_trapezoid( double entryfactor, double exitfactor ){
- //this->conveyor->kernel->streams->printf("%p calculating trapezoid\r\n", this);
-
+ // The planner passes us factors, we need to transform them in rates
this->initial_rate = ceil(this->nominal_rate * entryfactor); // (step/min)
this->final_rate = ceil(this->nominal_rate * exitfactor); // (step/min)
- //this->conveyor->kernel->streams->printf("initrate:%f finalrate:%f\r\n", this->initial_rate, this->final_rate);
-
+ // How many steps to accelerate and decelerate
double acceleration_per_minute = this->rate_delta * this->planner->kernel->stepper->acceleration_ticks_per_second * 60.0; // ( step/min^2)
int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_minute ) );
int decelerate_steps = floor( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate, -acceleration_per_minute ) );
-
- // Calculate the size of Plateau of Nominal Rate.
+ // Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise )
int plateau_steps = this->steps_event_count-accelerate_steps-decelerate_steps;
- //this->conveyor->kernel->streams->printf("accelperminute:%f accelerate_steps:%d decelerate_steps:%d plateau:%d \r\n", acceleration_per_minute, accelerate_steps, decelerate_steps, plateau_steps );
-
// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
// have to use intersection_distance() to calculate when to abort acceleration and start braking
// in order to reach the final_rate exactly at the end of this block.
accelerate_steps = min( accelerate_steps, int(this->steps_event_count) );
plateau_steps = 0;
}
-
this->accelerate_until = accelerate_steps;
this->decelerate_after = accelerate_steps+plateau_steps;
- //this->debug(this->conveyor->kernel);
-
- /*
- // 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
- if( this->final_rate < 0.01 ){
- this->decelerate_after += floor( this->nominal_rate / 60 / this->planner->kernel->stepper->acceleration_ticks_per_second ) * 3;
- }
- */
}
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
void Block::pop_and_execute_gcode(Kernel* &kernel){
Block* block = const_cast<Block*>(this);
for(unsigned short index=0; index<block->gcodes.size(); index++){
- //printf("GCODE Z: %s \r\n", block->gcodes[index].command.c_str() );
kernel->call_event(ON_GCODE_EXECUTE, &(block->gcodes[index]));
}
}
// Mark the block as taken by one more module
void Block::take(){
this->times_taken++;
- //printf("taking %p times now:%d\r\n", this, int(this->times_taken) );
}
// Mark the block as no longer taken by one module, go to next block if this free's it
+// This is one of the craziest bits in smoothie
void Block::release(){
- //printf("release %p \r\n", this );
+
+ // A block can be taken by several modules, we want to actually release it only when all modules have release()d it
this->times_taken--;
- //printf("releasing %p times now:%d\r\n", this, int(this->times_taken) );
if( this->times_taken < 1 ){
+
+ // All modules are done with this block
+ // Call the on_block_end event so all modules can act accordingly
this->conveyor->kernel->call_event(ON_BLOCK_END, this);
+
+ // Gcodes corresponding to the *following* blocks are stored in this block.
+ // We execute them all in order when this block is finished executing
this->pop_and_execute_gcode(this->conveyor->kernel);
+
+ // We would normally delete this block directly here, but we can't, because this is interrupt context, no crazy memory stuff here
+ // So instead we increment a counter, and it will be deleted in main loop context
Conveyor* conveyor = this->conveyor;
-
if( conveyor->queue.size() > conveyor->flush_blocks ){
conveyor->flush_blocks++;
}
+ // We don't look for the next block to execute if the conveyor is already doing that itself
if( conveyor->looking_for_new_block == false ){
+
+ // If there are still blocks to execute
if( conveyor->queue.size() > conveyor->flush_blocks ){
Block* candidate = conveyor->queue.get_ref(conveyor->flush_blocks);
+
+ // We only execute blocks that are ready ( their math is done )
if( candidate->is_ready ){
+
+ // Execute this candidate
conveyor->current_block = candidate;
conveyor->kernel->call_event(ON_BLOCK_BEGIN, conveyor->current_block);
+
+ // If no module took this block, release it ourselves, as nothing else will do it otherwise
if( conveyor->current_block->times_taken < 1 ){
conveyor->current_block->times_taken = 1;
conveyor->current_block->release();
}
}else{
-
conveyor->current_block = NULL;
-
}
}else{
conveyor->current_block = NULL;
#include "Conveyor.h"
#include "Planner.h"
+// The conveyor holds the queue of blocks, takes care of creating them, and starting the executing chain of blocks
+
Conveyor::Conveyor(){
this->current_block = NULL;
this->looking_for_new_block = false;
register_for_event(ON_IDLE);
}
+// Delete blocks here, because they can't be deleted in interrupt context ( see Block.cpp:release )
void Conveyor::on_idle(void* argument){
if (flush_blocks){
-
+ // Cleanly delete block
Block* block = queue.get_tail_ref();
block->gcodes.clear();
queue.delete_first();
-
__disable_irq();
flush_blocks--;
__enable_irq();
// Append a block to the list
Block* Conveyor::new_block(){
- // Clean up the vector of commands in the block we are about to replace
- // It is quite strange to do this here, we really should do it inside Block->pop_and_execute_gcode
- // but that function is called inside an interrupt and thus can break everything if the interrupt was trigerred during a memory access
-
// Take the next untaken block on the queue ( the one after the last one )
Block* block = this->queue.get_tail_ref();
// Then clean it up
}
-void Conveyor::wait_for_queue(int free_blocks)
-{
+// Wait for the queue to have a given number of free blocks
+void Conveyor::wait_for_queue(int free_blocks){
while( this->queue.size() >= this->queue.capacity()-free_blocks ){
this->kernel->call_event(ON_IDLE);
}
}
+// Wait for the queue to be empty
void Conveyor::wait_for_empty_queue(){
while( this->queue.size() > 0){
this->kernel->call_event(ON_IDLE);
#include "Planner.h"
#include "Conveyor.h"
+// The Planner does the acceleration math for the queue of Blocks ( movements ).
+// It makes sure the speed stays within the configured constraints ( acceleration, junction_deviation, etc )
+// It goes over the list in both direction, every time a block is added, re-doing the math to make sure everything is optimal
Planner::Planner(){
clear_vector(this->position);
this->on_config_reload(this);
}
+// Configure acceleration
void Planner::on_config_reload(void* argument){
this->acceleration = this->kernel->config->value(acceleration_checksum )->by_default(100 )->as_number() * 60 * 60; // Acceleration is in mm/minute^2, see https://github.com/grbl/grbl/commit/9141ad282540eaa50a41283685f901f29c24ddbd#planner.c
this->junction_deviation = this->kernel->config->value(junction_deviation_checksum )->by_default(0.05)->as_number();
// Append a block to the queue, compute it's speed factors
void Planner::append_block( int target[], double feed_rate, double distance, double deltas[] ){
- //printf("new block\r\n");
-
// Stall here if the queue is ful
this->kernel->conveyor->wait_for_queue(2);
+ // Create ( recycle ) a new block
Block* block = this->kernel->conveyor->new_block();
block->planner = this;
- //Block* test = new Block();
- //this->kernel->streams->printf("%p queue:%u\r\n", test, this->kernel->player->queue.size());
- //delete test;
- //
// Direction bits
block->direction_bits = 0;
for( int stepper=ALPHA_STEPPER; stepper<=GAMMA_STEPPER; stepper++){
// Max number of steps, for all axes
block->steps_event_count = max( block->steps[ALPHA_STEPPER], max( block->steps[BETA_STEPPER], block->steps[GAMMA_STEPPER] ) );
- //if( block->steps_event_count == 0 ){ this->computing = false; return; }
block->millimeters = distance;
double inverse_millimeters = 0;
block->nominal_rate = 0;
}
- //this->kernel->streams->printf("nom_speed: %f nom_rate: %u step_event_count: %u block->steps_z: %u \r\n", block->nominal_speed, block->nominal_rate, block->steps_event_count, block->steps[2] );
-
// Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
// average travel per step event changes. For a line along one axis the travel per step event
// is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
// Convert universal acceleration for direction-dependent stepper rate change parameter
block->rate_delta = (float)( ( block->steps_event_count*inverse_millimeters * this->acceleration ) / ( this->kernel->stepper->acceleration_ticks_per_second * 60 ) ); // (step/min/acceleration_tick)
-
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = deltas[X_AXIS]*inverse_millimeters;
// 3. Recalculate trapezoids for all blocks.
//
void Planner::recalculate() {
- //this->kernel->streams->printf("recalculate last: %p, queue size: %d \r\n", this->kernel->conveyor->queue.get_ref( this->kernel->conveyor->queue.size()-1 ), this->kernel->conveyor->queue.size() );
this->reverse_pass();
this->forward_pass();
this->recalculate_trapezoids();
while(block_index != this->kernel->conveyor->queue.tail){
current = next;
next = &this->kernel->conveyor->queue.buffer[block_index];
- //this->kernel->streams->printf("index:%d current:%p next:%p \r\n", block_index, current, next );
if( current ){
// Recalculate if current block entry or exit junction speed has changed.
if( current->recalculate_flag || next->recalculate_flag ){
// acceleration within the allotted distance.
double Planner::max_allowable_speed(double acceleration, double target_velocity, double distance) {
return(
- //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
sqrt(target_velocity*target_velocity-2L*acceleration*distance) //Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes
);
}
#include "arm_solutions/RostockSolution.h"
#include "arm_solutions/HBotSolution.h"
+// The Robot converts GCodes into actual movements, and then adds them to the Planner, which passes them to the Conveyor so they can be added to the queue
+// It takes care of cutting arcs into segments, same thing for line that are too long
+
Robot::Robot(){
this->inch_mode = false;
this->absolute_mode = true;
}
void Robot::on_config_reload(void* argument){
+
+ // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
+ // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
+ // To make adding those solution easier, they have their own, separate object.
+ // Here we read the config to find out which arm solution to use
if (this->arm_solution) delete this->arm_solution;
int solution_checksum = get_checksum(this->kernel->config->value(arm_solution_checksum)->by_default("cartesian")->as_string());
-
// Note checksums are not const expressions when in debug mode, so don't use switch
if(solution_checksum == hbot_checksum) {
this->arm_solution = new HBotSolution(this->kernel->config);
}
-//#pragma GCC push_options
-//#pragma GCC optimize ("O0")
-
-
//A GCode has been received
+//See if the current Gcode line has some orders for us
void Robot::on_gcode_received(void * argument){
Gcode* gcode = static_cast<Gcode*>(argument);
- this->process_gcode(gcode);
-}
-
-// We called process_gcode with a new gcode, and one of the functions
-// determined the distance for that given gcode. So now we can attach this gcode to the right block
-// and continue
-void Robot::distance_in_gcode_is_known(Gcode* gcode){
-
- //If the queue is empty, execute immediatly, otherwise attach to the last added block
- if( this->kernel->conveyor->queue.size() == 0 ){
- this->kernel->call_event(ON_GCODE_EXECUTE, gcode );
- }else{
- Block* block = this->kernel->conveyor->queue.get_ref( this->kernel->conveyor->queue.size() - 1 );
- block->append_gcode(gcode);
- }
-
-}
-
-//#pragma GCC pop_options
-
-void Robot::reset_axis_position(double position, int axis) {
- this->last_milestone[axis] = this->current_position[axis] = position;
- this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position);
-}
-
-
-//See if the current Gcode line has some orders for us
-void Robot::process_gcode(Gcode* gcode){
//Temp variables, constant properties are stored in the object
uint8_t next_action = NEXT_ACTION_DEFAULT;
// in any intermediate location.
memcpy(this->current_position, target, sizeof(double)*3); // this->position[] = target[];
+
+
+
+}
+
+// We received a new gcode, and one of the functions
+// determined the distance for that given gcode. So now we can attach this gcode to the right block
+// and continue
+void Robot::distance_in_gcode_is_known(Gcode* gcode){
+
+ //If the queue is empty, execute immediatly, otherwise attach to the last added block
+ if( this->kernel->conveyor->queue.size() == 0 ){
+ this->kernel->call_event(ON_GCODE_EXECUTE, gcode );
+ }else{
+ Block* block = this->kernel->conveyor->queue.get_ref( this->kernel->conveyor->queue.size() - 1 );
+ block->append_gcode(gcode);
+ }
+
+}
+
+// Reset the position for all axes ( used in homing and G92 stuff )
+void Robot::reset_axis_position(double position, int axis) {
+ this->last_milestone[axis] = this->current_position[axis] = position;
+ this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position);
}
+
// Convert target from millimeters to steps, and append this to the planner
void Robot::append_milestone( double target[], double rate ){
int steps[3]; //Holds the result of the conversion
+ // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
this->arm_solution->millimeters_to_steps( target, steps );
double deltas[3];
for(int axis=X_AXIS;axis<=Z_AXIS;axis++){deltas[axis]=target[axis]-this->last_milestone[axis];}
-
+ // Compute how long this move moves, so we can attach it to the block for later use
double millimeters_of_travel = sqrt( pow( deltas[X_AXIS], 2 ) + pow( deltas[Y_AXIS], 2 ) + pow( deltas[Z_AXIS], 2 ) );
+ // Do not move faster than the configured limits
for(int axis=X_AXIS;axis<=Z_AXIS;axis++){
if( this->max_speeds[axis] > 0 ){
double axis_speed = ( fabs(deltas[axis]) / ( millimeters_of_travel / rate )) * seconds_per_minute;
}
}
+ // Append the block to the planner
this->kernel->planner->append_block( steps, rate * seconds_per_minute, millimeters_of_travel, deltas );
+ // Update the last_milestone to the current target for the next time we use last_milestone
memcpy(this->last_milestone, target, sizeof(double)*3); // this->last_milestone[] = target[];
}
+// Append a move to the queue ( cutting it into segments if needed )
void Robot::append_line(Gcode* gcode, double target[], double rate ){
+ // Find out the distance for this gcode
gcode->millimeters_of_travel = sqrt( pow( target[X_AXIS]-this->current_position[X_AXIS], 2 ) + pow( target[Y_AXIS]-this->current_position[Y_AXIS], 2 ) + pow( target[Z_AXIS]-this->current_position[Z_AXIS], 2 ) );
+ // We ignore non-moves ( for example, extruder moves are not XYZ moves )
if( gcode->millimeters_of_travel < 0.0001 ){ return; }
+ // Mark the gcode as having a known distance
this->distance_in_gcode_is_known( gcode );
// We cut the line into smaller segments. This is not usefull in a cartesian robot, but necessary for robots with rotational axes.
// In cartesian robot, a high "mm_per_line_segment" setting will prevent waste.
// In delta robots either mm_per_line_segment can be used OR delta_segments_per_second The latter is more efficient and avoids splitting fast long lines into very small segments, like initial z move to 0, it is what Johanns Marlin delta port does
-
uint16_t segments;
if(this->delta_segments_per_second > 1.0) {
//For each segment
for( int i=0; i<segments-1; i++ ){
for(int axis=X_AXIS; axis <= Z_AXIS; axis++ ){ temp_target[axis] += ( target[axis]-this->current_position[axis] )/segments; }
+ // Append the end of this segment to the queue
this->append_milestone(temp_target, rate);
}
+
+ // Append the end of this full move to the queue
this->append_milestone(target, rate);
}
+// Append an arc to the queue ( cutting it into segments as needed )
void Robot::append_arc(Gcode* gcode, double target[], double offset[], double radius, bool is_clockwise ){
+ // Scary math
double center_axis0 = this->current_position[this->plane_axis_0] + offset[this->plane_axis_0];
double center_axis1 = this->current_position[this->plane_axis_1] + offset[this->plane_axis_1];
double linear_travel = target[this->plane_axis_2] - this->current_position[this->plane_axis_2];
if (angular_travel < 0) { angular_travel += 2*M_PI; }
if (is_clockwise) { angular_travel -= 2*M_PI; }
+ // Find the distance for this gcode
gcode->millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
+ // We don't care about non-XYZ moves ( for example the extruder produces some of those )
if( gcode->millimeters_of_travel < 0.0001 ){ return; }
+ // Mark the gcode as having a known distance
this->distance_in_gcode_is_known( gcode );
-
+
+ // Figure out how many segments for this gcode
uint16_t segments = floor(gcode->millimeters_of_travel/this->mm_per_arc_segment);
double theta_per_segment = angular_travel/segments;
arc_target[this->plane_axis_0] = center_axis0 + r_axis0;
arc_target[this->plane_axis_1] = center_axis1 + r_axis1;
arc_target[this->plane_axis_2] += linear_per_segment;
+
+ // Append this segment to the queue
this->append_milestone(arc_target, this->feed_rate);
}
+
// Ensure last segment arrives at target location.
this->append_milestone(target, this->feed_rate);
}
-
+// Do the math for an arc and add it to the queue
void Robot::compute_arc(Gcode* gcode, double offset[], double target[]){
// Find the radius
private:
void distance_in_gcode_is_known(Gcode* gcode);
- void process_gcode(Gcode* gcode);
void append_milestone( double target[], double feed_rate);
void append_line( Gcode* gcode, double target[], double feed_rate);
//void append_arc(double theta_start, double angular_travel, double radius, double depth, double rate);
#include <mri.h>
+// The stepper reacts to blocks that have XYZ movement to transform them into actual stepper motor moves
+// TODO: This does accel, accel should be in StepperMotor
+
Stepper* stepper;
uint32_t previous_step_count;
uint32_t skipped_speed_updates;
uint32_t speed_ticks_counter;
-
Stepper::Stepper(){
this->current_block = NULL;
this->paused = false;
this->kernel->robot->gamma_stepper_motor->unpause();
}
-
+// React to enable/disable gcodes
void Stepper::on_gcode_execute(void* argument){
Gcode* gcode = static_cast<Gcode*>(argument);
}
}
+// Enable steppers
void Stepper::turn_enable_pins_on(){
this->kernel->robot->alpha_en_pin.set(0);
this->kernel->robot->beta_en_pin.set(0);
this->enable_pins_status = true;
}
+// Disable steppers
void Stepper::turn_enable_pins_off(){
this->kernel->robot->alpha_en_pin.set(1);
this->kernel->robot->beta_en_pin.set(1);
if( this->kernel->robot->beta_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ){ this->main_stepper = this->kernel->robot->beta_stepper_motor; }
if( this->kernel->robot->gamma_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ){ this->main_stepper = this->kernel->robot->gamma_stepper_motor; }
-
+ // Set the initial speed for this move
this->trapezoid_generator_tick(0);
// Synchronise the acceleration curve with the stepping
this->current_block = NULL; //stfu !
}
-//#pragma GCC push_options
-//#pragma GCC optimize ("O0")
-
// When a stepper motor has finished it's assigned movement
uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy){
// current_block stays untouched by outside handlers for the duration of this function call.
uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy ) {
+ // Do not do the accel math for nothing
if(this->current_block && !this->paused && this->main_stepper->moving ) {
+
+ // Store this here because we use it a lot down there
uint32_t current_steps_completed = this->main_stepper->stepped;
+ // Do not accel, just set the value
if( this->force_speed_update ){
this->force_speed_update = false;
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
return 0;
}
+ // If we are accelerating
if(current_steps_completed <= this->current_block->accelerate_until + 1) {
- this->trapezoid_adjusted_rate += this->current_block->rate_delta;
-
+ // Increase speed
+ this->trapezoid_adjusted_rate += this->current_block->rate_delta;
if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
}
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
+
+ // If we are decelerating
}else if (current_steps_completed > this->current_block->decelerate_after) {
- // NOTE: We will only reduce speed if the result will be > 0. This catches small
+ // Reduce speed
+ // NOTE: We will only reduce speed if the result will be > 0. This catches small
// rounding errors that might leave steps hanging after the last trapezoid tick.
if(this->trapezoid_adjusted_rate > this->current_block->rate_delta * 1.5) {
this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
this->trapezoid_adjusted_rate = this->current_block->final_rate;
}
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
+
+ // If we are cruising
}else {
// Make sure we cruise at exactly nominal rate
if (this->trapezoid_adjusted_rate != this->current_block->nominal_rate) {
steps_per_minute = this->minimum_steps_per_minute;
}
-
// Instruct the stepper motors
if( this->kernel->robot->alpha_stepper_motor->moving ){ this->kernel->robot->alpha_stepper_motor->set_speed( (steps_per_minute/60L) * ( (double)this->current_block->steps[ALPHA_STEPPER] / (double)this->current_block->steps_event_count ) ); }
if( this->kernel->robot->beta_stepper_motor->moving ){ this->kernel->robot->beta_stepper_motor->set_speed( (steps_per_minute/60L) * ( (double)this->current_block->steps[BETA_STEPPER ] / (double)this->current_block->steps_event_count ) ); }
if( this->kernel->robot->gamma_stepper_motor->moving ){ this->kernel->robot->gamma_stepper_motor->set_speed( (steps_per_minute/60L) * ( (double)this->current_block->steps[GAMMA_STEPPER] / (double)this->current_block->steps_event_count ) ); }
+ // Other modules might want to know the speed changed
this->kernel->call_event(ON_SPEED_CHANGE, this);
}
return 0;
}
-
-//#pragma GCC pop_options
HCoop / clinton / Smoothieware.git / commitdiff