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 "mri.h"
   8 #include "libs/Kernel.h"
   9 #include "StepperMotor.h"
  10
  11 StepperMotor::StepperMotor(){
  12     this->moving = false;
  13     this->paused = false;
  14     this->fx_counter = 0;
  15     this->fx_ticks_per_step = 0;
  16     this->steps_to_move = 0;
  17     this->direction_bit = 0;
  18     this->step_bit = 0;
  19 }
  20
  21 StepperMotor::StepperMotor(Pin* step, Pin* dir, Pin* en) : step_pin(step), dir_pin(dir), en_pin(en) {
  22     this->moving = false;
  23     this->paused = false;
  24     this->fx_counter = 0;
  25     this->fx_ticks_per_step = 0;
  26     this->steps_to_move = 0;
  27     this->direction_bit = 0;
  28     this->step_bit = 0;
  29 }
  30
  31 // Called a great many times per second, to step if we have to now
  32 bool StepperMotor::tick(){
  33
  34     // output to pins 37t
  35     this->dir_pin->set(  this->direction_bit );
  36     this->step_pin->set( this->step_bit      );
  37
  38     // ignore inactive steppers 13t
  39     if( !this->moving || this->paused || this->fx_ticks_per_step == 0 ){ this->step_bit = false; return false; }
  40
  41     // increase the ( fixed point ) counter by one tick 11t
  42     this->fx_counter += (uint64_t)((uint64_t)1<<32);
  43
  44     // if we are to step now 10t
  45     if( this->fx_counter >= this->fx_ticks_per_step ){
  46
  47         // move counter back 11t
  48         this->fx_counter -= this->fx_ticks_per_step;
  49
  50         // we must step, actual output is done at the beginning of this function 8t
  51         this->step_bit = 1;
  52
  53         // we have moved a step 9t
  54         this->stepped++;
  55
  56         // is this move finished ? 11t
  57         if( this->moving && this->stepped == this->steps_to_move ){
  58
  59             // work is done ! 8t
  60             this->moving = false;
  61
  62             // signal it to whatever cares 41t 411t
  63             this->end_hook->call();
  64
  65         }
  66
  67     }else{
  68
  69         // we must not step
  70         this->step_bit = 0;
  71     }
  72
  73 }
  74
  75
  76 // Instruct the StepperMotor to move a certain number of steps
  77 void StepperMotor::move( bool direction, unsigned int steps ){
  78
  79     //printf("stepper move %p moving %u steps\r\n", this, steps);
  80
  81     // We do not set the direction directly, we will set the pin just before the step pin on the next tick
  82     this->direction_bit = direction;
  83
  84     // How many steps we have to move until the move is done
  85     this->steps_to_move = steps;
  86
  87     // Zero our tool counters
  88     this->fx_counter = 0;      // Bresenheim counter
  89     this->stepped = 0;
  90
  91     // Starting now we are moving
  92     if( steps > 0 ){ this->moving = true; }else{ this->moving = false; }
  93
  94 }
  95
  96 // Set the speed at which this steper moves
  97 void StepperMotor::set_speed( double speed ){
  98
  99     if( speed < 0.0001 ){
 100         this->steps_per_second = 0;
 101         this->fx_ticks_per_step = 1>>63;
 102         return;
 103     }
 104
 105     // How many steps we must output per second
 106     this->steps_per_second = speed;
 107
 108     // How many ticks ( base steps ) between each actual step at this speed, in fixed point 64
 109     double ticks_per_step = (double)( (double)this->step_ticker->frequency / speed );
 110     double double_fx_ticks_per_step = (double)(1<<16) * ( (double)(1<<16) * ticks_per_step );
 111     this->fx_ticks_per_step = (uint64_t)( floor(double_fx_ticks_per_step) );
 112
 113     // printf("speed: %f frequency: %f ticks_per_step: %f double_fx_ticks_per_step: %f floor: %f fx_ticks_per_step: %f \r\n", speed,  (double)this->step_ticker->frequency, ticks_per_step, double_fx_ticks_per_step, floor(double_fx_ticks_per_step), (double)this->fx_ticks_per_step );
 114
 115
 116 }
 117
 118
 119
 120