From edac9072272101276960879e5a5cfe2184342940 Mon Sep 17 00:00:00 2001 From: Arthur Wolf Date: Tue, 16 Apr 2013 17:46:34 +0200 Subject: [PATCH] adding comments to modules/ --- src/modules/communication/SerialConsole.cpp | 1 + src/modules/communication/utils/Gcode.cpp | 4 +- src/modules/robot/Block.cpp | 55 ++++++------ src/modules/robot/Conveyor.cpp | 15 ++-- src/modules/robot/Planner.cpp | 18 ++-- src/modules/robot/Robot.cpp | 94 +++++++++++++-------- src/modules/robot/Robot.h | 1 - src/modules/robot/Stepper.cpp | 33 +++++--- 8 files changed, 124 insertions(+), 97 deletions(-) diff --git a/src/modules/communication/SerialConsole.cpp b/src/modules/communication/SerialConsole.cpp index acdf959b..f0c62c5e 100644 --- a/src/modules/communication/SerialConsole.cpp +++ b/src/modules/communication/SerialConsole.cpp @@ -83,6 +83,7 @@ int SerialConsole::_getc() 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 ){ diff --git a/src/modules/communication/utils/Gcode.cpp b/src/modules/communication/utils/Gcode.cpp index 679c6936..9481bd3a 100644 --- a/src/modules/communication/utils/Gcode.cpp +++ b/src/modules/communication/utils/Gcode.cpp @@ -14,7 +14,8 @@ using std::string; #include - +// 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; @@ -100,6 +101,7 @@ int Gcode::get_num_args(){ 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; diff --git a/src/modules/robot/Block.cpp b/src/modules/robot/Block.cpp index 355d9f0b..d4ca07d9 100644 --- a/src/modules/robot/Block.cpp +++ b/src/modules/robot/Block.cpp @@ -17,6 +17,11 @@ using std::string; #include #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. @@ -38,26 +43,21 @@ void Block::debug(Kernel* kernel){ // | + <- 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. @@ -67,18 +67,9 @@ void Block::calculate_trapezoid( double entryfactor, double exitfactor ){ 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 @@ -175,7 +166,6 @@ void Block::append_gcode(Gcode* gcode){ void Block::pop_and_execute_gcode(Kernel* &kernel){ Block* block = const_cast(this); for(unsigned short index=0; indexgcodes.size(); index++){ - //printf("GCODE Z: %s \r\n", block->gcodes[index].command.c_str() ); kernel->call_event(ON_GCODE_EXECUTE, &(block->gcodes[index])); } } @@ -189,37 +179,52 @@ void Block::ready(){ // 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; diff --git a/src/modules/robot/Conveyor.cpp b/src/modules/robot/Conveyor.cpp index f3932af0..afc49ffc 100644 --- a/src/modules/robot/Conveyor.cpp +++ b/src/modules/robot/Conveyor.cpp @@ -18,6 +18,8 @@ using namespace std; #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; @@ -28,13 +30,13 @@ void Conveyor::on_module_loaded(){ 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(); @@ -44,10 +46,6 @@ void Conveyor::on_idle(void* argument){ // 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 @@ -109,13 +107,14 @@ void Conveyor::pop_and_process_new_block(int debug){ } -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); diff --git a/src/modules/robot/Planner.cpp b/src/modules/robot/Planner.cpp index 24bef125..6ccf322a 100644 --- a/src/modules/robot/Planner.cpp +++ b/src/modules/robot/Planner.cpp @@ -16,6 +16,9 @@ using namespace std; #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); @@ -29,6 +32,7 @@ void Planner::on_module_loaded(){ 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(); @@ -38,18 +42,13 @@ void Planner::on_config_reload(void* argument){ // 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++){ @@ -61,7 +60,6 @@ void Planner::append_block( int target[], double feed_rate, double distance, dou // 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; @@ -78,8 +76,6 @@ void Planner::append_block( int target[], double feed_rate, double distance, dou 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 @@ -89,7 +85,6 @@ void Planner::append_block( int target[], double feed_rate, double distance, dou // 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; @@ -178,7 +173,6 @@ void Planner::append_block( int target[], double feed_rate, double distance, dou // 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(); @@ -234,7 +228,6 @@ void Planner::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 ){ @@ -264,7 +257,6 @@ void Planner::dump_queue(){ // 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 ); } diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp index 81126c51..fb85212f 100644 --- a/src/modules/robot/Robot.cpp +++ b/src/modules/robot/Robot.cpp @@ -23,6 +23,9 @@ using std::string; #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; @@ -50,9 +53,13 @@ void Robot::on_module_loaded() { } 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); @@ -96,41 +103,10 @@ void Robot::on_config_reload(void* argument){ } -//#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(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; @@ -237,20 +213,47 @@ void Robot::process_gcode(Gcode* gcode){ // 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; @@ -260,24 +263,29 @@ void Robot::append_milestone( double target[], double rate ){ } } + // 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) { @@ -305,14 +313,19 @@ void Robot::append_line(Gcode* gcode, double target[], double rate ){ //For each segment for( int i=0; icurrent_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]; @@ -326,12 +339,16 @@ void Robot::append_arc(Gcode* gcode, double target[], double offset[], double ra 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; @@ -396,14 +413,17 @@ void Robot::append_arc(Gcode* gcode, double target[], double offset[], double ra 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 diff --git a/src/modules/robot/Robot.h b/src/modules/robot/Robot.h index a7da4585..15547bbf 100644 --- a/src/modules/robot/Robot.h +++ b/src/modules/robot/Robot.h @@ -68,7 +68,6 @@ class Robot : public Module { 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); diff --git a/src/modules/robot/Stepper.cpp b/src/modules/robot/Stepper.cpp index 246f77a2..fe346ce1 100644 --- a/src/modules/robot/Stepper.cpp +++ b/src/modules/robot/Stepper.cpp @@ -17,12 +17,14 @@ using namespace std; #include +// 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; @@ -80,7 +82,7 @@ void Stepper::on_play(void* argument){ this->kernel->robot->gamma_stepper_motor->unpause(); } - +// React to enable/disable gcodes void Stepper::on_gcode_execute(void* argument){ Gcode* gcode = static_cast(argument); @@ -94,6 +96,7 @@ void Stepper::on_gcode_execute(void* 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); @@ -101,6 +104,7 @@ void Stepper::turn_enable_pins_on(){ 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); @@ -142,7 +146,7 @@ void Stepper::on_block_begin(void* argument){ 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 @@ -155,9 +159,6 @@ void Stepper::on_block_end(void* argument){ 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){ @@ -178,24 +179,32 @@ 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; @@ -206,6 +215,8 @@ uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy ) { 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) { @@ -240,12 +251,12 @@ void Stepper::set_step_events_per_minute( double steps_per_minute ){ 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); } @@ -284,5 +295,3 @@ uint32_t Stepper::synchronize_acceleration(uint32_t dummy){ return 0; } - -//#pragma GCC pop_options -- 2.20.1