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 // TODO : Abstract this into Actuator
  20
  21 StepperMotor::StepperMotor()
  22 {
  23     init();
  24 }
  25
  26 StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_pin(dir), en_pin(en)
  27 {
  28     init();
  29     enable(false);
  30     set_high_on_debug(en.port_number, en.pin);
  31 }
  32
  33 StepperMotor::~StepperMotor()
  34 {
  35 }
  36
  37 void StepperMotor::init()
  38 {
  39     // register this motor with the step ticker, and get its index in that array and bit position
  40     this->index= THEKERNEL->step_ticker->register_motor(this);
  41     this->moving = false;
  42     this->paused = false;
  43     this->fx_counter = 0;
  44     this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set
  45     this->stepped = 0;
  46     this->steps_to_move = 0;
  47     this->is_move_finished = 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 // Thi sis in highest priority interrupt so cannot be pre-empted
  62 void StepperMotor::step()
  63 {
  64     // we can't do anything until the next move has been processed, but we will be able to offset the time by shortening the next step
  65     if(!this->moving) return;
  66
  67     // output to pins 37t
  68     this->step_pin.set( 1 );
  69     THEKERNEL->step_ticker->reset_step_pins = true;
  70
  71     // move counter back 11t
  72     this->fx_counter -= this->fx_ticks_per_step;
  73
  74     // we have moved a step 9t
  75     this->stepped++;
  76
  77     // keep track of actuators actual position in steps
  78     this->current_position_steps += (this->direction ? -1 : 1);
  79
  80     // Is this move finished ?
  81     if( this->stepped == this->steps_to_move ) {
  82         // Mark it as finished, then StepTicker will call signal_mode_finished()
  83         // This is so we don't call that before all the steps have been generated for this tick()
  84         this->is_move_finished = true;
  85         THEKERNEL->step_ticker->a_move_finished= true;
  86         this->fx_counter = 0;      // set this to zero here so we don't miss any for next move
  87         this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set again
  88     }
  89 }
  90
  91
  92 // If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
  93 void StepperMotor::signal_move_finished()
  94 {
  95     // work is done ! 8t
  96     this->moving = false;
  97     this->steps_to_move = 0;
  98     this->minimum_step_rate = default_minimum_actuator_rate;
  99
 100     // signal it to whatever cares 41t 411t
 101     this->end_hook->call();
 102
 103     // We only need to do this if we were not instructed to move
 104     // FIXME trouble here is that if this stops first but other axis are still going we will now stop getting ticks so we cannot know how long we were delayed
 105     if( this->moving == false ) {
 106         this->update_exit_tick();
 107         this->fx_counter = 0; // so it starts at zero next time we activate it, as we won't be getting any more ticks until then
 108     }
 109
 110     this->is_move_finished = false;
 111 }
 112
 113 // This is just a way not to check for ( !this->moving || this->paused || this->fx_ticks_per_step == 0 ) at every tick()
 114 void StepperMotor::update_exit_tick()
 115 {
 116     if( !this->moving || this->paused || this->steps_to_move == 0 ) {
 117         // We must exit tick() after setting the pins, no bresenham is done
 118         THEKERNEL->step_ticker->remove_motor_from_active_list(this);
 119     } else {
 120         // We must do the bresenham in tick()
 121         // We have to do this or there could be a bug where the removal still happens when it doesn't need to
 122         THEKERNEL->step_ticker->add_motor_to_active_list(this);
 123     }
 124 }
 125
 126 // Instruct the StepperMotor to move a certain number of steps
 127 void StepperMotor::move( bool direction, unsigned int steps, float initial_speed)
 128 {
 129     this->dir_pin.set(direction);
 130     this->direction = direction;
 131
 132     // How many steps we have to move until the move is done
 133     this->steps_to_move = steps;
 134
 135     // Zero our tool counters
 136     this->stepped = 0;
 137
 138     // Starting now we are moving
 139     if( steps > 0 ) {
 140         if(initial_speed >= 0.0F) set_speed(initial_speed);
 141         this->moving = true;
 142     } else {
 143         this->moving = false;
 144     }
 145     this->update_exit_tick();
 146 }
 147
 148 // Set the speed at which this stepper moves in steps/sec, should be called set_step_rate()
 149 // we need to make sure that we have a minimum speed here and that it fits the 32bit fixed point fx counters
 150 // Note nothing will really ever go as slow as the minimum speed here, it is just forced to avoid bad errors
 151 // fx_ticks_per_step is what actually sets the step rate, it is fixed point 18.14
 152 void StepperMotor::set_speed( float speed )
 153 {
 154     if(speed < minimum_step_rate) {
 155         speed= minimum_step_rate;
 156     }
 157
 158     // if(speed <= 0.0F) { // we can't actually do 0 but we can get close, need to avoid divide by zero later on
 159     //     this->fx_ticks_per_step= 0xFFFFFFFFUL; // 0.381 steps/sec
 160     //     this->steps_per_second = THEKERNEL->step_ticker->get_frequency() / (this->fx_ticks_per_step >> fx_shift);
 161     //     return;
 162     // }
 163
 164     // How many steps we must output per second
 165     this->steps_per_second = speed;
 166
 167     // set the new speed, NOTE this can be pre-empted by stepticker so the following write needs to be atomic
 168     this->fx_ticks_per_step= floor(fx_increment * THEKERNEL->step_ticker->get_frequency() / speed);
 169 }
 170
 171 // Pause this stepper motor
 172 void StepperMotor::pause()
 173 {
 174     this->paused = true;
 175     this->update_exit_tick();
 176 }
 177
 178 // Unpause this stepper motor
 179 void StepperMotor::unpause()
 180 {
 181     this->paused = false;
 182     this->update_exit_tick();
 183 }
 184
 185
 186 void StepperMotor::change_steps_per_mm(float new_steps)
 187 {
 188     steps_per_mm = new_steps;
 189     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
 190     current_position_steps = last_milestone_steps;
 191 }
 192
 193 void StepperMotor::change_last_milestone(float new_milestone)
 194 {
 195     last_milestone_mm = new_milestone;
 196     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
 197     current_position_steps = last_milestone_steps;
 198 }
 199
 200 int  StepperMotor::steps_to_target(float target)
 201 {
 202     int target_steps = lround(target * steps_per_mm);
 203     return target_steps - last_milestone_steps;
 204 }