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