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     this->active= false;
  49
  50     steps_per_mm         = 1.0F;
  51     max_rate             = 50.0F;
  52     minimum_step_rate    = default_minimum_actuator_rate;
  53
  54     last_milestone_steps = 0;
  55     last_milestone_mm    = 0.0F;
  56     current_position_steps= 0;
  57 }
  58
  59
  60 // 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
  61 // we also here check if the move is finished etc ..
  62 // Thi sis in highest priority interrupt so cannot be pre-empted
  63 void StepperMotor::step()
  64 {
  65     // 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
  66     if(!this->active) return;
  67
  68     // output to pins 37t
  69     this->step_pin.set( 1 );
  70     THEKERNEL->step_ticker->reset_step_pins = true;
  71
  72     // move counter back 11t
  73     this->fx_counter -= this->fx_ticks_per_step;
  74
  75     // we have moved a step 9t
  76     this->stepped++;
  77
  78     // keep track of actuators actual position in steps
  79     this->current_position_steps += (this->direction ? -1 : 1);
  80
  81     // Is this move finished ?
  82     if( this->stepped == this->steps_to_move ) {
  83         // Mark it as finished, then StepTicker will call signal_mode_finished()
  84         // This is so we don't call that before all the steps have been generated for this tick()
  85         this->is_move_finished = true;
  86         THEKERNEL->step_ticker->a_move_finished= true;
  87     }
  88 }
  89
  90
  91 // If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
  92 void StepperMotor::signal_move_finished()
  93 {
  94     // work is done ! 8t
  95     this->fx_counter = 0;      // set this to zero here so we don't miss any for next move
  96     this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set again
  97     this->moving = false;
  98     this->steps_to_move = 0;
  99     this->minimum_step_rate = default_minimum_actuator_rate;
 100
 101     // signal it to whatever cares 41t 411t
 102     this->end_hook->call();
 103
 104     // We only need to do this if we were not instructed to move
 105     // 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
 106     if( this->moving == false ) {
 107         this->update_exit_tick();
 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         // No more ticks will be recieved and no more events from StepTicker
 118         THEKERNEL->step_ticker->remove_motor_from_active_list(this);
 119         this->active= false;
 120     } else {
 121         // we will now get ticks and StepTIcker will send us events
 122         THEKERNEL->step_ticker->add_motor_to_active_list(this);
 123         this->active= true;
 124     }
 125 }
 126
 127 // Instruct the StepperMotor to move a certain number of steps
 128 void StepperMotor::move( bool direction, unsigned int steps, float initial_speed)
 129 {
 130     this->dir_pin.set(direction);
 131     this->direction = direction;
 132
 133     // How many steps we have to move until the move is done
 134     this->steps_to_move = steps;
 135
 136     // Zero our tool counters
 137     this->stepped = 0;
 138
 139     // Starting now we are moving
 140     if( steps > 0 ) {
 141         if(initial_speed >= 0.0F) set_speed(initial_speed);
 142         this->moving = true;
 143     } else {
 144         this->moving = false;
 145     }
 146     this->update_exit_tick();
 147 }
 148
 149 // Set the speed at which this stepper moves in steps/sec, should be called set_step_rate()
 150 // we need to make sure that we have a minimum speed here and that it fits the 32bit fixed point fx counters
 151 // Note nothing will really ever go as slow as the minimum speed here, it is just forced to avoid bad errors
 152 // fx_ticks_per_step is what actually sets the step rate, it is fixed point 18.14
 153 void StepperMotor::set_speed( float speed )
 154 {
 155     if(speed < minimum_step_rate) {
 156         speed= minimum_step_rate;
 157     }
 158
 159     // if(speed <= 0.0F) { // we can't actually do 0 but we can get close, need to avoid divide by zero later on
 160     //     this->fx_ticks_per_step= 0xFFFFFFFFUL; // 0.381 steps/sec
 161     //     this->steps_per_second = THEKERNEL->step_ticker->get_frequency() / (this->fx_ticks_per_step >> fx_shift);
 162     //     return;
 163     // }
 164
 165     // How many steps we must output per second
 166     this->steps_per_second = speed;
 167
 168     // set the new speed, NOTE this can be pre-empted by stepticker so the following write needs to be atomic
 169     this->fx_ticks_per_step= floor(fx_increment * THEKERNEL->step_ticker->get_frequency() / speed);
 170 }
 171
 172 // Pause this stepper motor
 173 void StepperMotor::pause()
 174 {
 175     this->paused = true;
 176     this->update_exit_tick();
 177 }
 178
 179 // Unpause this stepper motor
 180 void StepperMotor::unpause()
 181 {
 182     this->paused = false;
 183     this->update_exit_tick();
 184 }
 185
 186
 187 void StepperMotor::change_steps_per_mm(float new_steps)
 188 {
 189     steps_per_mm = new_steps;
 190     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
 191     current_position_steps = last_milestone_steps;
 192 }
 193
 194 void StepperMotor::change_last_milestone(float new_milestone)
 195 {
 196     last_milestone_mm = new_milestone;
 197     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
 198     current_position_steps = last_milestone_steps;
 199 }
 200
 201 int  StepperMotor::steps_to_target(float target)
 202 {
 203     int target_steps = lround(target * steps_per_mm);
 204     return target_steps - last_milestone_steps;
 205 }