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/>.
8 #include "libs/Kernel.h"
9 #include "StepperMotor.h"
11 StepperMotor::StepperMotor(){
15 this->fx_ticks_per_step
= 0;
16 this->steps_to_move
= 0;
17 this->direction_bit
= 0;
19 this->update_exit_tick();
20 this->dont_remove_from_active_list_yet
= false;
23 StepperMotor::StepperMotor(Pin
* step
, Pin
* dir
, Pin
* en
) : step_pin(step
), dir_pin(dir
), en_pin(en
) {
27 this->fx_ticks_per_step
= 0;
28 this->steps_to_move
= 0;
29 this->direction_bit
= 0;
31 this->update_exit_tick();
32 this->dont_remove_from_active_list_yet
= false;
35 // Called a great many times per second, to step if we have to now
36 bool StepperMotor::tick(){
40 this->dir_pin
->set( this->direction_bit
);
41 this->step_pin
->set( this->step_bit
);
43 // ignore inactive steppers 13t
44 if( this->exit_tick
){
49 // increase the ( fixed point ) counter by one tick 11t
50 this->fx_counter
+= (uint64_t)((uint64_t)1<<32);
52 // if we are to step now 10t
53 if( this->fx_counter
>= this->fx_ticks_per_step
){
55 // move counter back 11t
56 this->fx_counter
-= this->fx_ticks_per_step
;
58 // we must step, actual output is done at the beginning of this function 8t
61 // we have moved a step 9t
64 // is this move finished ? 11t
65 if( this->stepped
== this->steps_to_move
){
71 this->update_exit_tick();
73 // signal it to whatever cares 41t 411t
74 this->end_hook
->call();
86 // This is just a way not to check for ( !this->moving || this->paused || this->fx_ticks_per_step == 0 ) at every tick()
87 inline void StepperMotor::update_exit_tick(){
88 if( !this->moving
|| this->paused
|| this->fx_ticks_per_step
== 0 ){
89 // We must exit tick() after setting the pins, no bresenham is done
90 if( this->exit_tick
== false ){
91 //printf("set for removal %p \r\n", this);
92 this->exit_tick
= true;
93 this->dont_remove_from_active_list_yet
= true;
96 // We must do the bresenham in tick()
97 if( this->exit_tick
== true ){
98 this->exit_tick
= false;
99 //printf("adding motor %p \r\n", this);
100 this->step_ticker
->add_motor_to_active_list(this);
107 // Instruct the StepperMotor to move a certain number of steps
108 void StepperMotor::move( bool direction
, unsigned int steps
){
110 // We do not set the direction directly, we will set the pin just before the step pin on the next tick
111 this->direction_bit
= direction
;
113 // How many steps we have to move until the move is done
114 this->steps_to_move
= steps
;
116 // Zero our tool counters
117 this->fx_counter
= 0; // Bresenheim counter
120 // Starting now we are moving
121 if( steps
> 0 ){ this->moving
= true; }else{ this->moving
= false; }
122 this->update_exit_tick();
126 // Set the speed at which this steper moves
127 void StepperMotor::set_speed( double speed
){
129 if( speed
< 0.0001 ){
130 this->steps_per_second
= 0;
131 this->fx_ticks_per_step
= 1>>63;
135 // How many steps we must output per second
136 this->steps_per_second
= speed
;
138 // How many ticks ( base steps ) between each actual step at this speed, in fixed point 64
139 double ticks_per_step
= (double)( (double)this->step_ticker
->frequency
/ speed
);
140 double double_fx_ticks_per_step
= (double)(1<<16) * ( (double)(1<<16) * ticks_per_step
);
141 this->fx_ticks_per_step
= (uint64_t)( floor(double_fx_ticks_per_step
) );
142 this->update_exit_tick();