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 = 0xFFFFF00000000000ULL; // 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 void StepperMotor::step()
  62 {
  63     // 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
  64     if(this->is_move_finished) return;
  65
  66     // output to pins 37t
  67     this->step_pin.set( 1 );
  68     THEKERNEL->step_ticker->reset_step_pins = true;
  69
  70     // move counter back 11t
  71     if(this->fx_counter > this->fx_ticks_per_step) {
  72         this->fx_counter -= this->fx_ticks_per_step;
  73     }else{
  74         this->fx_counter= 0; // can't make it less than 0 as it is a uint, unless we get interrupted I don't think this case is possible
  75     }
  76
  77     // we have moved a step 9t
  78     this->stepped++;
  79
  80     // keep track of actuators actual position in steps
  81     this->current_position_steps += (this->direction ? -1 : 1);
  82
  83     // Is this move finished ?
  84     if( this->stepped == this->steps_to_move ) {
  85         // Mark it as finished, then StepTicker will call signal_mode_finished()
  86         // This is so we don't call that before all the steps have been generated for this tick()
  87         this->is_move_finished = true;
  88         THEKERNEL->step_ticker->a_move_finished = true;
  89         this->fx_counter = 0;      // set this to zero here so we don't miss any for next move
  90         this->fx_ticks_per_step = 0xFFFFF00000000000ULL; // some big number so we don't start stepping before it is set again
  91     }
  92 }
  93
  94
  95 // If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
  96 void StepperMotor::signal_move_finished()
  97 {
  98     // work is done ! 8t
  99     this->moving = false;
 100     this->steps_to_move = 0;
 101     this->minimum_step_rate = default_minimum_actuator_rate;
 102
 103     // signal it to whatever cares 41t 411t
 104     this->end_hook->call();
 105
 106     // We only need to do this if we were not instructed to move
 107     if( this->moving == false ) {
 108         this->update_exit_tick();
 109         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
 110     }
 111
 112     this->is_move_finished = false;
 113 }
 114
 115 // This is just a way not to check for ( !this->moving || this->paused || this->fx_ticks_per_step == 0 ) at every tick()
 116 void StepperMotor::update_exit_tick()
 117 {
 118     if( !this->moving || this->paused || this->steps_to_move == 0 ) {
 119         // We must exit tick() after setting the pins, no bresenham is done
 120         THEKERNEL->step_ticker->remove_motor_from_active_list(this);
 121     } else {
 122         // We must do the bresenham in tick()
 123         // We have to do this or there could be a bug where the removal still happens when it doesn't need to
 124         THEKERNEL->step_ticker->add_motor_to_active_list(this);
 125     }
 126 }
 127
 128 // Instruct the StepperMotor to move a certain number of steps
 129 void StepperMotor::move( bool direction, unsigned int steps, float initial_speed)
 130 {
 131     this->dir_pin.set(direction);
 132     this->direction = direction;
 133
 134     // How many steps we have to move until the move is done
 135     this->steps_to_move = steps;
 136
 137     // Zero our tool counters
 138     this->stepped = 0;
 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 }
 149
 150 // this is called to set the step rate based on this blocks rate, we use this instead of set_speed for coordinated moves
 151 // so that we can floor to a minimum speed which is proportional for all axis. the minimum step rate is set at the start
 152 // of each block based on the slowest axis of all coordinated axis.
 153 // the rate passed in is the requested rate, it is scaled for this motor based on steps_to_move and block_steps_event_count
 154 void StepperMotor::set_step_rate(float requested_rate, uint32_t block_steps_event_count)
 155 {
 156     float rate= requested_rate * ((float)steps_to_move / (float)block_steps_event_count);
 157     if(rate < minimum_step_rate) {
 158         rate= minimum_step_rate;
 159     }
 160     set_speed(rate);
 161 }
 162
 163 // Set the speed at which this stepper moves in steps/sec, should be called set_step_rate()
 164 // we need to make sure that we have a minimum speed here and that it fits the 64bit fixed point fx counters
 165 // Note nothing will really ever go as slow as the minimum speed here, it is just forced to avoid bad errors
 166 // fx_ticks_per_step is what actually sets the step rate, it is fixed point 32.32
 167 void StepperMotor::set_speed( float speed )
 168 {
 169     if(speed <= 0.0F) { // we can't actually do 0 but we can get close, need to avoid divide by zero later on
 170         this->fx_ticks_per_step= 0xFFFFF00000000000ULL; // that is 4,294,963,200 10us ticks which is ~11.9 hours for 1 step
 171         this->steps_per_second = THEKERNEL->step_ticker->frequency / (this->fx_ticks_per_step>>fx_shift);
 172         return;
 173     }
 174
 175     // How many steps we must output per second
 176     this->steps_per_second = speed;
 177
 178     // How many ticks ( base steps ) between each actual step at this speed, in fixed point 64
 179     // we need to use double here to match the 64bit resolution of the ticker
 180     double ticks_per_step = (double)THEKERNEL->step_ticker->frequency / speed;
 181     if(ticks_per_step > 0xFFFFF000UL) { // maximum we can really do and allow a few overflow steps
 182         ticks_per_step= 0xFFFFF000UL;
 183         this->steps_per_second = THEKERNEL->step_ticker->frequency / ticks_per_step;
 184     }
 185     double double_fx_ticks_per_step = fx_increment * ticks_per_step;
 186
 187     // set the new speed
 188     this->fx_ticks_per_step = floor(double_fx_ticks_per_step);
 189 }
 190
 191 // Pause this stepper motor
 192 void StepperMotor::pause()
 193 {
 194     this->paused = true;
 195     this->update_exit_tick();
 196 }
 197
 198 // Unpause this stepper motor
 199 void StepperMotor::unpause()
 200 {
 201     this->paused = false;
 202     this->update_exit_tick();
 203 }
 204
 205
 206 void StepperMotor::change_steps_per_mm(float new_steps)
 207 {
 208     steps_per_mm = new_steps;
 209     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
 210     current_position_steps = last_milestone_steps;
 211 }
 212
 213 void StepperMotor::change_last_milestone(float new_milestone)
 214 {
 215     last_milestone_mm = new_milestone;
 216     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
 217     current_position_steps = last_milestone_steps;
 218 }
 219
 220 int  StepperMotor::steps_to_target(float target)
 221 {
 222     int target_steps = lround(target * steps_per_mm);
 223     return target_steps - last_milestone_steps;
 224 }