Commit | Line | Data |
---|---|---|
7b49793d | 1 | /* |
cd011f58 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/>. |
cd011f58 AW |
6 | */ |
7 | ||
8 | ||
5673fe39 | 9 | #include "StepTicker.h" |
cd011f58 | 10 | |
3b1e82d2 AW |
11 | #include "libs/nuts_bolts.h" |
12 | #include "libs/Module.h" | |
13 | #include "libs/Kernel.h" | |
5673fe39 | 14 | #include "StepperMotor.h" |
c9cc5e06 | 15 | #include "StreamOutputPool.h" |
8b260c2c | 16 | #include "Block.h" |
f6542ad9 | 17 | #include "Conveyor.h" |
8b260c2c | 18 | |
da3a10b9 | 19 | #include "system_LPC17xx.h" // mbed.h lib |
61134a65 | 20 | #include <math.h> |
bd0f7508 AW |
21 | #include <mri.h> |
22 | ||
9e089978 | 23 | #ifdef STEPTICKER_DEBUG_PIN |
265bab4b | 24 | // debug pins, only used if defined in src/makefile |
9e089978 | 25 | #include "gpio.h" |
265bab4b | 26 | GPIO stepticker_debug_pin(STEPTICKER_DEBUG_PIN); |
ccb06c1d JM |
27 | #define SET_STEPTICKER_DEBUG_PIN(n) {if(n) stepticker_debug_pin.set(); else stepticker_debug_pin.clear(); } |
28 | #else | |
29 | #define SET_STEPTICKER_DEBUG_PIN(n) | |
9e089978 JM |
30 | #endif |
31 | ||
8b260c2c | 32 | StepTicker *StepTicker::instance; |
61134a65 | 33 | |
8b260c2c JM |
34 | StepTicker::StepTicker() |
35 | { | |
36 | instance = this; // setup the Singleton instance of the stepticker | |
93694d6b AW |
37 | |
38 | // Configure the timer | |
8aea2a35 | 39 | LPC_TIM0->MR0 = 10000000; // Initial dummy value for Match Register |
e0a9f9e1 | 40 | LPC_TIM0->MCR = 3; // Match on MR0, reset on MR0 |
8aea2a35 | 41 | LPC_TIM0->TCR = 0; // Disable interrupt |
796c9f32 | 42 | |
8aea2a35 | 43 | LPC_SC->PCONP |= (1 << 2); // Power Ticker ON |
813727fb | 44 | LPC_TIM1->MR0 = 1000000; |
e0a9f9e1 | 45 | LPC_TIM1->MCR = 5; // match on Mr0, stop on match |
aed1f6ca | 46 | LPC_TIM1->TCR = 0; // Disable interrupt |
813727fb | 47 | |
7b49793d | 48 | // Default start values |
3b1acdaa | 49 | this->set_frequency(100000); |
8b260c2c JM |
50 | this->set_unstep_time(100); |
51 | ||
52 | this->unstep.reset(); | |
53 | this->num_motors = 0; | |
54 | ||
b5708347 | 55 | this->running = false; |
a19a873f | 56 | this->current_block = nullptr; |
265bab4b JM |
57 | |
58 | #ifdef STEPTICKER_DEBUG_PIN | |
59 | // setup debug pin if defined | |
60 | stepticker_debug_pin.output(); | |
61 | stepticker_debug_pin= 0; | |
62 | #endif | |
3b1e82d2 AW |
63 | } |
64 | ||
8b260c2c JM |
65 | StepTicker::~StepTicker() |
66 | { | |
3eadcfee JM |
67 | } |
68 | ||
b772a11c | 69 | //called when everything is setup and interrupts can start |
8b260c2c JM |
70 | void StepTicker::start() |
71 | { | |
dc3542cf JM |
72 | NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler |
73 | NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler | |
b9ad75de | 74 | current_tick= 0; |
dc3542cf JM |
75 | } |
76 | ||
921bdb42 | 77 | // Set the base stepping frequency |
8b260c2c JM |
78 | void StepTicker::set_frequency( float frequency ) |
79 | { | |
3b1e82d2 | 80 | this->frequency = frequency; |
8b260c2c | 81 | this->period = floorf((SystemCoreClock / 4.0F) / frequency); // SystemCoreClock/4 = Timer increments in a second |
feb204be | 82 | LPC_TIM0->MR0 = this->period; |
1598a726 JM |
83 | LPC_TIM0->TCR = 3; // Reset |
84 | LPC_TIM0->TCR = 1; // start | |
3b1e82d2 AW |
85 | } |
86 | ||
12ae3902 | 87 | // Set the reset delay, must be called after set_frequency |
8b260c2c JM |
88 | void StepTicker::set_unstep_time( float microseconds ) |
89 | { | |
90 | uint32_t delay = floorf((SystemCoreClock / 4.0F) * (microseconds / 1000000.0F)); // SystemCoreClock/4 = Timer increments in a second | |
aed1f6ca | 91 | LPC_TIM1->MR0 = delay; |
12ae3902 JM |
92 | |
93 | // TODO check that the unstep time is less than the step period, if not slow down step ticker | |
3b1e82d2 AW |
94 | } |
95 | ||
aed1f6ca | 96 | // Reset step pins on any motor that was stepped |
8b260c2c JM |
97 | void StepTicker::unstep_tick() |
98 | { | |
1fce036c | 99 | for (int i = 0; i < num_motors; i++) { |
8b260c2c | 100 | if(this->unstep[i]) { |
1fce036c | 101 | this->motor[i]->unstep(); |
aed1f6ca | 102 | } |
3b1e82d2 | 103 | } |
13256955 | 104 | this->unstep.reset(); |
3b1e82d2 AW |
105 | } |
106 | ||
8b260c2c JM |
107 | extern "C" void TIMER1_IRQHandler (void) |
108 | { | |
7b49793d | 109 | LPC_TIM1->IR |= 1 << 0; |
8b260c2c | 110 | StepTicker::getInstance()->unstep_tick(); |
813727fb AW |
111 | } |
112 | ||
921bdb42 | 113 | // The actual interrupt handler where we do all the work |
8b260c2c JM |
114 | extern "C" void TIMER0_IRQHandler (void) |
115 | { | |
1598a726 JM |
116 | // Reset interrupt register |
117 | LPC_TIM0->IR |= 1 << 0; | |
118 | StepTicker::getInstance()->step_tick(); | |
dc3542cf JM |
119 | } |
120 | ||
8b260c2c JM |
121 | extern "C" void PendSV_Handler(void) |
122 | { | |
1598a726 | 123 | StepTicker::getInstance()->handle_finish(); |
16220afe JM |
124 | } |
125 | ||
126 | // slightly lower priority than TIMER0, the whole end of block/start of block is done here allowing the timer to continue ticking | |
1598a726 | 127 | void StepTicker::handle_finish (void) |
8b260c2c | 128 | { |
8b260c2c JM |
129 | // all moves finished signal block is finished |
130 | if(finished_fnc) finished_fnc(); | |
cb2e6bc6 JM |
131 | } |
132 | ||
8b260c2c | 133 | // step clock |
1598a726 | 134 | void StepTicker::step_tick (void) |
8b260c2c | 135 | { |
ccb06c1d | 136 | //SET_STEPTICKER_DEBUG_PIN(running ? 1 : 0); |
8b260c2c | 137 | |
a19a873f | 138 | // if nothing has been setup we ignore the ticks |
b5708347 | 139 | if(!running){ |
a19a873f JM |
140 | // check if anything new available |
141 | if(THECONVEYOR->get_next_block(¤t_block)) { // returns false if no new block is available | |
142 | running= start_next_block(); // returns true if there is at least one motor with steps to issue | |
143 | if(!running) return; | |
144 | }else{ | |
145 | return; | |
146 | } | |
147 | } | |
148 | ||
149 | if(THEKERNEL->is_halted()) { | |
150 | running= false; | |
b9ad75de | 151 | current_tick = 0; |
6c0d8cf7 | 152 | current_block= nullptr; |
a19a873f | 153 | return; |
b5708347 | 154 | } |
8b260c2c | 155 | |
b5708347 | 156 | bool still_moving= false; |
8b260c2c JM |
157 | // foreach motor, if it is active see if time to issue a step to that motor |
158 | for (uint8_t m = 0; m < num_motors; m++) { | |
f6542ad9 | 159 | if(current_block->tick_info[m].steps_to_move == 0) continue; // not active |
8b260c2c | 160 | |
f6542ad9 | 161 | current_block->tick_info[m].steps_per_tick += current_block->tick_info[m].acceleration_change; |
d1d120e1 | 162 | |
f6542ad9 JM |
163 | if(current_tick == current_block->tick_info[m].next_accel_event) { |
164 | if(current_tick == current_block->accelerate_until) { // We are done accelerating, deceleration becomes 0 : plateau | |
165 | current_block->tick_info[m].acceleration_change = 0; | |
166 | if(current_block->decelerate_after < current_block->total_move_ticks) { | |
167 | current_block->tick_info[m].next_accel_event = current_block->decelerate_after; | |
168 | if(current_tick != current_block->decelerate_after) { // We are plateauing | |
1598a726 | 169 | // steps/sec / tick frequency to get steps per tick |
f6542ad9 | 170 | current_block->tick_info[m].steps_per_tick = current_block->tick_info[m].plateau_rate; |
8b260c2c JM |
171 | } |
172 | } | |
173 | } | |
174 | ||
f6542ad9 JM |
175 | if(current_tick == current_block->decelerate_after) { // We start decelerating |
176 | current_block->tick_info[m].acceleration_change = current_block->tick_info[m].deceleration_change; | |
8b260c2c JM |
177 | } |
178 | } | |
179 | ||
180 | // protect against rounding errors and such | |
f6542ad9 JM |
181 | if(current_block->tick_info[m].steps_per_tick <= 0) { |
182 | current_block->tick_info[m].counter = STEPTICKER_FPSCALE; // we force completion this step by setting to 1.0 | |
183 | current_block->tick_info[m].steps_per_tick = 0; | |
8b260c2c JM |
184 | } |
185 | ||
f6542ad9 | 186 | current_block->tick_info[m].counter += current_block->tick_info[m].steps_per_tick; |
8b260c2c | 187 | |
f6542ad9 JM |
188 | if(current_block->tick_info[m].counter >= STEPTICKER_FPSCALE) { // >= 1.0 step time |
189 | current_block->tick_info[m].counter -= STEPTICKER_FPSCALE; // -= 1.0F; | |
190 | ++current_block->tick_info[m].step_count; | |
8b260c2c JM |
191 | |
192 | // step the motor | |
c8bac202 | 193 | bool ismoving= motor[m]->step(); // returns false if the moving flag was set to false externally (probes, endstops etc) |
778093ce | 194 | // we stepped so schedule an unstep |
8b260c2c JM |
195 | unstep.set(m); |
196 | ||
c8bac202 | 197 | if(!ismoving || current_block->tick_info[m].step_count == current_block->tick_info[m].steps_to_move) { |
8b260c2c | 198 | // done |
f6542ad9 | 199 | current_block->tick_info[m].steps_to_move = 0; |
ad6a77d1 | 200 | motor[m]->stop_moving(); // let motor know it is no longer moving |
8b260c2c | 201 | } |
aed1f6ca | 202 | } |
b5708347 JM |
203 | |
204 | // see if any motors are still moving after this tick | |
ad6a77d1 | 205 | if(motor[m]->is_moving()) still_moving= true; |
12800c08 | 206 | } |
4464301d | 207 | |
1ae56063 JM |
208 | // do this after so we start at tick 0 |
209 | current_tick++; // count number of ticks | |
210 | ||
aed1f6ca JM |
211 | // We may have set a pin on in this tick, now we reset the timer to set it off |
212 | // Note there could be a race here if we run another tick before the unsteps have happened, | |
213 | // right now it takes about 3-4us but if the unstep were near 10uS or greater it would be an issue | |
214 | // also it takes at least 2us to get here so even when set to 1us pulse width it will still be about 3us | |
8b260c2c | 215 | if( unstep.any()) { |
bd0f7508 AW |
216 | LPC_TIM1->TCR = 3; |
217 | LPC_TIM1->TCR = 1; | |
a157d099 | 218 | } |
3b1acdaa | 219 | |
b5708347 JM |
220 | |
221 | // see if any motors are still moving | |
8b260c2c | 222 | if(!still_moving) { |
e0a9f9e1 | 223 | //SET_STEPTICKER_DEBUG_PIN(0); |
d1d120e1 | 224 | |
1598a726 | 225 | // all moves finished |
8b260c2c JM |
226 | current_tick = 0; |
227 | ||
f6542ad9 | 228 | // get next block |
8b260c2c | 229 | // do it here so there is no delay in ticks |
a19a873f | 230 | THECONVEYOR->block_finished(); |
f6542ad9 | 231 | |
a19a873f | 232 | if(THECONVEYOR->get_next_block(¤t_block)) { // returns false if no new block is available |
f6542ad9 | 233 | running= start_next_block(); // returns true if there is at least one motor with steps to issue |
a19a873f | 234 | |
f6542ad9 JM |
235 | }else{ |
236 | current_block= nullptr; | |
237 | running= false; | |
238 | } | |
8b260c2c JM |
239 | |
240 | // all moves finished | |
f095cddd | 241 | // we delegate the slow stuff to the pendsv handler which will run as soon as this interrupt exits |
16220afe | 242 | //NVIC_SetPendingIRQ(PendSV_IRQn); this doesn't work |
b5708347 | 243 | //SCB->ICSR = 0x10000000; // SCB_ICSR_PENDSVSET_Msk; |
bd0f7508 | 244 | } |
3b1e82d2 AW |
245 | } |
246 | ||
d1d120e1 | 247 | // only called from the step tick ISR (single consumer) |
f6542ad9 | 248 | bool StepTicker::start_next_block() |
d1d120e1 | 249 | { |
f6542ad9 | 250 | if(current_block == nullptr) return false; |
d1d120e1 | 251 | |
f6542ad9 | 252 | bool ok= false; |
d1d120e1 | 253 | // need to prepare each active motor |
d1d120e1 | 254 | for (uint8_t m = 0; m < num_motors; m++) { |
f6542ad9 | 255 | if(current_block->tick_info[m].steps_to_move == 0) continue; |
d1d120e1 | 256 | |
f6542ad9 | 257 | ok= true; // mark at least one motor is moving |
d1d120e1 JM |
258 | // set direction bit here |
259 | // NOTE this would be at least 10us before first step pulse. | |
260 | // TODO does this need to be done sooner, if so how without delaying next tick | |
f6542ad9 | 261 | motor[m]->set_direction(current_block->direction_bits[m]); |
ad6a77d1 | 262 | motor[m]->start_moving(); // also let motor know it is moving now |
d1d120e1 JM |
263 | } |
264 | ||
b9ad75de JM |
265 | current_tick= 0; |
266 | ||
f6542ad9 | 267 | if(ok) { |
e0a9f9e1 | 268 | //SET_STEPTICKER_DEBUG_PIN(1); |
433d636f | 269 | return true; |
4de83d43 JM |
270 | |
271 | }else{ | |
272 | // this is an edge condition that should never happen, but we need to discard this block if it ever does | |
273 | // basically it is a block that has zero steps for all motors | |
274 | THECONVEYOR->block_finished(); | |
433d636f JM |
275 | } |
276 | ||
277 | return false; | |
796c9f32 AW |
278 | } |
279 | ||
f6542ad9 | 280 | |
8b260c2c JM |
281 | // returns index of the stepper motor in the array and bitset |
282 | int StepTicker::register_motor(StepperMotor* m) | |
6e0063ab | 283 | { |
8b260c2c JM |
284 | motor[num_motors++] = m; |
285 | return num_motors - 1; | |
796c9f32 | 286 | } |