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 StepperMotor::StepperMotor(Pin
&step
, Pin
&dir
, Pin
&en
) : step_pin(step
), dir_pin(dir
), en_pin(en
)
18 set_high_on_debug(en
.port_number
, en
.pin
);
23 last_milestone_steps
= 0;
24 last_milestone_mm
= 0.0F
;
25 current_position_steps
= 0;
31 this->set_direction(false);
33 this->register_for_event(ON_HALT
);
34 this->register_for_event(ON_ENABLE
);
37 StepperMotor::~StepperMotor()
39 THEKERNEL
->unregister_for_event(ON_HALT
, this);
40 THEKERNEL
->unregister_for_event(ON_ENABLE
, this);
43 void StepperMotor::on_halt(void *argument
)
45 if(argument
== nullptr) {
51 void StepperMotor::on_enable(void *argument
)
53 enable(argument
!= nullptr);
56 void StepperMotor::change_steps_per_mm(float new_steps
)
58 steps_per_mm
= new_steps
;
59 last_milestone_steps
= lroundf(last_milestone_mm
* steps_per_mm
);
60 current_position_steps
= last_milestone_steps
;
63 void StepperMotor::change_last_milestone(float new_milestone
)
65 last_milestone_mm
= new_milestone
;
66 last_milestone_steps
= lroundf(last_milestone_mm
* steps_per_mm
);
67 current_position_steps
= last_milestone_steps
;
70 void StepperMotor::set_last_milestones(float mm
, int32_t steps
)
72 last_milestone_mm
= mm
;
73 last_milestone_steps
= steps
;
74 current_position_steps
= last_milestone_steps
;
77 void StepperMotor::update_last_milestones(float mm
, int32_t steps
)
79 last_milestone_steps
+= steps
;
80 last_milestone_mm
= mm
;
83 int32_t StepperMotor::steps_to_target(float target
)
85 int32_t target_steps
= lroundf(target
* steps_per_mm
);
86 return target_steps
- last_milestone_steps
;
89 // Does a manual step pulse, used for direct encoder control of a stepper
90 void StepperMotor::manual_step(bool dir
)
92 if(!is_enabled()) enable(true);
94 // set direction if needed
95 if(this->direction
!= dir
) {
97 this->dir_pin
.set(dir
);
102 this->step_pin
.set(1);
104 this->step_pin
.set(0);
107 // keep track of actuators actual position in steps
108 this->current_position_steps
+= (dir
? -1 : 1);