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