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@2:1df0b61d3b5a, 2014-02-28 (annotated)

Committer:
Michael J. Spencer
Date:
Fri Feb 28 18:52:52 2014 -0800
Revision:
2:1df0b61d3b5a
Parent:
0:31e91bb0ef3c 
Update to latest Smoothie.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Michael J. Spencer 2:1df0b61d3b5a 1 /*
Michael J. Spencer 2:1df0b61d3b5a 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)
Michael J. Spencer 2:1df0b61d3b5a 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.
Michael J. Spencer 2:1df0b61d3b5a 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.
Michael J. Spencer 2:1df0b61d3b5a 5 You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
Michael J. Spencer 2:1df0b61d3b5a 6 */
Michael J. Spencer 2:1df0b61d3b5a 7
Michael J. Spencer 2:1df0b61d3b5a 8 #include "Stepper.h"
Michael J. Spencer 2:1df0b61d3b5a 9
Michael J. Spencer 2:1df0b61d3b5a 10 #include "libs/Module.h"
Michael J. Spencer 2:1df0b61d3b5a 11 #include "libs/Kernel.h"
Michael J. Spencer 2:1df0b61d3b5a 12 #include "Planner.h"
Michael J. Spencer 2:1df0b61d3b5a 13 #include "Conveyor.h"
Michael J. Spencer 2:1df0b61d3b5a 14 #include "StepperMotor.h"
Michael J. Spencer 2:1df0b61d3b5a 15
Michael J. Spencer 2:1df0b61d3b5a 16 #include <vector>
Michael J. Spencer 2:1df0b61d3b5a 17 using namespace std;
Michael J. Spencer 2:1df0b61d3b5a 18
Michael J. Spencer 2:1df0b61d3b5a 19 #include "libs/nuts_bolts.h"
Michael J. Spencer 2:1df0b61d3b5a 20 #include "libs/Hook.h"
Michael J. Spencer 2:1df0b61d3b5a 21
Michael J. Spencer 2:1df0b61d3b5a 22 #include <mri.h>
Michael J. Spencer 2:1df0b61d3b5a 23
Michael J. Spencer 2:1df0b61d3b5a 24
Michael J. Spencer 2:1df0b61d3b5a 25 // The stepper reacts to blocks that have XYZ movement to transform them into actual stepper motor moves
Michael J. Spencer 2:1df0b61d3b5a 26 // TODO: This does accel, accel should be in StepperMotor
Michael J. Spencer 2:1df0b61d3b5a 27
Michael J. Spencer 2:1df0b61d3b5a 28 Stepper* stepper;
Michael J. Spencer 2:1df0b61d3b5a 29 uint32_t previous_step_count;
Michael J. Spencer 2:1df0b61d3b5a 30 uint32_t skipped_speed_updates;
Michael J. Spencer 2:1df0b61d3b5a 31 uint32_t speed_ticks_counter;
Michael J. Spencer 2:1df0b61d3b5a 32
Michael J. Spencer 2:1df0b61d3b5a 33 Stepper::Stepper(){
Michael J. Spencer 2:1df0b61d3b5a 34 this->current_block = NULL;
Michael J. Spencer 2:1df0b61d3b5a 35 this->paused = false;
Michael J. Spencer 2:1df0b61d3b5a 36 this->trapezoid_generator_busy = false;
Michael J. Spencer 2:1df0b61d3b5a 37 this->force_speed_update = false;
Michael J. Spencer 2:1df0b61d3b5a 38 skipped_speed_updates = 0;
Michael J. Spencer 2:1df0b61d3b5a 39 }
Michael J. Spencer 2:1df0b61d3b5a 40
Michael J. Spencer 2:1df0b61d3b5a 41 //Called when the module has just been loaded
Michael J. Spencer 2:1df0b61d3b5a 42 void Stepper::on_module_loaded(){
Michael J. Spencer 2:1df0b61d3b5a 43 stepper = this;
Michael J. Spencer 2:1df0b61d3b5a 44 register_for_event(ON_CONFIG_RELOAD);
Michael J. Spencer 2:1df0b61d3b5a 45 this->register_for_event(ON_BLOCK_BEGIN);
Michael J. Spencer 2:1df0b61d3b5a 46 this->register_for_event(ON_BLOCK_END);
Michael J. Spencer 2:1df0b61d3b5a 47 this->register_for_event(ON_GCODE_EXECUTE);
Michael J. Spencer 2:1df0b61d3b5a 48 this->register_for_event(ON_GCODE_RECEIVED);
Michael J. Spencer 2:1df0b61d3b5a 49 this->register_for_event(ON_PLAY);
Michael J. Spencer 2:1df0b61d3b5a 50 this->register_for_event(ON_PAUSE);
Michael J. Spencer 2:1df0b61d3b5a 51
Michael J. Spencer 2:1df0b61d3b5a 52 // Get onfiguration
Michael J. Spencer 2:1df0b61d3b5a 53 this->on_config_reload(this);
Michael J. Spencer 2:1df0b61d3b5a 54
Michael J. Spencer 2:1df0b61d3b5a 55 // Acceleration ticker
Michael J. Spencer 2:1df0b61d3b5a 56 this->acceleration_tick_hook = THEKERNEL->slow_ticker->attach( this->acceleration_ticks_per_second, this, &Stepper::trapezoid_generator_tick );
Michael J. Spencer 2:1df0b61d3b5a 57
Michael J. Spencer 2:1df0b61d3b5a 58 // Attach to the end_of_move stepper event
Michael J. Spencer 2:1df0b61d3b5a 59 THEKERNEL->robot->alpha_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move );
Michael J. Spencer 2:1df0b61d3b5a 60 THEKERNEL->robot->beta_stepper_motor->attach( this, &Stepper::stepper_motor_finished_move );
Michael J. Spencer 2:1df0b61d3b5a 61 THEKERNEL->robot->gamma_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move );
Michael J. Spencer 2:1df0b61d3b5a 62 }
Michael J. Spencer 2:1df0b61d3b5a 63
Michael J. Spencer 2:1df0b61d3b5a 64 // Get configuration from the config file
Michael J. Spencer 2:1df0b61d3b5a 65 void Stepper::on_config_reload(void* argument){
Michael J. Spencer 2:1df0b61d3b5a 66
Michael J. Spencer 2:1df0b61d3b5a 67 this->acceleration_ticks_per_second = THEKERNEL->config->value(acceleration_ticks_per_second_checksum)->by_default(100 )->as_number();
Michael J. Spencer 2:1df0b61d3b5a 68 this->minimum_steps_per_second = THEKERNEL->config->value(minimum_steps_per_minute_checksum )->by_default(3000 )->as_number() / 60.0F;
Michael J. Spencer 2:1df0b61d3b5a 69
Michael J. Spencer 2:1df0b61d3b5a 70 // Steppers start off by default
Michael J. Spencer 2:1df0b61d3b5a 71 this->turn_enable_pins_off();
Michael J. Spencer 2:1df0b61d3b5a 72 }
Michael J. Spencer 2:1df0b61d3b5a 73
Michael J. Spencer 2:1df0b61d3b5a 74 // When the play/pause button is set to pause, or a module calls the ON_PAUSE event
Michael J. Spencer 2:1df0b61d3b5a 75 void Stepper::on_pause(void* argument){
Michael J. Spencer 2:1df0b61d3b5a 76 this->paused = true;
Michael J. Spencer 2:1df0b61d3b5a 77 THEKERNEL->robot->alpha_stepper_motor->pause();
Michael J. Spencer 2:1df0b61d3b5a 78 THEKERNEL->robot->beta_stepper_motor->pause();
Michael J. Spencer 2:1df0b61d3b5a 79 THEKERNEL->robot->gamma_stepper_motor->pause();
Michael J. Spencer 2:1df0b61d3b5a 80 }
Michael J. Spencer 2:1df0b61d3b5a 81
Michael J. Spencer 2:1df0b61d3b5a 82 // When the play/pause button is set to play, or a module calls the ON_PLAY event
Michael J. Spencer 2:1df0b61d3b5a 83 void Stepper::on_play(void* argument){
Michael J. Spencer 2:1df0b61d3b5a 84 // TODO: Re-compute the whole queue for a cold-start
Michael J. Spencer 2:1df0b61d3b5a 85 this->paused = false;
Michael J. Spencer 2:1df0b61d3b5a 86 THEKERNEL->robot->alpha_stepper_motor->unpause();
Michael J. Spencer 2:1df0b61d3b5a 87 THEKERNEL->robot->beta_stepper_motor->unpause();
Michael J. Spencer 2:1df0b61d3b5a 88 THEKERNEL->robot->gamma_stepper_motor->unpause();
Michael J. Spencer 2:1df0b61d3b5a 89 }
Michael J. Spencer 2:1df0b61d3b5a 90
Michael J. Spencer 2:1df0b61d3b5a 91 void Stepper::on_gcode_received(void* argument){
Michael J. Spencer 2:1df0b61d3b5a 92 Gcode* gcode = static_cast<Gcode*>(argument);
Michael J. Spencer 2:1df0b61d3b5a 93 // Attach gcodes to the last block for on_gcode_execute
Michael J. Spencer 2:1df0b61d3b5a 94 if( gcode->has_m && (gcode->m == 84 || gcode->m == 17 || gcode->m == 18 )) {
Michael J. Spencer 2:1df0b61d3b5a 95 THEKERNEL->conveyor->append_gcode(gcode);
Michael J. Spencer 2:1df0b61d3b5a 96 }
Michael J. Spencer 2:1df0b61d3b5a 97 }
Michael J. Spencer 2:1df0b61d3b5a 98
Michael J. Spencer 2:1df0b61d3b5a 99 // React to enable/disable gcodes
Michael J. Spencer 2:1df0b61d3b5a 100 void Stepper::on_gcode_execute(void* argument){
Michael J. Spencer 2:1df0b61d3b5a 101 Gcode* gcode = static_cast<Gcode*>(argument);
Michael J. Spencer 2:1df0b61d3b5a 102
Michael J. Spencer 2:1df0b61d3b5a 103 if( gcode->has_m){
Michael J. Spencer 2:1df0b61d3b5a 104 if( gcode->m == 17 ){
Michael J. Spencer 2:1df0b61d3b5a 105 this->turn_enable_pins_on();
Michael J. Spencer 2:1df0b61d3b5a 106 }
Michael J. Spencer 2:1df0b61d3b5a 107 if( (gcode->m == 84 || gcode->m == 18) && !gcode->has_letter('E') ){
Michael J. Spencer 2:1df0b61d3b5a 108 this->turn_enable_pins_off();
Michael J. Spencer 2:1df0b61d3b5a 109 }
Michael J. Spencer 2:1df0b61d3b5a 110 }
Michael J. Spencer 2:1df0b61d3b5a 111 }
Michael J. Spencer 2:1df0b61d3b5a 112
Michael J. Spencer 2:1df0b61d3b5a 113 // Enable steppers
Michael J. Spencer 2:1df0b61d3b5a 114 void Stepper::turn_enable_pins_on(){
Michael J. Spencer 2:1df0b61d3b5a 115 for (StepperMotor* m : THEKERNEL->robot->actuators)
Michael J. Spencer 2:1df0b61d3b5a 116 m->enable(true);
Michael J. Spencer 2:1df0b61d3b5a 117 this->enable_pins_status = true;
Michael J. Spencer 2:1df0b61d3b5a 118 }
Michael J. Spencer 2:1df0b61d3b5a 119
Michael J. Spencer 2:1df0b61d3b5a 120 // Disable steppers
Michael J. Spencer 2:1df0b61d3b5a 121 void Stepper::turn_enable_pins_off(){
Michael J. Spencer 2:1df0b61d3b5a 122 for (StepperMotor* m : THEKERNEL->robot->actuators)
Michael J. Spencer 2:1df0b61d3b5a 123 m->enable(false);
Michael J. Spencer 2:1df0b61d3b5a 124 this->enable_pins_status = false;
Michael J. Spencer 2:1df0b61d3b5a 125 }
Michael J. Spencer 2:1df0b61d3b5a 126
Michael J. Spencer 2:1df0b61d3b5a 127 // A new block is popped from the queue
Michael J. Spencer 2:1df0b61d3b5a 128 void Stepper::on_block_begin(void* argument){
Michael J. Spencer 2:1df0b61d3b5a 129 Block* block = static_cast<Block*>(argument);
Michael J. Spencer 2:1df0b61d3b5a 130
Michael J. Spencer 2:1df0b61d3b5a 131 // The stepper does not care about 0-blocks
Michael J. Spencer 2:1df0b61d3b5a 132 if( block->millimeters == 0.0F ){ return; }
Michael J. Spencer 2:1df0b61d3b5a 133
Michael J. Spencer 2:1df0b61d3b5a 134 // Mark the new block as of interrest to us
Michael J. Spencer 2:1df0b61d3b5a 135 if( block->steps[ALPHA_STEPPER] > 0 || block->steps[BETA_STEPPER] > 0 || block->steps[GAMMA_STEPPER] > 0 ){
Michael J. Spencer 2:1df0b61d3b5a 136 block->take();
Michael J. Spencer 2:1df0b61d3b5a 137 }else{
Michael J. Spencer 2:1df0b61d3b5a 138 return;
Michael J. Spencer 2:1df0b61d3b5a 139 }
Michael J. Spencer 2:1df0b61d3b5a 140
Michael J. Spencer 2:1df0b61d3b5a 141 // We can't move with the enable pins off
Michael J. Spencer 2:1df0b61d3b5a 142 if( this->enable_pins_status == false ){
Michael J. Spencer 2:1df0b61d3b5a 143 this->turn_enable_pins_on();
Michael J. Spencer 2:1df0b61d3b5a 144 }
Michael J. Spencer 2:1df0b61d3b5a 145
Michael J. Spencer 2:1df0b61d3b5a 146 // Setup : instruct stepper motors to move
Michael J. Spencer 2:1df0b61d3b5a 147 if( block->steps[ALPHA_STEPPER] > 0 ){ THEKERNEL->robot->alpha_stepper_motor->move( ( block->direction_bits >> 0 ) & 1 , block->steps[ALPHA_STEPPER] ); }
Michael J. Spencer 2:1df0b61d3b5a 148 if( block->steps[BETA_STEPPER ] > 0 ){ THEKERNEL->robot->beta_stepper_motor->move( ( block->direction_bits >> 1 ) & 1 , block->steps[BETA_STEPPER ] ); }
Michael J. Spencer 2:1df0b61d3b5a 149 if( block->steps[GAMMA_STEPPER] > 0 ){ THEKERNEL->robot->gamma_stepper_motor->move( ( block->direction_bits >> 2 ) & 1 , block->steps[GAMMA_STEPPER] ); }
Michael J. Spencer 2:1df0b61d3b5a 150
Michael J. Spencer 2:1df0b61d3b5a 151 this->current_block = block;
Michael J. Spencer 2:1df0b61d3b5a 152
Michael J. Spencer 2:1df0b61d3b5a 153 // Setup acceleration for this block
Michael J. Spencer 2:1df0b61d3b5a 154 this->trapezoid_generator_reset();
Michael J. Spencer 2:1df0b61d3b5a 155
Michael J. Spencer 2:1df0b61d3b5a 156 // Find the stepper with the more steps, it's the one the speed calculations will want to follow
Michael J. Spencer 2:1df0b61d3b5a 157 this->main_stepper = THEKERNEL->robot->alpha_stepper_motor;
Michael J. Spencer 2:1df0b61d3b5a 158 if( THEKERNEL->robot->beta_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ){ this->main_stepper = THEKERNEL->robot->beta_stepper_motor; }
Michael J. Spencer 2:1df0b61d3b5a 159 if( THEKERNEL->robot->gamma_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ){ this->main_stepper = THEKERNEL->robot->gamma_stepper_motor; }
Michael J. Spencer 2:1df0b61d3b5a 160
Michael J. Spencer 2:1df0b61d3b5a 161 // Set the initial speed for this move
Michael J. Spencer 2:1df0b61d3b5a 162 this->trapezoid_generator_tick(0);
Michael J. Spencer 2:1df0b61d3b5a 163
Michael J. Spencer 2:1df0b61d3b5a 164 // Synchronise the acceleration curve with the stepping
Michael J. Spencer 2:1df0b61d3b5a 165 this->synchronize_acceleration(0);
Michael J. Spencer 2:1df0b61d3b5a 166
Michael J. Spencer 2:1df0b61d3b5a 167 }
Michael J. Spencer 2:1df0b61d3b5a 168
Michael J. Spencer 2:1df0b61d3b5a 169 // Current block is discarded
Michael J. Spencer 2:1df0b61d3b5a 170 void Stepper::on_block_end(void* argument){
Michael J. Spencer 2:1df0b61d3b5a 171 this->current_block = NULL; //stfu !
Michael J. Spencer 2:1df0b61d3b5a 172 }
Michael J. Spencer 2:1df0b61d3b5a 173
Michael J. Spencer 2:1df0b61d3b5a 174 // When a stepper motor has finished it's assigned movement
Michael J. Spencer 2:1df0b61d3b5a 175 uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy){
Michael J. Spencer 2:1df0b61d3b5a 176
Michael J. Spencer 2:1df0b61d3b5a 177 // We care only if none is still moving
Michael J. Spencer 2:1df0b61d3b5a 178 if( THEKERNEL->robot->alpha_stepper_motor->moving || THEKERNEL->robot->beta_stepper_motor->moving || THEKERNEL->robot->gamma_stepper_motor->moving ){ return 0; }
Michael J. Spencer 2:1df0b61d3b5a 179
Michael J. Spencer 2:1df0b61d3b5a 180 // This block is finished, release it
Michael J. Spencer 2:1df0b61d3b5a 181 if( this->current_block != NULL ){
Michael J. Spencer 2:1df0b61d3b5a 182 this->current_block->release();
Michael J. Spencer 2:1df0b61d3b5a 183 }
Michael J. Spencer 2:1df0b61d3b5a 184
Michael J. Spencer 2:1df0b61d3b5a 185 return 0;
Michael J. Spencer 2:1df0b61d3b5a 186 }
Michael J. Spencer 2:1df0b61d3b5a 187
Michael J. Spencer 2:1df0b61d3b5a 188
Michael J. Spencer 2:1df0b61d3b5a 189 // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
Michael J. Spencer 2:1df0b61d3b5a 190 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
Michael J. Spencer 2:1df0b61d3b5a 191 // current_block stays untouched by outside handlers for the duration of this function call.
Michael J. Spencer 2:1df0b61d3b5a 192 uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy ) {
Michael J. Spencer 2:1df0b61d3b5a 193
Michael J. Spencer 2:1df0b61d3b5a 194 // Do not do the accel math for nothing
Michael J. Spencer 2:1df0b61d3b5a 195 if(this->current_block && !this->paused && this->main_stepper->moving ) {
Michael J. Spencer 2:1df0b61d3b5a 196
Michael J. Spencer 2:1df0b61d3b5a 197 // Store this here because we use it a lot down there
Michael J. Spencer 2:1df0b61d3b5a 198 uint32_t current_steps_completed = this->main_stepper->stepped;
Michael J. Spencer 2:1df0b61d3b5a 199
Michael J. Spencer 2:1df0b61d3b5a 200 // Do not accel, just set the value
Michael J. Spencer 2:1df0b61d3b5a 201 if( this->force_speed_update ){
Michael J. Spencer 2:1df0b61d3b5a 202 this->force_speed_update = false;
Michael J. Spencer 2:1df0b61d3b5a 203 this->set_step_events_per_second(this->trapezoid_adjusted_rate);
Michael J. Spencer 2:1df0b61d3b5a 204 return 0;
Michael J. Spencer 2:1df0b61d3b5a 205 }
Michael J. Spencer 2:1df0b61d3b5a 206
Michael J. Spencer 2:1df0b61d3b5a 207 // If we are accelerating
Michael J. Spencer 2:1df0b61d3b5a 208 if(current_steps_completed <= this->current_block->accelerate_until + 1) {
Michael J. Spencer 2:1df0b61d3b5a 209 // Increase speed
Michael J. Spencer 2:1df0b61d3b5a 210 this->trapezoid_adjusted_rate += this->current_block->rate_delta;
Michael J. Spencer 2:1df0b61d3b5a 211 if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
Michael J. Spencer 2:1df0b61d3b5a 212 this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
Michael J. Spencer 2:1df0b61d3b5a 213 }
Michael J. Spencer 2:1df0b61d3b5a 214 this->set_step_events_per_second(this->trapezoid_adjusted_rate);
Michael J. Spencer 2:1df0b61d3b5a 215
Michael J. Spencer 2:1df0b61d3b5a 216 // If we are decelerating
Michael J. Spencer 2:1df0b61d3b5a 217 }else if (current_steps_completed > this->current_block->decelerate_after) {
Michael J. Spencer 2:1df0b61d3b5a 218 // Reduce speed
Michael J. Spencer 2:1df0b61d3b5a 219 // NOTE: We will only reduce speed if the result will be > 0. This catches small
Michael J. Spencer 2:1df0b61d3b5a 220 // rounding errors that might leave steps hanging after the last trapezoid tick.
Michael J. Spencer 2:1df0b61d3b5a 221 if(this->trapezoid_adjusted_rate > this->current_block->rate_delta * 1.5F) {
Michael J. Spencer 2:1df0b61d3b5a 222 this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
Michael J. Spencer 2:1df0b61d3b5a 223 }else{
Michael J. Spencer 2:1df0b61d3b5a 224 this->trapezoid_adjusted_rate = this->current_block->rate_delta * 1.5F;
Michael J. Spencer 2:1df0b61d3b5a 225 }
Michael J. Spencer 2:1df0b61d3b5a 226 if(this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
Michael J. Spencer 2:1df0b61d3b5a 227 this->trapezoid_adjusted_rate = this->current_block->final_rate;
Michael J. Spencer 2:1df0b61d3b5a 228 }
Michael J. Spencer 2:1df0b61d3b5a 229 this->set_step_events_per_second(this->trapezoid_adjusted_rate);
Michael J. Spencer 2:1df0b61d3b5a 230
Michael J. Spencer 2:1df0b61d3b5a 231 // If we are cruising
Michael J. Spencer 2:1df0b61d3b5a 232 }else {
Michael J. Spencer 2:1df0b61d3b5a 233 // Make sure we cruise at exactly nominal rate
Michael J. Spencer 2:1df0b61d3b5a 234 if (this->trapezoid_adjusted_rate != this->current_block->nominal_rate) {
Michael J. Spencer 2:1df0b61d3b5a 235 this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
Michael J. Spencer 2:1df0b61d3b5a 236 this->set_step_events_per_second(this->trapezoid_adjusted_rate);
Michael J. Spencer 2:1df0b61d3b5a 237 }
Michael J. Spencer 2:1df0b61d3b5a 238 }
Michael J. Spencer 2:1df0b61d3b5a 239 }
Michael J. Spencer 2:1df0b61d3b5a 240
Michael J. Spencer 2:1df0b61d3b5a 241 return 0;
Michael J. Spencer 2:1df0b61d3b5a 242 }
Michael J. Spencer 2:1df0b61d3b5a 243
Michael J. Spencer 2:1df0b61d3b5a 244
Michael J. Spencer 2:1df0b61d3b5a 245
Michael J. Spencer 2:1df0b61d3b5a 246 // Initializes the trapezoid generator from the current block. Called whenever a new
Michael J. Spencer 2:1df0b61d3b5a 247 // block begins.
Michael J. Spencer 2:1df0b61d3b5a 248 inline void Stepper::trapezoid_generator_reset(){
Michael J. Spencer 2:1df0b61d3b5a 249 this->trapezoid_adjusted_rate = this->current_block->initial_rate;
Michael J. Spencer 2:1df0b61d3b5a 250 this->force_speed_update = true;
Michael J. Spencer 2:1df0b61d3b5a 251 this->trapezoid_tick_cycle_counter = 0;
Michael J. Spencer 2:1df0b61d3b5a 252 previous_step_count = 0;
Michael J. Spencer 2:1df0b61d3b5a 253 skipped_speed_updates = 0;
Michael J. Spencer 2:1df0b61d3b5a 254 speed_ticks_counter = 0;
Michael J. Spencer 2:1df0b61d3b5a 255 }
Michael J. Spencer 2:1df0b61d3b5a 256
Michael J. Spencer 2:1df0b61d3b5a 257 // Update the speed for all steppers
Michael J. Spencer 2:1df0b61d3b5a 258 void Stepper::set_step_events_per_second( float steps_per_second )
Michael J. Spencer 2:1df0b61d3b5a 259 {
Michael J. Spencer 2:1df0b61d3b5a 260 // We do not step slower than this
Michael J. Spencer 2:1df0b61d3b5a 261 //steps_per_second = max(steps_per_second, this->minimum_steps_per_second);
Michael J. Spencer 2:1df0b61d3b5a 262 if( steps_per_second < this->minimum_steps_per_second ){
Michael J. Spencer 2:1df0b61d3b5a 263 steps_per_second = this->minimum_steps_per_second;
Michael J. Spencer 2:1df0b61d3b5a 264 }
Michael J. Spencer 2:1df0b61d3b5a 265
Michael J. Spencer 2:1df0b61d3b5a 266 // Instruct the stepper motors
Michael J. Spencer 2:1df0b61d3b5a 267 if( THEKERNEL->robot->alpha_stepper_motor->moving ){ THEKERNEL->robot->alpha_stepper_motor->set_speed( steps_per_second * ( (float)this->current_block->steps[ALPHA_STEPPER] / (float)this->current_block->steps_event_count ) ); }
Michael J. Spencer 2:1df0b61d3b5a 268 if( THEKERNEL->robot->beta_stepper_motor->moving ){ THEKERNEL->robot->beta_stepper_motor->set_speed( steps_per_second * ( (float)this->current_block->steps[BETA_STEPPER ] / (float)this->current_block->steps_event_count ) ); }
Michael J. Spencer 2:1df0b61d3b5a 269 if( THEKERNEL->robot->gamma_stepper_motor->moving ){ THEKERNEL->robot->gamma_stepper_motor->set_speed( steps_per_second * ( (float)this->current_block->steps[GAMMA_STEPPER] / (float)this->current_block->steps_event_count ) ); }
Michael J. Spencer 2:1df0b61d3b5a 270
Michael J. Spencer 2:1df0b61d3b5a 271 // Other modules might want to know the speed changed
Michael J. Spencer 2:1df0b61d3b5a 272 THEKERNEL->call_event(ON_SPEED_CHANGE, this);
Michael J. Spencer 2:1df0b61d3b5a 273
Michael J. Spencer 2:1df0b61d3b5a 274 }
Michael J. Spencer 2:1df0b61d3b5a 275
Michael J. Spencer 2:1df0b61d3b5a 276 // This function has the role of making sure acceleration and deceleration curves have their
Michael J. Spencer 2:1df0b61d3b5a 277 // rhythm synchronized. The accel/decel must start at the same moment as the speed update routine
Michael J. Spencer 2:1df0b61d3b5a 278 // This is caller in "step just occured" or "block just began" ( step Timer ) context, so we need to be fast.
Michael J. Spencer 2:1df0b61d3b5a 279 // All we do is reset the other timer so that it does what we want
Michael J. Spencer 2:1df0b61d3b5a 280 uint32_t Stepper::synchronize_acceleration(uint32_t dummy){
Michael J. Spencer 2:1df0b61d3b5a 281
Michael J. Spencer 2:1df0b61d3b5a 282 // No move was done, this is called from on_block_begin
Michael J. Spencer 2:1df0b61d3b5a 283 // This means we setup the accel timer in a way where it gets called right after
Michael J. Spencer 2:1df0b61d3b5a 284 // we exit this step interrupt, and so that it is then in synch with
Michael J. Spencer 2:1df0b61d3b5a 285 if( this->main_stepper->stepped == 0 ){
Michael J. Spencer 2:1df0b61d3b5a 286 // Whatever happens, we must call the accel interrupt asap
Michael J. Spencer 2:1df0b61d3b5a 287 // Because it will set the initial rate
Michael J. Spencer 2:1df0b61d3b5a 288 // We also want to synchronize in case we start accelerating or decelerating now
Michael J. Spencer 2:1df0b61d3b5a 289
Michael J. Spencer 2:1df0b61d3b5a 290 // Accel interrupt must happen asap
Michael J. Spencer 2:1df0b61d3b5a 291 NVIC_SetPendingIRQ(TIMER2_IRQn);
Michael J. Spencer 2:1df0b61d3b5a 292 // Synchronize both counters
Michael J. Spencer 2:1df0b61d3b5a 293 LPC_TIM2->TC = LPC_TIM0->TC;
Michael J. Spencer 2:1df0b61d3b5a 294
Michael J. Spencer 2:1df0b61d3b5a 295 // If we start decelerating after this, we must ask the actuator to warn us
Michael J. Spencer 2:1df0b61d3b5a 296 // so we can do what we do in the "else" bellow
Michael J. Spencer 2:1df0b61d3b5a 297 if( this->current_block->decelerate_after > 0 && this->current_block->decelerate_after < this->main_stepper->steps_to_move ){
Michael J. Spencer 2:1df0b61d3b5a 298 this->main_stepper->attach_signal_step(this->current_block->decelerate_after, this, &Stepper::synchronize_acceleration);
Michael J. Spencer 2:1df0b61d3b5a 299 }
Michael J. Spencer 2:1df0b61d3b5a 300 }else{
Michael J. Spencer 2:1df0b61d3b5a 301 // If we are called not at the first steps, this means we are beginning deceleration
Michael J. Spencer 2:1df0b61d3b5a 302 NVIC_SetPendingIRQ(TIMER2_IRQn);
Michael J. Spencer 2:1df0b61d3b5a 303 // Synchronize both counters
Michael J. Spencer 2:1df0b61d3b5a 304 LPC_TIM2->TC = LPC_TIM0->TC;
Michael J. Spencer 2:1df0b61d3b5a 305 }
Michael J. Spencer 2:1df0b61d3b5a 306
Michael J. Spencer 2:1df0b61d3b5a 307 return 0;
Michael J. Spencer 2:1df0b61d3b5a 308 }
Michael J. Spencer 2:1df0b61d3b5a 309