Commit | Line | Data |
---|---|---|
7b49793d | 1 | /* |
feb204be AW |
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. | |
7b49793d | 5 | You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>. |
feb204be | 6 | */ |
670fa10b | 7 | #include "StepperMotor.h" |
5673fe39 MM |
8 | |
9 | #include "Kernel.h" | |
8f91e4e6 | 10 | #include "MRI_Hooks.h" |
61134a65 JM |
11 | #include "StepTicker.h" |
12 | ||
13 | #include <math.h> | |
f9a0f86d | 14 | #include "mbed.h" |
feb204be | 15 | |
728477c4 JM |
16 | StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_pin(dir), en_pin(en) |
17 | { | |
728477c4 | 18 | set_high_on_debug(en.port_number, en.pin); |
96f12364 | 19 | // register this motor with the step ticker, and get its index in that array and bit position |
1fce036c | 20 | this->index= THEKERNEL->step_ticker->register_motor(this); |
8f91e4e6 | 21 | |
df6a30f2 MM |
22 | steps_per_mm = 1.0F; |
23 | max_rate = 50.0F; | |
24 | ||
78d0e16a MM |
25 | last_milestone_steps = 0; |
26 | last_milestone_mm = 0.0F; | |
3494f3d0 | 27 | current_position_steps= 0; |
9e6014a6 | 28 | enable(false); |
d1d120e1 | 29 | moving= false; |
9e6014a6 JM |
30 | |
31 | this->register_for_event(ON_HALT); | |
32 | this->register_for_event(ON_ENABLE); | |
33 | } | |
34 | ||
35 | StepperMotor::~StepperMotor() | |
36 | { | |
37 | THEKERNEL->unregister_for_event(ON_HALT, this); | |
38 | THEKERNEL->unregister_for_event(ON_ENABLE, this); | |
39 | } | |
40 | ||
41 | void StepperMotor::on_halt(void *argument) | |
42 | { | |
43 | if(argument == nullptr) { | |
44 | enable(false); | |
45 | } | |
46 | } | |
47 | ||
48 | void StepperMotor::on_enable(void *argument) | |
49 | { | |
50 | enable(argument != nullptr); | |
feb204be AW |
51 | } |
52 | ||
78d0e16a MM |
53 | void StepperMotor::change_steps_per_mm(float new_steps) |
54 | { | |
55 | steps_per_mm = new_steps; | |
586cc733 | 56 | last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm); |
58c32991 | 57 | current_position_steps = last_milestone_steps; |
78d0e16a MM |
58 | } |
59 | ||
60 | void StepperMotor::change_last_milestone(float new_milestone) | |
61 | { | |
62 | last_milestone_mm = new_milestone; | |
586cc733 | 63 | last_milestone_steps = lroundf(last_milestone_mm * steps_per_mm); |
58c32991 | 64 | current_position_steps = last_milestone_steps; |
78d0e16a MM |
65 | } |
66 | ||
67 | int StepperMotor::steps_to_target(float target) | |
68 | { | |
586cc733 | 69 | int target_steps = lroundf(target * steps_per_mm); |
78d0e16a MM |
70 | return target_steps - last_milestone_steps; |
71 | } | |
58b69de8 JM |
72 | |
73 | // Does a manual step pulse, used for direct encoder control of a stepper | |
74 | void StepperMotor::manual_step(bool dir) | |
75 | { | |
76 | if(!is_enabled()) enable(true); | |
77 | ||
78 | // set direction if needed | |
79 | if(this->direction != dir) { | |
80 | this->direction= dir; | |
81 | this->dir_pin.set(dir); | |
82 | wait_us(1); | |
83 | } | |
84 | ||
85 | // pulse step pin | |
86 | this->step_pin.set(1); | |
87 | wait_us(3); | |
88 | this->step_pin.set(0); | |
89 | ||
90 | ||
91 | // keep track of actuators actual position in steps | |
92 | this->current_position_steps += (dir ? -1 : 1); | |
93 | } |