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 // A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed
  16 // TODO : Abstract this into Actuator
  17
  18 StepperMotor::StepperMotor()
  19 {
  20     init();
  21 }
  22
  23 StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_pin(dir), en_pin(en)
  24 {
  25     init();
  26     enable(false);
  27     set_high_on_debug(en.port_number, en.pin);
  28 }
  29
  30 void StepperMotor::init()
  31 {
  32     this->moving = false;
  33     this->paused = false;
  34     this->fx_counter = 0;
  35     this->stepped = 0;
  36     this->fx_ticks_per_step = 0;
  37     this->steps_to_move = 0;
  38     this->remove_from_active_list_next_reset = false;
  39     this->is_move_finished = false;
  40     this->signal_step = false;
  41     this->step_signal_hook = new Hook();
  42
  43     steps_per_mm         = 1.0F;
  44     max_rate             = 50.0F;
  45
  46     current_position_steps= 0;
  47     last_milestone_steps = 0;
  48     last_milestone_mm    = 0.0F;
  49 }
  50
  51
  52 // 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
  53 // we also here check if the move is finished etc ...
  54 void StepperMotor::step()
  55 {
  56     // output to pins 37t
  57     this->step_pin.set( 1 );
  58     this->step_ticker->reset_step_pins = true;
  59
  60     // move counter back 11t
  61     this->fx_counter -= this->fx_ticks_per_step;
  62
  63     // we have moved a step 9t
  64     this->stepped++;
  65
  66     // Do we need to signal this step
  67     if( this->stepped == this->signal_step_number && this->signal_step ) {
  68         this->step_signal_hook->call();
  69     }
  70
  71     // keep track of actuators actual position in steps
  72     this->current_position_steps += (this->direction ? -1 : 1);
  73
  74     // Is this move finished ?
  75     if( this->stepped == this->steps_to_move ) {
  76         // Mark it as finished, then StepTicker will call signal_mode_finished()
  77         // This is so we don't call that before all the steps have been generated for this tick()
  78         this->is_move_finished = true;
  79         this->step_ticker->moves_finished = true;
  80     }
  81 }
  82
  83
  84 // If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
  85 void StepperMotor::signal_move_finished()
  86 {
  87     // work is done ! 8t
  88     this->moving = false;
  89     this->steps_to_move = 0;
  90
  91     // signal it to whatever cares 41t 411t
  92     this->end_hook->call();
  93
  94     // We only need to do this if we were not instructed to move
  95     // NOTE this is always true
  96     if( this->moving == false ) {
  97         this->update_exit_tick();
  98     }
  99
 100     this->is_move_finished = false;
 101 }
 102
 103 // This is just a way not to check for ( !this->moving || this->paused || this->fx_ticks_per_step == 0 ) at every tick()
 104 inline void StepperMotor::update_exit_tick()
 105 {
 106     if( !this->moving || this->paused || this->steps_to_move == 0 ) {
 107         // We must exit tick() after setting the pins, no bresenham is done
 108         //this->remove_from_active_list_next_reset = true;
 109         this->step_ticker->remove_motor_from_active_list(this);
 110     } else {
 111         // We must do the bresenham in tick()
 112         // We have to do this or there could be a bug where the removal still happens when it doesn't need to
 113         this->step_ticker->add_motor_to_active_list(this);
 114     }
 115 }
 116
 117
 118
 119 // Instruct the StepperMotor to move a certain number of steps
 120 void StepperMotor::move( bool direction, unsigned int steps )
 121 {
 122     // We do not set the direction directly, we will set the pin just before the step pin on the next tick
 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->fx_counter = 0;      // Bresenheim counter
 131     this->stepped = 0;
 132
 133     // Do not signal steps until we get instructed to
 134     this->signal_step = false;
 135
 136     // Starting now we are moving
 137     if( steps > 0 ) {
 138         this->moving = true;
 139     } else {
 140         this->moving = false;
 141     }
 142     this->update_exit_tick();
 143 }
 144
 145 // Set the speed at which this steper moves
 146 void StepperMotor::set_speed( float speed )
 147 {
 148     // How many steps we must output per second
 149     this->steps_per_second = speed;
 150
 151     // How many ticks ( base steps ) between each actual step at this speed, in fixed point 64 <--- REALLY? I don't think it is at the moment looks like 32bit fixed point
 152     float ticks_per_step = (float)( (float)this->step_ticker->frequency / speed );
 153
 154     float double_fx_ticks_per_step = 65536.0F * ticks_per_step; // isn't this better on a 32bit machine?
 155     if(double_fx_ticks_per_step >= 65536.0F*65536.0F) { // too slow exceeds the 32 bit uint
 156         // minimum_speed = ceil(step_ticker->frequency/65536.0F), which would be 2 at 100Khz (NOTE was set to 20)
 157         this->steps_per_second = ceil(this->step_ticker->frequency/65536.0F); // this is the maximum it can be
 158         ticks_per_step = (float)( (float)this->step_ticker->frequency / this->steps_per_second);
 159         double_fx_ticks_per_step = 65536.0F * ticks_per_step;
 160     }
 161
 162     this->fx_ticks_per_step = (uint32_t)( floor(double_fx_ticks_per_step) );
 163 }
 164
 165 // Pause this stepper motor
 166 void StepperMotor::pause()
 167 {
 168     this->paused = true;
 169     this->update_exit_tick();
 170 }
 171
 172 // Unpause this stepper motor
 173 void StepperMotor::unpause()
 174 {
 175     this->paused = false;
 176     this->update_exit_tick();
 177 }
 178
 179
 180 void StepperMotor::change_steps_per_mm(float new_steps)
 181 {
 182     steps_per_mm = new_steps;
 183     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
 184     current_position_steps = last_milestone_steps;
 185 }
 186
 187 void StepperMotor::change_last_milestone(float new_milestone)
 188 {
 189     last_milestone_mm = new_milestone;
 190     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
 191     current_position_steps = last_milestone_steps;
 192 }
 193
 194 int  StepperMotor::steps_to_target(float target)
 195 {
 196     int target_steps = lround(target * steps_per_mm);
 197     return target_steps - last_milestone_steps;
 198 }