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->stepped = 0;
  16     this->fx_ticks_per_step = 0;
  17     this->steps_to_move = 0;
  18     this->direction_bit = 0;
  19     //this->update_exit_tick();
  20     this->remove_from_active_list_next_reset = false;
  21     this->is_move_finished = false;
  22 }
  23
  24 StepperMotor::StepperMotor(Pin* step, Pin* dir, Pin* en) : step_pin(step), dir_pin(dir), en_pin(en) {
  25     this->moving = false;
  26     this->paused = false;
  27     this->fx_counter = 0;
  28     this->stepped = 0;
  29     this->fx_ticks_per_step = 0;
  30     this->steps_to_move = 0;
  31     this->direction_bit = 0;
  32     //this->update_exit_tick();
  33     this->remove_from_active_list_next_reset = false;
  34     this->is_move_finished = false;
  35 }
  36
  37 // Called a great many times per second, to step if we have to now
  38 bool StepperMotor::tick(){
  39
  40     // increase the ( fixed point ) counter by one tick 11t
  41     this->fx_counter += (uint64_t)((uint64_t)1<<32);
  42
  43     // if we are to step now 10t
  44     if( this->fx_counter >= this->fx_ticks_per_step ){
  45
  46         // output to pins 37t
  47         this->dir_pin->set(  this->direction_bit );
  48         this->step_pin->set( 1                   );
  49         this->step_ticker->reset_step_pins = true;
  50
  51         // move counter back 11t
  52         this->fx_counter -= this->fx_ticks_per_step;
  53
  54         // we have moved a step 9t
  55         this->stepped++;
  56
  57         // is this move finished ? 11t
  58         if( this->stepped == this->steps_to_move ){
  59             this->is_move_finished = true;
  60             this->step_ticker->moves_finished = true;
  61         }
  62
  63     }
  64
  65 }
  66
  67 void StepperMotor::signal_move_finished(){
  68
  69             // work is done ! 8t
  70             this->moving = false;
  71             this->steps_to_move = 0;
  72
  73             // signal it to whatever cares 41t 411t
  74             this->end_hook->call();
  75
  76             // We only need to do this if we were not instructed to move
  77             if( this->moving == false ){
  78                 this->update_exit_tick();
  79             }
  80
  81             this->is_move_finished = false;
  82
  83 }
  84
  85 // This is just a way not to check for ( !this->moving || this->paused || this->fx_ticks_per_step == 0 ) at every tick()
  86 inline void StepperMotor::update_exit_tick(){
  87     //printf("try update list %u %u %u\r\n", this->moving, this->paused, this->fx_ticks_per_step == 0 );
  88     if( !this->moving || this->paused || this->steps_to_move == 0 ){
  89         // We must exit tick() after setting the pins, no bresenham is done
  90         //this->remove_from_active_list_next_reset = true;
  91         this->step_ticker->remove_motor_from_active_list(this);
  92     }else{
  93         // We must do the bresenham in tick()
  94         // We have to do this or there could be a bug where the removal still happens when it doesn't need to
  95         //this->remove_from_active_list_next_reset = false;
  96         this->step_ticker->add_motor_to_active_list(this);
  97     }
  98 }
  99
 100
 101
 102 // Instruct the StepperMotor to move a certain number of steps
 103 void StepperMotor::move( bool direction, unsigned int steps ){
 104
 105    //printf("stepper %p moving %u \r\n", this, steps );
 106     // We do not set the direction directly, we will set the pin just before the step pin on the next tick
 107     this->direction_bit = direction;
 108
 109     // How many steps we have to move until the move is done
 110     this->steps_to_move = steps;
 111
 112     // Zero our tool counters
 113     this->fx_counter = 0;      // Bresenheim counter
 114     this->stepped = 0;
 115
 116     // Starting now we are moving
 117     if( steps > 0 ){
 118         this->moving = true;
 119     }else{
 120         this->moving = false;
 121     }
 122     this->update_exit_tick();
 123
 124 }
 125
 126 // Set the speed at which this steper moves
 127 void StepperMotor::set_speed( double speed ){
 128     //printf(" steppermotor %p set speed : %f \r\n", this, speed);
 129     if( speed < 0.0001 ){
 130         this->steps_per_second = 0;
 131         this->fx_ticks_per_step = 1>>63;
 132         return;
 133     }
 134
 135     //if( speed < this->steps_per_second ){
 136         LPC_GPIO1->FIOSET = 1<<19;
 137     //}
 138
 139     // How many steps we must output per second
 140     this->steps_per_second = speed;
 141
 142     // How many ticks ( base steps ) between each actual step at this speed, in fixed point 64
 143     double ticks_per_step = (double)( (double)this->step_ticker->frequency / speed );
 144     double double_fx_ticks_per_step = (double)(1<<16) * ( (double)(1<<16) * ticks_per_step );
 145     this->fx_ticks_per_step = (uint64_t)( floor(double_fx_ticks_per_step) );
 146
 147     LPC_GPIO1->FIOCLR = 1<<19;
 148
 149 }
 150
 151 void StepperMotor::pause(){
 152     this->paused = true;
 153     this->update_exit_tick();
 154 }
 155
 156 void StepperMotor::unpause(){
 157     this->paused = false;
 158     this->update_exit_tick();
 159 }
 160
 161