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
);
19 // register this motor with the step ticker, and get its index in that array and bit position
20 this->index
= THEKERNEL
->step_ticker
->register_motor(this);
25 last_milestone_steps
= 0;
26 last_milestone_mm
= 0.0F
;
27 current_position_steps
= 0;
31 this->register_for_event(ON_HALT
);
32 this->register_for_event(ON_ENABLE
);
35 StepperMotor::~StepperMotor()
37 THEKERNEL
->unregister_for_event(ON_HALT
, this);
38 THEKERNEL
->unregister_for_event(ON_ENABLE
, this);
41 void StepperMotor::on_halt(void *argument
)
43 if(argument
== nullptr) {
49 void StepperMotor::on_enable(void *argument
)
51 enable(argument
!= nullptr);
54 void StepperMotor::change_steps_per_mm(float new_steps
)
56 steps_per_mm
= new_steps
;
57 last_milestone_steps
= lroundf(last_milestone_mm
* steps_per_mm
);
58 current_position_steps
= last_milestone_steps
;
61 void StepperMotor::change_last_milestone(float new_milestone
)
63 last_milestone_mm
= new_milestone
;
64 last_milestone_steps
= lroundf(last_milestone_mm
* steps_per_mm
);
65 current_position_steps
= last_milestone_steps
;
68 int StepperMotor::steps_to_target(float target
)
70 int target_steps
= lroundf(target
* steps_per_mm
);
71 return target_steps
- last_milestone_steps
;
74 // Does a manual step pulse, used for direct encoder control of a stepper
75 void StepperMotor::manual_step(bool dir
)
77 if(!is_enabled()) enable(true);
79 // set direction if needed
80 if(this->direction
!= dir
) {
82 this->dir_pin
.set(dir
);
87 this->step_pin
.set(1);
89 this->step_pin
.set(0);
92 // keep track of actuators actual position in steps
93 this->current_position_steps
+= (dir
? -1 : 1);