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()
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);
44 this->fx_ticks_per_step
= 0xFFFFF000UL
; // some big number so we don't start stepping before it is set
46 this->steps_to_move
= 0;
47 this->is_move_finished
= false;
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 // Thi sis in highest priority interrupt so cannot be pre-empted
63 void StepperMotor::step()
65 // 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
66 if(!this->active
) return;
69 this->step_pin
.set( 1 );
70 THEKERNEL
->step_ticker
->reset_step_pins
= true;
72 // move counter back 11t
73 this->fx_counter
-= this->fx_ticks_per_step
;
75 // we have moved a step 9t
78 // keep track of actuators actual position in steps
79 this->current_position_steps
+= (this->direction
? -1 : 1);
81 // Is this move finished ?
82 if( this->stepped
== this->steps_to_move
) {
83 // Mark it as finished, then StepTicker will call signal_mode_finished()
84 // This is so we don't call that before all the steps have been generated for this tick()
85 this->is_move_finished
= true;
86 THEKERNEL
->step_ticker
->a_move_finished
= true;
91 // If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
92 void StepperMotor::signal_move_finished()
95 this->fx_counter
= 0; // set this to zero here so we don't miss any for next move
96 this->fx_ticks_per_step
= 0xFFFFF000UL
; // some big number so we don't start stepping before it is set again
98 this->steps_to_move
= 0;
99 this->minimum_step_rate
= default_minimum_actuator_rate
;
101 // signal it to whatever cares 41t 411t
102 this->end_hook
->call();
104 // We only need to do this if we were not instructed to move
105 // FIXME trouble here is that if this stops first but other axis are still going we will now stop getting ticks so we cannot know how long we were delayed
106 if( this->moving
== false ) {
107 this->update_exit_tick();
110 this->is_move_finished
= false;
113 // This is just a way not to check for ( !this->moving || this->paused || this->fx_ticks_per_step == 0 ) at every tick()
114 void StepperMotor::update_exit_tick()
116 if( !this->moving
|| this->paused
|| this->steps_to_move
== 0 ) {
117 // No more ticks will be recieved and no more events from StepTicker
118 THEKERNEL
->step_ticker
->remove_motor_from_active_list(this);
121 // we will now get ticks and StepTIcker will send us events
122 THEKERNEL
->step_ticker
->add_motor_to_active_list(this);
127 // Instruct the StepperMotor to move a certain number of steps
128 void StepperMotor::move( bool direction
, unsigned int steps
, float initial_speed
)
130 this->dir_pin
.set(direction
);
131 this->direction
= direction
;
133 // How many steps we have to move until the move is done
134 this->steps_to_move
= steps
;
136 // Zero our tool counters
139 // Starting now we are moving
141 if(initial_speed
>= 0.0F
) set_speed(initial_speed
);
144 this->moving
= false;
146 this->update_exit_tick();
149 // Set the speed at which this stepper moves in steps/sec, should be called set_step_rate()
150 // we need to make sure that we have a minimum speed here and that it fits the 32bit fixed point fx counters
151 // Note nothing will really ever go as slow as the minimum speed here, it is just forced to avoid bad errors
152 // fx_ticks_per_step is what actually sets the step rate, it is fixed point 18.14
153 void StepperMotor::set_speed( float speed
)
155 if(speed
< minimum_step_rate
) {
156 speed
= minimum_step_rate
;
159 // if(speed <= 0.0F) { // we can't actually do 0 but we can get close, need to avoid divide by zero later on
160 // this->fx_ticks_per_step= 0xFFFFFFFFUL; // 0.381 steps/sec
161 // this->steps_per_second = THEKERNEL->step_ticker->get_frequency() / (this->fx_ticks_per_step >> fx_shift);
165 // How many steps we must output per second
166 this->steps_per_second
= speed
;
168 // set the new speed, NOTE this can be pre-empted by stepticker so the following write needs to be atomic
169 this->fx_ticks_per_step
= floor(fx_increment
* THEKERNEL
->step_ticker
->get_frequency() / speed
);
172 // Pause this stepper motor
173 void StepperMotor::pause()
176 this->update_exit_tick();
179 // Unpause this stepper motor
180 void StepperMotor::unpause()
182 this->paused
= false;
183 this->update_exit_tick();
187 void StepperMotor::change_steps_per_mm(float new_steps
)
189 steps_per_mm
= new_steps
;
190 last_milestone_steps
= lround(last_milestone_mm
* steps_per_mm
);
191 current_position_steps
= last_milestone_steps
;
194 void StepperMotor::change_last_milestone(float new_milestone
)
196 last_milestone_mm
= new_milestone
;
197 last_milestone_steps
= lround(last_milestone_mm
* steps_per_mm
);
198 current_position_steps
= last_milestone_steps
;
201 int StepperMotor::steps_to_target(float target
)
203 int target_steps
= lround(target
* steps_per_mm
);
204 return target_steps
- last_milestone_steps
;