src/libs/StepperMotor.cpp

   1 /*
   2       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
   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.
   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.
   5       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
   6 */
   7 #include "StepperMotor.h"
   8
   9 #include "Kernel.h"
  10 #include "MRI_Hooks.h"
  11 #include "StepTicker.h"
  12
  13 #include <math.h>
  14
  15 // in steps/sec the default minimum speed (was 20steps/sec hardcoded)
  16 float StepperMotor::default_minimum_actuator_rate= 20.0F;
  17
  18 // A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed
  19
  20 StepperMotor::StepperMotor()
  21 {
  22     init();
  23 }
  24
  25 StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_pin(dir), en_pin(en)
  26 {
  27     init();
  28     enable(false);
  29     set_high_on_debug(en.port_number, en.pin);
  30 }
  31
  32 StepperMotor::~StepperMotor()
  33 {
  34 }
  35
  36 void StepperMotor::init()
  37 {
  38     // register this motor with the step ticker, and get its index in that array and bit position
  39     this->index= THEKERNEL->step_ticker->register_motor(this);
  40     this->moving = false;
  41     this->paused = false;
  42     this->fx_counter = 0;
  43     this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set
  44     this->stepped = 0;
  45     this->steps_to_move = 0;
  46     this->is_move_finished = false;
  47     this->last_step_tick_valid= false;
  48
  49     steps_per_mm         = 1.0F;
  50     max_rate             = 50.0F;
  51     minimum_step_rate    = default_minimum_actuator_rate;
  52
  53     last_milestone_steps = 0;
  54     last_milestone_mm    = 0.0F;
  55     current_position_steps= 0;
  56 }
  57
  58
  59 // This is called ( see the .h file, we had to put a part of things there for obscure inline reasons ) when a step has to be generated
  60 // we also here check if the move is finished etc ..
  61 // This is in highest priority interrupt so cannot be pre-empted
  62 void StepperMotor::step()
  63 {
  64     // output to pins 37t
  65     this->step_pin.set( 1 );
  66     THEKERNEL->step_ticker->reset_step_pins = true;
  67
  68     // move counter back 11t
  69     this->fx_counter -= this->fx_ticks_per_step;
  70
  71     // we have moved a step 9t
  72     this->stepped++;
  73
  74     // keep track of actuators actual position in steps
  75     this->current_position_steps += (this->direction ? -1 : 1);
  76
  77     // Is this move finished ?
  78     if( this->stepped == this->steps_to_move ) {
  79         // Mark it as finished, then StepTicker will call signal_mode_finished()
  80         // This is so we don't call that before all the steps have been generated for this tick()
  81         this->is_move_finished = true;
  82         THEKERNEL->step_ticker->a_move_finished= true;
  83         this->last_step_tick= THEKERNEL->step_ticker->get_tick_cnt(); // remember when last step was
  84     }
  85 }
  86
  87
  88 // If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
  89 void StepperMotor::signal_move_finished()
  90 {
  91     // work is done ! 8t
  92     this->moving = false;
  93     this->steps_to_move = 0;
  94     this->minimum_step_rate = default_minimum_actuator_rate;
  95
  96     // signal it to whatever cares
  97     // in this call a new block may start, new moves set and new speeds
  98     this->end_hook->call();
  99
 100     // We only need to do this if we were not instructed to move
 101     if( !this->moving ) {
 102         this->update_exit_tick();
 103     }
 104
 105     this->is_move_finished = false;
 106 }
 107
 108 // This is just a way not to check for ( !this->moving || this->paused || this->fx_ticks_per_step == 0 ) at every tick()
 109 void StepperMotor::update_exit_tick()
 110 {
 111     if( !this->moving || this->paused || this->steps_to_move == 0 ) {
 112         // No more ticks will be recieved and no more events from StepTicker
 113         THEKERNEL->step_ticker->remove_motor_from_active_list(this);
 114     } else {
 115         // we will now get ticks and StepTIcker will send us events
 116         THEKERNEL->step_ticker->add_motor_to_active_list(this);
 117     }
 118 }
 119
 120 // Instruct the StepperMotor to move a certain number of steps
 121 StepperMotor* StepperMotor::move( bool direction, unsigned int steps, float initial_speed)
 122 {
 123     this->dir_pin.set(direction);
 124     this->direction = direction;
 125
 126     // How many steps we have to move until the move is done
 127     this->steps_to_move = steps;
 128
 129     // Zero our tool counters
 130     this->stepped = 0;
 131     this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set again
 132     if(this->last_step_tick_valid) {
 133         // NOTE only steppermotors managed in Stepper.cpp set or unset this flag
 134         // we set this based on when the last step was, thus compensating for missed ticks
 135         this->fx_counter= THEKERNEL->step_ticker->ticks_since(this->last_step_tick)*fx_increment;
 136     }else{
 137         this->fx_counter = 0; // set to zero as there was no step last block
 138     }
 139
 140     // Starting now we are moving
 141     if( steps > 0 ) {
 142         if(initial_speed >= 0.0F) set_speed(initial_speed);
 143         this->moving = true;
 144     } else {
 145         this->moving = false;
 146     }
 147     this->update_exit_tick();
 148     return this;
 149 }
 150
 151 // Set the speed at which this stepper moves in steps/sec, should be called set_step_rate()
 152 // we need to make sure that we have a minimum speed here and that it fits the 32bit fixed point fx counters
 153 // Note nothing will really ever go as slow as the minimum speed here, it is just forced to avoid bad errors
 154 // fx_ticks_per_step is what actually sets the step rate, it is fixed point 18.14
 155 StepperMotor* StepperMotor::set_speed( float speed )
 156 {
 157     if(speed < minimum_step_rate) {
 158         speed= minimum_step_rate;
 159     }
 160
 161     // if(speed <= 0.0F) { // we can't actually do 0 but we can get close, need to avoid divide by zero later on
 162     //     this->fx_ticks_per_step= 0xFFFFFFFFUL; // 0.381 steps/sec
 163     //     this->steps_per_second = THEKERNEL->step_ticker->get_frequency() / (this->fx_ticks_per_step >> fx_shift);
 164     //     return;
 165     // }
 166
 167     // How many steps we must output per second
 168     this->steps_per_second = speed;
 169
 170     // set the new speed, NOTE this can be pre-empted by stepticker so the following write needs to be atomic
 171     this->fx_ticks_per_step= floor(fx_increment * THEKERNEL->step_ticker->get_frequency() / speed);
 172     return this;
 173 }
 174
 175 // Pause this stepper motor
 176 void StepperMotor::pause()
 177 {
 178     this->paused = true;
 179     this->update_exit_tick();
 180 }
 181
 182 // Unpause this stepper motor
 183 void StepperMotor::unpause()
 184 {
 185     this->paused = false;
 186     this->update_exit_tick();
 187 }
 188
 189
 190 void StepperMotor::change_steps_per_mm(float new_steps)
 191 {
 192     steps_per_mm = new_steps;
 193     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
 194     current_position_steps = last_milestone_steps;
 195 }
 196
 197 void StepperMotor::change_last_milestone(float new_milestone)
 198 {
 199     last_milestone_mm = new_milestone;
 200     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
 201     current_position_steps = last_milestone_steps;
 202 }
 203
 204 int  StepperMotor::steps_to_target(float target)
 205 {
 206     int target_steps = lround(target * steps_per_mm);
 207     return target_steps - last_milestone_steps;
 208 }