Michael Spencer / Smoothie

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Smoothie by Stéphane Cachat

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

Committer:
scachat
Date:
Tue Jul 31 21:11:18 2012 +0000
Revision:
0:31e91bb0ef3c
Child:
2:1df0b61d3b5a
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) with additions from Sungeun K. Jeon (https://github.com/chamnit/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 "Stepper.h"
scachat 0:31e91bb0ef3c 11 #include "Planner.h"
scachat 0:31e91bb0ef3c 12 #include "Player.h"
scachat 0:31e91bb0ef3c 13 #include <vector>
scachat 0:31e91bb0ef3c 14 using namespace std;
scachat 0:31e91bb0ef3c 15 #include "libs/nuts_bolts.h"
scachat 0:31e91bb0ef3c 16
scachat 0:31e91bb0ef3c 17 Stepper* stepper;
scachat 0:31e91bb0ef3c 18
scachat 0:31e91bb0ef3c 19 Stepper::Stepper(){
scachat 0:31e91bb0ef3c 20 this->current_block = NULL;
scachat 0:31e91bb0ef3c 21 this->step_events_completed = 0;
scachat 0:31e91bb0ef3c 22 this->divider = 0;
scachat 0:31e91bb0ef3c 23 this->paused = false;
scachat 0:31e91bb0ef3c 24 }
scachat 0:31e91bb0ef3c 25
scachat 0:31e91bb0ef3c 26 //Called when the module has just been loaded
scachat 0:31e91bb0ef3c 27 void Stepper::on_module_loaded(){
scachat 0:31e91bb0ef3c 28 stepper = this;
scachat 0:31e91bb0ef3c 29 this->register_for_event(ON_BLOCK_BEGIN);
scachat 0:31e91bb0ef3c 30 this->register_for_event(ON_BLOCK_END);
scachat 0:31e91bb0ef3c 31 this->register_for_event(ON_GCODE_EXECUTE);
scachat 0:31e91bb0ef3c 32 this->register_for_event(ON_PLAY);
scachat 0:31e91bb0ef3c 33 this->register_for_event(ON_PAUSE);
scachat 0:31e91bb0ef3c 34
scachat 0:31e91bb0ef3c 35 // Get onfiguration
scachat 0:31e91bb0ef3c 36 this->on_config_reload(this);
scachat 0:31e91bb0ef3c 37
scachat 0:31e91bb0ef3c 38 // Acceleration ticker
scachat 0:31e91bb0ef3c 39 //this->kernel->slow_ticker->set_frequency(this->acceleration_ticks_per_second/10);
scachat 0:31e91bb0ef3c 40 this->kernel->slow_ticker->attach( this->acceleration_ticks_per_second, this, &Stepper::trapezoid_generator_tick );
scachat 0:31e91bb0ef3c 41
scachat 0:31e91bb0ef3c 42 // Initiate main_interrupt timer and step reset timer
scachat 0:31e91bb0ef3c 43 this->kernel->step_ticker->attach( this, &Stepper::main_interrupt );
scachat 0:31e91bb0ef3c 44 this->kernel->step_ticker->reset_attach( this, &Stepper::reset_step_pins );
scachat 0:31e91bb0ef3c 45
scachat 0:31e91bb0ef3c 46 }
scachat 0:31e91bb0ef3c 47
scachat 0:31e91bb0ef3c 48 // Get configuration from the config file
scachat 0:31e91bb0ef3c 49 void Stepper::on_config_reload(void* argument){
scachat 0:31e91bb0ef3c 50
scachat 0:31e91bb0ef3c 51 this->microseconds_per_step_pulse = this->kernel->config->value(microseconds_per_step_pulse_checksum )->by_default(5 )->as_number();
scachat 0:31e91bb0ef3c 52 this->acceleration_ticks_per_second = this->kernel->config->value(acceleration_ticks_per_second_checksum)->by_default(100 )->as_number();
scachat 0:31e91bb0ef3c 53 this->minimum_steps_per_minute = this->kernel->config->value(minimum_steps_per_minute_checksum )->by_default(1200 )->as_number();
scachat 0:31e91bb0ef3c 54 this->base_stepping_frequency = this->kernel->config->value(base_stepping_frequency_checksum )->by_default(100000)->as_number();
scachat 0:31e91bb0ef3c 55 this->alpha_step_pin = this->kernel->config->value(alpha_step_pin_checksum )->by_default("1.21" )->as_pin()->as_output();
scachat 0:31e91bb0ef3c 56 this->beta_step_pin = this->kernel->config->value(beta_step_pin_checksum )->by_default("1.23" )->as_pin()->as_output();
scachat 0:31e91bb0ef3c 57 this->gamma_step_pin = this->kernel->config->value(gamma_step_pin_checksum )->by_default("1.22!" )->as_pin()->as_output();
scachat 0:31e91bb0ef3c 58 this->alpha_dir_pin = this->kernel->config->value(alpha_dir_pin_checksum )->by_default("1.18" )->as_pin()->as_output();
scachat 0:31e91bb0ef3c 59 this->beta_dir_pin = this->kernel->config->value(beta_dir_pin_checksum )->by_default("1.20" )->as_pin()->as_output();
scachat 0:31e91bb0ef3c 60 this->gamma_dir_pin = this->kernel->config->value(gamma_dir_pin_checksum )->by_default("1.19" )->as_pin()->as_output();
scachat 0:31e91bb0ef3c 61 this->alpha_en_pin = this->kernel->config->value(alpha_en_pin_checksum )->by_default("0.4" )->as_pin()->as_output()->as_open_drain();
scachat 0:31e91bb0ef3c 62 this->beta_en_pin = this->kernel->config->value(beta_en_pin_checksum )->by_default("0.10" )->as_pin()->as_output()->as_open_drain();
scachat 0:31e91bb0ef3c 63 this->gamma_en_pin = this->kernel->config->value(gamma_en_pin_checksum )->by_default("0.19" )->as_pin()->as_output()->as_open_drain();
scachat 0:31e91bb0ef3c 64
scachat 0:31e91bb0ef3c 65
scachat 0:31e91bb0ef3c 66 // TODO : This is supposed to be done by gcodes
scachat 0:31e91bb0ef3c 67 this->alpha_en_pin->set(0);
scachat 0:31e91bb0ef3c 68 this->beta_en_pin->set(0);
scachat 0:31e91bb0ef3c 69 this->gamma_en_pin->set(0);
scachat 0:31e91bb0ef3c 70
scachat 0:31e91bb0ef3c 71
scachat 0:31e91bb0ef3c 72 // Set the Timer interval for Match Register 1,
scachat 0:31e91bb0ef3c 73 this->kernel->step_ticker->set_reset_delay( this->microseconds_per_step_pulse / 1000000 );
scachat 0:31e91bb0ef3c 74 this->kernel->step_ticker->set_frequency( this->base_stepping_frequency );
scachat 0:31e91bb0ef3c 75 }
scachat 0:31e91bb0ef3c 76
scachat 0:31e91bb0ef3c 77 // When the play/pause button is set to pause, or a module calls the ON_PAUSE event
scachat 0:31e91bb0ef3c 78 void Stepper::on_pause(void* argument){
scachat 0:31e91bb0ef3c 79 this->paused = true;
scachat 0:31e91bb0ef3c 80 }
scachat 0:31e91bb0ef3c 81
scachat 0:31e91bb0ef3c 82 // When the play/pause button is set to play, or a module calls the ON_PLAY event
scachat 0:31e91bb0ef3c 83 void Stepper::on_play(void* argument){
scachat 0:31e91bb0ef3c 84 // TODO: Re-compute the whole queue for a cold-start
scachat 0:31e91bb0ef3c 85 this->paused = false;
scachat 0:31e91bb0ef3c 86 }
scachat 0:31e91bb0ef3c 87
scachat 0:31e91bb0ef3c 88 void Stepper::on_gcode_execute(void* argument){
scachat 0:31e91bb0ef3c 89 Gcode* gcode = static_cast<Gcode*>(argument);
scachat 0:31e91bb0ef3c 90
scachat 0:31e91bb0ef3c 91 if( gcode->has_letter('M')){
scachat 0:31e91bb0ef3c 92 int code = (int) gcode->get_value('M');
scachat 0:31e91bb0ef3c 93 if( code == 84 ){
scachat 0:31e91bb0ef3c 94 this->alpha_en_pin->set(0);
scachat 0:31e91bb0ef3c 95 this->beta_en_pin->set(0);
scachat 0:31e91bb0ef3c 96 this->gamma_en_pin->set(0);
scachat 0:31e91bb0ef3c 97 }
scachat 0:31e91bb0ef3c 98 }
scachat 0:31e91bb0ef3c 99 }
scachat 0:31e91bb0ef3c 100
scachat 0:31e91bb0ef3c 101 // A new block is popped from the queue
scachat 0:31e91bb0ef3c 102 void Stepper::on_block_begin(void* argument){
scachat 0:31e91bb0ef3c 103 Block* block = static_cast<Block*>(argument);
scachat 0:31e91bb0ef3c 104
scachat 0:31e91bb0ef3c 105 // The stepper does not care about 0-blocks
scachat 0:31e91bb0ef3c 106 if( block->millimeters == 0.0 ){ return; }
scachat 0:31e91bb0ef3c 107
scachat 0:31e91bb0ef3c 108 // Mark the new block as of interrest to us
scachat 0:31e91bb0ef3c 109 block->take();
scachat 0:31e91bb0ef3c 110
scachat 0:31e91bb0ef3c 111 // Setup
scachat 0:31e91bb0ef3c 112 for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){ this->counters[stpr] = 0; this->stepped[stpr] = 0; }
scachat 0:31e91bb0ef3c 113 this->step_events_completed = 0;
scachat 0:31e91bb0ef3c 114
scachat 0:31e91bb0ef3c 115 this->current_block = block;
scachat 0:31e91bb0ef3c 116
scachat 0:31e91bb0ef3c 117 // This is just to save computing power and not do it every step
scachat 0:31e91bb0ef3c 118 this->update_offsets();
scachat 0:31e91bb0ef3c 119
scachat 0:31e91bb0ef3c 120 // Setup acceleration for this block
scachat 0:31e91bb0ef3c 121 this->trapezoid_generator_reset();
scachat 0:31e91bb0ef3c 122
scachat 0:31e91bb0ef3c 123 }
scachat 0:31e91bb0ef3c 124
scachat 0:31e91bb0ef3c 125 // Current block is discarded
scachat 0:31e91bb0ef3c 126 void Stepper::on_block_end(void* argument){
scachat 0:31e91bb0ef3c 127 Block* block = static_cast<Block*>(argument);
scachat 0:31e91bb0ef3c 128 this->current_block = NULL; //stfu !
scachat 0:31e91bb0ef3c 129 }
scachat 0:31e91bb0ef3c 130
scachat 0:31e91bb0ef3c 131 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Smoothie. It is executed at the rate set with
scachat 0:31e91bb0ef3c 132 // config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
scachat 0:31e91bb0ef3c 133 inline uint32_t Stepper::main_interrupt(uint32_t dummy){
scachat 0:31e91bb0ef3c 134 if( this->paused ){ return 0; }
scachat 0:31e91bb0ef3c 135
scachat 0:31e91bb0ef3c 136 // Step dir pins first, then step pinse, stepper drivers like to know the direction before the step signal comes in
scachat 0:31e91bb0ef3c 137 this->alpha_dir_pin->set( ( this->out_bits >> 0 ) & 1 );
scachat 0:31e91bb0ef3c 138 this->beta_dir_pin->set( ( this->out_bits >> 1 ) & 1 );
scachat 0:31e91bb0ef3c 139 this->gamma_dir_pin->set( ( this->out_bits >> 2 ) & 1 );
scachat 0:31e91bb0ef3c 140 this->alpha_step_pin->set( ( this->out_bits >> 3 ) & 1 );
scachat 0:31e91bb0ef3c 141 this->beta_step_pin->set( ( this->out_bits >> 4 ) & 1 );
scachat 0:31e91bb0ef3c 142 this->gamma_step_pin->set( ( this->out_bits >> 5 ) & 1 );
scachat 0:31e91bb0ef3c 143
scachat 0:31e91bb0ef3c 144 if( this->current_block != NULL ){
scachat 0:31e91bb0ef3c 145 // Set bits for direction and steps
scachat 0:31e91bb0ef3c 146 this->out_bits = this->current_block->direction_bits;
scachat 0:31e91bb0ef3c 147 for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){
scachat 0:31e91bb0ef3c 148 this->counters[stpr] += this->counter_increment;
scachat 0:31e91bb0ef3c 149 if( this->counters[stpr] > this->offsets[stpr] && this->stepped[stpr] < this->current_block->steps[stpr] ){
scachat 0:31e91bb0ef3c 150 this->counters[stpr] -= this->offsets[stpr] ;
scachat 0:31e91bb0ef3c 151 this->stepped[stpr]++;
scachat 0:31e91bb0ef3c 152 this->out_bits |= (1 << (stpr+3) );
scachat 0:31e91bb0ef3c 153 }
scachat 0:31e91bb0ef3c 154 }
scachat 0:31e91bb0ef3c 155 // If current block is finished, reset pointer
scachat 0:31e91bb0ef3c 156 this->step_events_completed += this->counter_increment;
scachat 0:31e91bb0ef3c 157 if( this->step_events_completed >= this->current_block->steps_event_count<<16 ){
scachat 0:31e91bb0ef3c 158 if( this->stepped[ALPHA_STEPPER] == this->current_block->steps[ALPHA_STEPPER] && this->stepped[BETA_STEPPER] == this->current_block->steps[BETA_STEPPER] && this->stepped[GAMMA_STEPPER] == this->current_block->steps[GAMMA_STEPPER] ){
scachat 0:31e91bb0ef3c 159 if( this->current_block != NULL ){
scachat 0:31e91bb0ef3c 160 this->current_block->release();
scachat 0:31e91bb0ef3c 161 }
scachat 0:31e91bb0ef3c 162 }
scachat 0:31e91bb0ef3c 163 }
scachat 0:31e91bb0ef3c 164 }else{
scachat 0:31e91bb0ef3c 165 this->out_bits = 0;
scachat 0:31e91bb0ef3c 166 }
scachat 0:31e91bb0ef3c 167
scachat 0:31e91bb0ef3c 168 }
scachat 0:31e91bb0ef3c 169
scachat 0:31e91bb0ef3c 170 // We compute this here instead of each time in the interrupt
scachat 0:31e91bb0ef3c 171 void Stepper::update_offsets(){
scachat 0:31e91bb0ef3c 172 for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){
scachat 0:31e91bb0ef3c 173 this->offsets[stpr] = (int)floor((float)((float)(1<<16)*(float)((float)this->current_block->steps_event_count / (float)this->current_block->steps[stpr])));
scachat 0:31e91bb0ef3c 174 }
scachat 0:31e91bb0ef3c 175 }
scachat 0:31e91bb0ef3c 176
scachat 0:31e91bb0ef3c 177 // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
scachat 0:31e91bb0ef3c 178 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
scachat 0:31e91bb0ef3c 179 // current_block stays untouched by outside handlers for the duration of this function call.
scachat 0:31e91bb0ef3c 180 uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy ) {
scachat 0:31e91bb0ef3c 181 if(this->current_block && !this->trapezoid_generator_busy && !this->paused ) {
scachat 0:31e91bb0ef3c 182 if(this->step_events_completed < this->current_block->accelerate_until<<16) {
scachat 0:31e91bb0ef3c 183 this->trapezoid_adjusted_rate += this->current_block->rate_delta;
scachat 0:31e91bb0ef3c 184 if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
scachat 0:31e91bb0ef3c 185 this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
scachat 0:31e91bb0ef3c 186 }
scachat 0:31e91bb0ef3c 187 this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
scachat 0:31e91bb0ef3c 188 } else if (this->step_events_completed >= this->current_block->decelerate_after<<16) {
scachat 0:31e91bb0ef3c 189 // NOTE: We will only reduce speed if the result will be > 0. This catches small
scachat 0:31e91bb0ef3c 190 // rounding errors that might leave steps hanging after the last trapezoid tick.
scachat 0:31e91bb0ef3c 191 if (this->trapezoid_adjusted_rate > double(this->current_block->rate_delta) * 1.5) {
scachat 0:31e91bb0ef3c 192 this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
scachat 0:31e91bb0ef3c 193 }else{
scachat 0:31e91bb0ef3c 194 this->trapezoid_adjusted_rate = double(this->current_block->rate_delta) * 1.5;
scachat 0:31e91bb0ef3c 195 //this->trapezoid_adjusted_rate = floor(double(this->trapezoid_adjusted_rate / 2 ));
scachat 0:31e91bb0ef3c 196 //this->kernel->serial->printf("over!\r\n");
scachat 0:31e91bb0ef3c 197 }
scachat 0:31e91bb0ef3c 198 if (this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
scachat 0:31e91bb0ef3c 199 this->trapezoid_adjusted_rate = this->current_block->final_rate;
scachat 0:31e91bb0ef3c 200 }
scachat 0:31e91bb0ef3c 201 this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
scachat 0:31e91bb0ef3c 202 } else {
scachat 0:31e91bb0ef3c 203 // Make sure we cruise at exactly nominal rate
scachat 0:31e91bb0ef3c 204 if (this->trapezoid_adjusted_rate != this->current_block->nominal_rate) {
scachat 0:31e91bb0ef3c 205 this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
scachat 0:31e91bb0ef3c 206 this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
scachat 0:31e91bb0ef3c 207 }
scachat 0:31e91bb0ef3c 208 }
scachat 0:31e91bb0ef3c 209 }
scachat 0:31e91bb0ef3c 210 }
scachat 0:31e91bb0ef3c 211
scachat 0:31e91bb0ef3c 212 // Initializes the trapezoid generator from the current block. Called whenever a new
scachat 0:31e91bb0ef3c 213 // block begins.
scachat 0:31e91bb0ef3c 214 void Stepper::trapezoid_generator_reset(){
scachat 0:31e91bb0ef3c 215 this->trapezoid_adjusted_rate = this->current_block->initial_rate;
scachat 0:31e91bb0ef3c 216 this->trapezoid_tick_cycle_counter = 0;
scachat 0:31e91bb0ef3c 217 // Because this can be called directly from the main loop, it could be interrupted by the acceleration ticker, and that would be bad, so we use a flag
scachat 0:31e91bb0ef3c 218 this->trapezoid_generator_busy = true;
scachat 0:31e91bb0ef3c 219 this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
scachat 0:31e91bb0ef3c 220 this->trapezoid_generator_busy = false;
scachat 0:31e91bb0ef3c 221 }
scachat 0:31e91bb0ef3c 222
scachat 0:31e91bb0ef3c 223 void Stepper::set_step_events_per_minute( double steps_per_minute ){
scachat 0:31e91bb0ef3c 224
scachat 0:31e91bb0ef3c 225 // We do not step slower than this
scachat 0:31e91bb0ef3c 226 steps_per_minute = max(steps_per_minute, this->minimum_steps_per_minute);
scachat 0:31e91bb0ef3c 227
scachat 0:31e91bb0ef3c 228 // The speed factor is the factor by which we must multiply the minimal step frequency to reach the maximum step frequency
scachat 0:31e91bb0ef3c 229 // The higher, the smoother the movement will be, but the closer the maximum and minimum frequencies are, the smaller the factor is
scachat 0:31e91bb0ef3c 230 // speed_factor = base_stepping_frequency / steps_per_second
scachat 0:31e91bb0ef3c 231 // counter_increment = 1 / speed_factor ( 1 is 1<<16 because we do fixed point )
scachat 0:31e91bb0ef3c 232 this->counter_increment = int(floor(double(1<<16)/double(this->base_stepping_frequency / (steps_per_minute/60L))));
scachat 0:31e91bb0ef3c 233
scachat 0:31e91bb0ef3c 234 this->kernel->call_event(ON_SPEED_CHANGE, this);
scachat 0:31e91bb0ef3c 235
scachat 0:31e91bb0ef3c 236 }
scachat 0:31e91bb0ef3c 237
scachat 0:31e91bb0ef3c 238 uint32_t Stepper::reset_step_pins(uint32_t dummy){
scachat 0:31e91bb0ef3c 239 this->alpha_step_pin->set(0);
scachat 0:31e91bb0ef3c 240 this->beta_step_pin->set(0);
scachat 0:31e91bb0ef3c 241 this->gamma_step_pin->set(0);
scachat 0:31e91bb0ef3c 242
scachat 0:31e91bb0ef3c 243 }