65b02486529808cd70daeecbe34684f0a660d885
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 unstep(); // initialize step pin
32 set_direction(false); // initialize dor pin
34 this->register_for_event(ON_HALT
);
35 this->register_for_event(ON_ENABLE
);
38 StepperMotor::~StepperMotor()
40 THEKERNEL
->unregister_for_event(ON_HALT
, this);
41 THEKERNEL
->unregister_for_event(ON_ENABLE
, this);
44 void StepperMotor::on_halt(void *argument
)
46 if(argument
== nullptr) {
52 void StepperMotor::on_enable(void *argument
)
54 // argument is a uin32_t where bit0 is on or off, and bit 1:X, 2:Y, 3:Z, 4:A, 5:B, 6:C etc
55 // for now if bit0 is 1 we turn all on, if 0 we turn all off otherwise we turn selected axis off
56 uint32_t bm
= (uint32_t)argument
;
60 }else if(bm
== 0 || ((bm
&0x01) == 0 && ((bm
&(0x02<<motor_id
)) != 0)) ) {
65 void StepperMotor::change_steps_per_mm(float new_steps
)
67 steps_per_mm
= new_steps
;
68 last_milestone_steps
= lroundf(last_milestone_mm
* steps_per_mm
);
69 current_position_steps
= last_milestone_steps
;
72 void StepperMotor::change_last_milestone(float new_milestone
)
74 last_milestone_mm
= new_milestone
;
75 last_milestone_steps
= lroundf(last_milestone_mm
* steps_per_mm
);
76 current_position_steps
= last_milestone_steps
;
79 void StepperMotor::set_last_milestones(float mm
, int32_t steps
)
81 last_milestone_mm
= mm
;
82 last_milestone_steps
= steps
;
83 current_position_steps
= last_milestone_steps
;
86 void StepperMotor::update_last_milestones(float mm
, int32_t steps
)
88 last_milestone_steps
+= steps
;
89 last_milestone_mm
= mm
;
92 int32_t StepperMotor::steps_to_target(float target
)
94 int32_t target_steps
= lroundf(target
* steps_per_mm
);
95 return target_steps
- last_milestone_steps
;
98 // Does a manual step pulse, used for direct encoder control of a stepper
99 // NOTE this is experimental and may change and/or be reomved in the future, it is an unsupported feature.
100 // use at your own risk
101 void StepperMotor::manual_step(bool dir
)
103 if(!is_enabled()) enable(true);
105 // set direction if needed
106 if(this->direction
!= dir
) {
107 this->direction
= dir
;
108 this->dir_pin
.set(dir
);
113 this->step_pin
.set(1);
115 this->step_pin
.set(0);
118 // keep track of actuators actual position in steps
119 this->current_position_steps
+= (dir
? -1 : 1);