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"
16 // in steps/sec the default minimum speed (was 20steps/sec hardcoded)
17 float StepperMotor::default_minimum_actuator_rate
= 20.0F
;
19 // A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed
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()
37 void StepperMotor::init()
39 // register this motor with the step ticker, and get its index in that array and bit position
40 this->index
= THEKERNEL
->step_ticker
->register_motor(this);
43 this->fx_ticks_per_step
= 0xFFFFF000UL
; // some big number so we don't start stepping before it is set
45 this->steps_to_move
= 0;
46 this->is_move_finished
= false;
47 this->last_step_tick_valid
= false;
48 this->last_step_tick
= 0;
49 this->force_finish
= false;
53 minimum_step_rate
= default_minimum_actuator_rate
;
55 last_milestone_steps
= 0;
56 last_milestone_mm
= 0.0F
;
57 current_position_steps
= 0;
62 // 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
63 // we also here check if the move is finished etc ..
64 // This is in highest priority interrupt so cannot be pre-empted
65 void StepperMotor::step()
67 // ignore if we are still processing the end of a block
68 if(this->is_move_finished
) return;
70 if(!this->force_finish
) {
72 this->step_pin
.set( 1 );
74 // move counter back 11t
75 this->fx_counter
-= this->fx_ticks_per_step
;
77 // we have moved a step 9t
80 // keep track of actuators actual position in steps
81 this->current_position_steps
+= (this->direction
? -1 : 1);
83 // we may need to callback on a specific step, usually used to synchronize deceleration timer
84 if(this->signal_step
!= 0 && this->stepped
== this->signal_step
) {
85 THEKERNEL
->step_ticker
->synchronize_acceleration(true);
90 // Is this move finished ?
91 if( this->force_finish
|| this->stepped
== this->steps_to_move
) {
92 // Mark it as finished, then StepTicker will call signal_move_finished()
93 // This is so we don't call that before all the steps have been generated for this tick()
94 this->is_move_finished
= true;
95 THEKERNEL
->step_ticker
->a_move_finished
= true;
96 this->last_step_tick
= THEKERNEL
->step_ticker
->get_tick_cnt(); // remember when last step was
97 if(this->force_finish
) this->steps_to_move
= stepped
;
101 // Does a manual step pulse, used for direct encoder control of a stepper
102 void StepperMotor::manual_step(bool dir
)
104 if(!enabled
) enable(true);
106 // set direction if needed
107 if(this->direction
!= dir
) {
108 this->direction
= dir
;
109 this->dir_pin
.set(dir
);
114 this->step_pin
.set(1);
116 // schedule for unstep
117 THEKERNEL
->step_ticker
->schedule_unstep(this->index
);
119 // keep track of actuators actual position in steps
120 this->current_position_steps
+= (dir
? -1 : 1);
123 // If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
124 void StepperMotor::signal_move_finished()
127 this->moving
= false;
128 this->steps_to_move
= 0;
129 this->minimum_step_rate
= default_minimum_actuator_rate
;
131 // signal it to whatever cares
132 // in this call a new block may start, new moves set and new speeds
133 this->end_hook
->call();
135 // We only need to do this if we were not instructed to move
136 if( !this->moving
) {
137 this->update_exit_tick();
140 this->is_move_finished
= false;
143 // This is just a way not to check for ( !this->moving || this->fx_ticks_per_step == 0 ) at every tick()
144 void StepperMotor::update_exit_tick()
146 if( !this->moving
|| this->steps_to_move
== 0 ) {
147 // No more ticks will be received and no more events from StepTicker
148 THEKERNEL
->step_ticker
->remove_motor_from_active_list(this);
150 // we will now get ticks and StepTIcker will send us events
151 THEKERNEL
->step_ticker
->add_motor_to_active_list(this);
155 // Instruct the StepperMotor to move a certain number of steps
156 StepperMotor
* StepperMotor::move( bool direction
, unsigned int steps
, float initial_speed
)
158 this->dir_pin
.set(direction
);
159 this->direction
= direction
;
160 this->force_finish
= false;
162 // How many steps we have to move until the move is done
163 this->steps_to_move
= steps
;
165 // Zero our tool counters
167 this->fx_ticks_per_step
= 0xFFFFF000UL
; // some big number so we don't start stepping before it is set again
168 if(this->last_step_tick_valid
) {
169 // we set this based on when the last step was, thus compensating for missed ticks
170 uint32_t ts
= THEKERNEL
->step_ticker
->ticks_since(this->last_step_tick
);
171 // if an axis stops too soon then we can get a huge number of ticks here which causes problems, so if the number of ticks is too great we ignore them
172 // example of when this happens is when one axis is going very slow and the min 20steps/sec kicks in, the axis will reach its target much sooner leaving a long gap
173 // until the end of the block.
174 // TODO we may need to set this based on the current step rate, trouble is we don't know what that is yet, we could use the last fx_ticks_per_step as a guide
175 if(ts
> 5) ts
= 5; // limit to 50us catch up around 1-2 steps
176 else if(ts
> 15) ts
= 0; // no way to know what the delay was
177 this->fx_counter
= ts
*fx_increment
;
179 this->fx_counter
= 0; // set to zero as there was no step last block
182 // Starting now we are moving
184 if(initial_speed
>= 0.0F
) set_speed(initial_speed
);
187 this->moving
= false;
189 this->update_exit_tick();
193 // Set the speed at which this stepper moves in steps/sec, should be called set_step_rate()
194 // we need to make sure that we have a minimum speed here and that it fits the 32bit fixed point fx counters
195 // Note nothing will really ever go as slow as the minimum speed here, it is just forced to avoid bad errors
196 // fx_ticks_per_step is what actually sets the step rate, it is fixed point 18.14
197 StepperMotor
* StepperMotor::set_speed( float speed
)
199 if(speed
< minimum_step_rate
) {
200 speed
= minimum_step_rate
;
203 // if(speed <= 0.0F) { // we can't actually do 0 but we can get close, need to avoid divide by zero later on
204 // this->fx_ticks_per_step= 0xFFFFFFFFUL; // 0.381 steps/sec
205 // this->steps_per_second = THEKERNEL->step_ticker->get_frequency() / (this->fx_ticks_per_step >> fx_shift);
209 // How many steps we must output per second
210 this->steps_per_second
= speed
;
212 // set the new speed, NOTE this can be pre-empted by stepticker so the following write needs to be atomic
213 this->fx_ticks_per_step
= floor(fx_increment
* THEKERNEL
->step_ticker
->get_frequency() / speed
);
217 void StepperMotor::change_steps_per_mm(float new_steps
)
219 steps_per_mm
= new_steps
;
220 last_milestone_steps
= lroundf(last_milestone_mm
* steps_per_mm
);
221 current_position_steps
= last_milestone_steps
;
224 void StepperMotor::change_last_milestone(float new_milestone
)
226 last_milestone_mm
= new_milestone
;
227 last_milestone_steps
= lroundf(last_milestone_mm
* steps_per_mm
);
228 current_position_steps
= last_milestone_steps
;
231 int StepperMotor::steps_to_target(float target
)
233 int target_steps
= lroundf(target
* steps_per_mm
);
234 return target_steps
- last_milestone_steps
;