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