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/>.
7 #include "StepperMotor.h"
10 #include "MRI_Hooks.h"
11 #include "StepTicker.h"
15 // in steps/sec the default minimum speed (was 20steps/sec hardcoded)
16 float StepperMotor::default_minimum_actuator_rate
= 20.0F
;
18 // A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed
19 // TODO : Abstract this into Actuator
21 StepperMotor::StepperMotor()
26 StepperMotor::StepperMotor(Pin
&step
, Pin
&dir
, Pin
&en
) : step_pin(step
), dir_pin(dir
), en_pin(en
)
30 set_high_on_debug(en
.port_number
, en
.pin
);
33 StepperMotor::~StepperMotor()
35 delete step_signal_hook
;
38 void StepperMotor::init()
44 this->fx_ticks_per_step
= 0;
45 this->steps_to_move
= 0;
46 this->is_move_finished
= false;
47 this->signal_step
= false;
48 this->step_signal_hook
= new Hook();
52 minimum_step_rate
= default_minimum_actuator_rate
;
54 last_milestone_steps
= 0;
55 last_milestone_mm
= 0.0F
;
56 current_position_steps
= 0;
60 // This is called ( see the .h file, we had to put a part of things there for obscure inline reasons ) when a step has to be generated
61 // we also here check if the move is finished etc ...
62 void StepperMotor::step()
64 if(this->is_move_finished
) return; // we can't do anything until the next move has been processed, but we will be able to offset the time by shortening the next step
67 this->step_pin
.set( 1 );
68 THEKERNEL
->step_ticker
->reset_step_pins
= true;
70 // move counter back 11t
71 if(this->fx_counter
> this->fx_ticks_per_step
) {
72 this->fx_counter
-= this->fx_ticks_per_step
;
74 this->fx_counter
= 0; // can't make it less than 0 as it is a uint, unless we get interrupted I don't think this case is possible
77 // we have moved a step 9t
80 // Do we need to signal this step
81 if( this->stepped
== this->signal_step_number
&& this->signal_step
) {
82 this->step_signal_hook
->call();
85 // keep track of actuators actual position in steps
86 this->current_position_steps
+= (this->direction
? -1 : 1);
88 // Is this move finished ?
89 if( this->stepped
== this->steps_to_move
) {
90 // Mark it as finished, then StepTicker will call signal_mode_finished()
91 // This is so we don't call that before all the steps have been generated for this tick()
92 this->is_move_finished
= true;
93 THEKERNEL
->step_ticker
->a_move_finished
= true;
94 this->fx_counter
= 0; // set this to zero here so we don't miss any for next move
99 // If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
100 void StepperMotor::signal_move_finished()
103 this->moving
= false;
104 this->steps_to_move
= 0;
105 this->minimum_step_rate
= default_minimum_actuator_rate
;
107 // signal it to whatever cares 41t 411t
108 this->end_hook
->call();
110 // We only need to do this if we were not instructed to move
111 if( this->moving
== false ) {
112 this->update_exit_tick();
115 this->is_move_finished
= false;
118 // This is just a way not to check for ( !this->moving || this->paused || this->fx_ticks_per_step == 0 ) at every tick()
119 inline void StepperMotor::update_exit_tick()
121 if( !this->moving
|| this->paused
|| this->steps_to_move
== 0 ) {
122 // We must exit tick() after setting the pins, no bresenham is done
123 THEKERNEL
->step_ticker
->remove_motor_from_active_list(this);
125 // We must do the bresenham in tick()
126 // We have to do this or there could be a bug where the removal still happens when it doesn't need to
127 THEKERNEL
->step_ticker
->add_motor_to_active_list(this);
131 // Instruct the StepperMotor to move a certain number of steps
132 void StepperMotor::move( bool direction
, unsigned int steps
, float initial_speed
)
134 this->dir_pin
.set(direction
);
135 this->direction
= direction
;
137 // How many steps we have to move until the move is done
138 this->steps_to_move
= steps
;
140 // Zero our tool counters
143 // Do not signal steps until we get instructed to
144 this->signal_step
= false;
146 // Starting now we are moving
148 if(initial_speed
>= 0.0F
) set_speed(initial_speed
);
151 this->moving
= false;
153 this->update_exit_tick();
156 // this is called to set the step rate based on this blocks rate, we use this instead of set_speed for coordinated moves
157 // so that we can floor to a minimum speed which is proportional for all axis. the minimum step rate is set at the start
158 // of each block based on the slowest axis of all coordinated axis.
159 // the rate passed in is the requested rate, it is scaled for this motor based on steps_to_move and block_steps_event_count
160 void StepperMotor::set_step_rate(float requested_rate
, uint32_t block_steps_event_count
)
162 float rate
= requested_rate
* ((float)steps_to_move
/ (float)block_steps_event_count
);
163 if(rate
< minimum_step_rate
) {
164 rate
= minimum_step_rate
;
169 // Set the speed at which this stepper moves in steps/sec, should be called set_step_rate()
170 // we need to make sure that we have a minimum speed here and that it fits the 64bit fixed point fx counters
171 // Note nothing will really ever go as slow as the minimum speed here, it is just forced to avoid bad errors
172 // fx_ticks_per_step is what actually sets the step rate, it is fixed point 32.32
173 void StepperMotor::set_speed( float speed
)
175 if(speed
<= 0.0F
) { // we can't actually do 0 but we can get close, need to avoid divide by zero later on
176 this->fx_ticks_per_step
= 0xFFFFF00000000000ULL
; // that is 4,294,963,200 10us ticks which is ~11.9 hours for 1 step
177 this->steps_per_second
= THEKERNEL
->step_ticker
->frequency
/ (this->fx_ticks_per_step
>>fx_shift
);
181 // How many steps we must output per second
182 this->steps_per_second
= speed
;
184 // How many ticks ( base steps ) between each actual step at this speed, in fixed point 64
185 // we need to use double here to match the 64bit resolution of the ticker
186 double ticks_per_step
= (double)THEKERNEL
->step_ticker
->frequency
/ speed
;
187 if(ticks_per_step
> 0xFFFFF000UL
) { // maximum we can really do and allow a few overflow steps
188 ticks_per_step
= 0xFFFFF000UL
;
189 this->steps_per_second
= THEKERNEL
->step_ticker
->frequency
/ ticks_per_step
;
191 double double_fx_ticks_per_step
= fx_increment
* ticks_per_step
;
194 this->fx_ticks_per_step
= floor(double_fx_ticks_per_step
);
197 // Pause this stepper motor
198 void StepperMotor::pause()
201 this->update_exit_tick();
204 // Unpause this stepper motor
205 void StepperMotor::unpause()
207 this->paused
= false;
208 this->update_exit_tick();
212 void StepperMotor::change_steps_per_mm(float new_steps
)
214 steps_per_mm
= new_steps
;
215 last_milestone_steps
= lround(last_milestone_mm
* steps_per_mm
);
216 current_position_steps
= last_milestone_steps
;
219 void StepperMotor::change_last_milestone(float new_milestone
)
221 last_milestone_mm
= new_milestone
;
222 last_milestone_steps
= lround(last_milestone_mm
* steps_per_mm
);
223 current_position_steps
= last_milestone_steps
;
226 int StepperMotor::steps_to_target(float target
)
228 int target_steps
= lround(target
* steps_per_mm
);
229 return target_steps
- last_milestone_steps
;