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 JM |
16 | #include "Block.h" |
17 | ||
da3a10b9 | 18 | #include "system_LPC17xx.h" // mbed.h lib |
61134a65 | 19 | #include <math.h> |
bd0f7508 AW |
20 | #include <mri.h> |
21 | ||
9e089978 JM |
22 | #ifdef STEPTICKER_DEBUG_PIN |
23 | #include "gpio.h" | |
24 | extern GPIO stepticker_debug_pin; | |
25 | #endif | |
26 | ||
8b260c2c | 27 | StepTicker *StepTicker::instance; |
61134a65 | 28 | |
1ae56063 JM |
29 | // handle 2.30 Fixed point |
30 | #define FPSCALE (1<<30) | |
31 | #define TOFP(x) ((int32_t)roundf((float)(x)*FPSCALE)) | |
32 | #define FROMFP(x) ((float)(x)/FPSCALE) | |
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 |
813727fb | 40 | LPC_TIM0->MCR = 3; // Match on MR0, reset on MR0, match on MR1 |
8aea2a35 | 41 | LPC_TIM0->TCR = 0; // Disable interrupt |
796c9f32 | 42 | |
8aea2a35 | 43 | LPC_SC->PCONP |= (1 << 2); // Power Ticker ON |
813727fb AW |
44 | LPC_TIM1->MR0 = 1000000; |
45 | LPC_TIM1->MCR = 1; | |
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 | ||
55 | this->move_issued = false; | |
3b1e82d2 AW |
56 | } |
57 | ||
8b260c2c JM |
58 | StepTicker::~StepTicker() |
59 | { | |
3eadcfee JM |
60 | } |
61 | ||
b772a11c | 62 | //called when everything is setup and interrupts can start |
8b260c2c JM |
63 | void StepTicker::start() |
64 | { | |
dc3542cf JM |
65 | NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler |
66 | NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler | |
dc3542cf JM |
67 | } |
68 | ||
921bdb42 | 69 | // Set the base stepping frequency |
8b260c2c JM |
70 | void StepTicker::set_frequency( float frequency ) |
71 | { | |
3b1e82d2 | 72 | this->frequency = frequency; |
8b260c2c | 73 | this->period = floorf((SystemCoreClock / 4.0F) / frequency); // SystemCoreClock/4 = Timer increments in a second |
feb204be | 74 | LPC_TIM0->MR0 = this->period; |
1598a726 JM |
75 | LPC_TIM0->TCR = 3; // Reset |
76 | LPC_TIM0->TCR = 1; // start | |
3b1e82d2 AW |
77 | } |
78 | ||
921bdb42 | 79 | // Set the reset delay |
8b260c2c JM |
80 | void StepTicker::set_unstep_time( float microseconds ) |
81 | { | |
82 | uint32_t delay = floorf((SystemCoreClock / 4.0F) * (microseconds / 1000000.0F)); // SystemCoreClock/4 = Timer increments in a second | |
aed1f6ca | 83 | LPC_TIM1->MR0 = delay; |
3b1e82d2 AW |
84 | } |
85 | ||
aed1f6ca | 86 | // Reset step pins on any motor that was stepped |
8b260c2c JM |
87 | void StepTicker::unstep_tick() |
88 | { | |
1fce036c | 89 | for (int i = 0; i < num_motors; i++) { |
8b260c2c | 90 | if(this->unstep[i]) { |
1fce036c | 91 | this->motor[i]->unstep(); |
aed1f6ca | 92 | } |
3b1e82d2 | 93 | } |
13256955 | 94 | this->unstep.reset(); |
3b1e82d2 AW |
95 | } |
96 | ||
8b260c2c JM |
97 | extern "C" void TIMER1_IRQHandler (void) |
98 | { | |
7b49793d | 99 | LPC_TIM1->IR |= 1 << 0; |
8b260c2c | 100 | StepTicker::getInstance()->unstep_tick(); |
813727fb AW |
101 | } |
102 | ||
921bdb42 | 103 | // The actual interrupt handler where we do all the work |
8b260c2c JM |
104 | extern "C" void TIMER0_IRQHandler (void) |
105 | { | |
1598a726 JM |
106 | // Reset interrupt register |
107 | LPC_TIM0->IR |= 1 << 0; | |
108 | StepTicker::getInstance()->step_tick(); | |
dc3542cf JM |
109 | } |
110 | ||
8b260c2c JM |
111 | extern "C" void PendSV_Handler(void) |
112 | { | |
1598a726 | 113 | StepTicker::getInstance()->handle_finish(); |
16220afe JM |
114 | } |
115 | ||
116 | // slightly lower priority than TIMER0, the whole end of block/start of block is done here allowing the timer to continue ticking | |
1598a726 | 117 | void StepTicker::handle_finish (void) |
8b260c2c | 118 | { |
8b260c2c JM |
119 | // all moves finished signal block is finished |
120 | if(finished_fnc) finished_fnc(); | |
cb2e6bc6 JM |
121 | } |
122 | ||
16220afe | 123 | |
8b260c2c | 124 | // step clock |
1598a726 | 125 | void StepTicker::step_tick (void) |
8b260c2c JM |
126 | { |
127 | static uint32_t current_tick = 0; | |
128 | ||
a3bb687b JM |
129 | if(!move_issued){ |
130 | if(jobq.empty()) return; // if nothing has been setup we ignore the ticks | |
131 | // get next job, and copy data | |
132 | Block *b= nullptr; | |
133 | jobq.get(b); | |
134 | copy_block(b); | |
135 | } | |
8b260c2c | 136 | |
8b260c2c JM |
137 | bool still_moving = false; |
138 | ||
139 | // foreach motor, if it is active see if time to issue a step to that motor | |
140 | for (uint8_t m = 0; m < num_motors; m++) { | |
141 | if(tick_info[m].steps_to_move == 0) continue; // not active | |
142 | ||
143 | still_moving = true; | |
144 | tick_info[m].steps_per_tick += tick_info[m].acceleration_change; | |
145 | ||
146 | if(current_tick == tick_info[m].next_accel_event) { | |
147 | if(current_tick == block_info.accelerate_until) { // We are done accelerating, deceleration becomes 0 : plateau | |
148 | tick_info[m].acceleration_change = 0; | |
149 | if(block_info.decelerate_after < block_info.total_move_ticks) { | |
150 | tick_info[m].next_accel_event = block_info.decelerate_after; | |
1598a726 JM |
151 | if(current_tick != block_info.decelerate_after) { // We are plateauing |
152 | // steps/sec / tick frequency to get steps per tick | |
153 | tick_info[m].steps_per_tick = tick_info[m].plateau_rate; | |
8b260c2c JM |
154 | } |
155 | } | |
156 | } | |
157 | ||
158 | if(current_tick == block_info.decelerate_after) { // We start decelerating | |
1598a726 | 159 | tick_info[m].acceleration_change = tick_info[m].deceleration_change; |
8b260c2c JM |
160 | } |
161 | } | |
162 | ||
163 | // protect against rounding errors and such | |
164 | if(tick_info[m].steps_per_tick <= 0) { | |
1ae56063 | 165 | tick_info[m].counter = FPSCALE; // we force completion this step by setting to 1.0 |
8b260c2c JM |
166 | tick_info[m].steps_per_tick = 0; |
167 | } | |
168 | ||
169 | tick_info[m].counter += tick_info[m].steps_per_tick; | |
170 | ||
1ae56063 JM |
171 | if(tick_info[m].counter >= FPSCALE) { // > 1.0 step time |
172 | tick_info[m].counter -= FPSCALE; // -= 1.0F; | |
8b260c2c JM |
173 | ++tick_info[m].step_count; |
174 | ||
175 | // step the motor | |
176 | motor[m]->step(); | |
778093ce | 177 | // we stepped so schedule an unstep |
8b260c2c JM |
178 | unstep.set(m); |
179 | ||
180 | if(tick_info[m].step_count == tick_info[m].steps_to_move) { | |
181 | // done | |
182 | tick_info[m].steps_to_move = 0; | |
183 | } | |
aed1f6ca | 184 | } |
12800c08 | 185 | } |
4464301d | 186 | |
1ae56063 JM |
187 | // do this after so we start at tick 0 |
188 | current_tick++; // count number of ticks | |
189 | ||
aed1f6ca JM |
190 | // We may have set a pin on in this tick, now we reset the timer to set it off |
191 | // Note there could be a race here if we run another tick before the unsteps have happened, | |
192 | // right now it takes about 3-4us but if the unstep were near 10uS or greater it would be an issue | |
193 | // also it takes at least 2us to get here so even when set to 1us pulse width it will still be about 3us | |
8b260c2c | 194 | if( unstep.any()) { |
bd0f7508 AW |
195 | LPC_TIM1->TCR = 3; |
196 | LPC_TIM1->TCR = 1; | |
a157d099 | 197 | } |
3b1acdaa | 198 | |
8b260c2c | 199 | if(!still_moving) { |
1598a726 | 200 | // all moves finished |
8b260c2c JM |
201 | current_tick = 0; |
202 | ||
203 | // get next static block and tick info from next block | |
204 | // do it here so there is no delay in ticks | |
a3bb687b | 205 | if(!jobq.empty()) { |
1598a726 JM |
206 | #ifdef STEPTICKER_DEBUG_PIN |
207 | stepticker_debug_pin = 1; | |
208 | #endif | |
209 | ||
a3bb687b JM |
210 | // get next job, and copy data |
211 | Block *b= nullptr; | |
212 | jobq.get(b); | |
213 | copy_block(b); | |
8b260c2c | 214 | |
1598a726 JM |
215 | #ifdef STEPTICKER_DEBUG_PIN |
216 | stepticker_debug_pin = 0; | |
217 | #endif | |
a3bb687b | 218 | |
8b260c2c | 219 | } else { |
1ae56063 | 220 | move_issued = false; // nothing to do as no more jobs |
8b260c2c JM |
221 | } |
222 | ||
223 | // all moves finished | |
f095cddd | 224 | // we delegate the slow stuff to the pendsv handler which will run as soon as this interrupt exits |
16220afe JM |
225 | //NVIC_SetPendingIRQ(PendSV_IRQn); this doesn't work |
226 | SCB->ICSR = 0x10000000; // SCB_ICSR_PENDSVSET_Msk; | |
bd0f7508 | 227 | } |
3b1e82d2 AW |
228 | } |
229 | ||
1ae56063 | 230 | // this takes about 20us, so we may miss two ticks, we can make this up if needed |
8b260c2c JM |
231 | // called in ISR if running, else can be called from anything to start |
232 | void StepTicker::copy_block(Block *block) | |
6e0063ab | 233 | { |
a3bb687b | 234 | stepticker_debug_pin = 1; |
8b260c2c JM |
235 | block_info.accelerate_until = block->accelerate_until; |
236 | block_info.decelerate_after = block->decelerate_after; | |
8b260c2c JM |
237 | block_info.total_move_ticks = block->total_move_ticks; |
238 | ||
239 | float inv = 1.0F / block->steps_event_count; | |
240 | for (uint8_t m = 0; m < num_motors; m++) { | |
241 | uint32_t steps = block->steps[m]; | |
242 | tick_info[m].steps_to_move = steps; | |
243 | if(steps == 0) continue; | |
244 | ||
245 | // set direction bit here | |
246 | motor[m]->set_direction(block->direction_bits[m]); | |
247 | ||
248 | float aratio = inv * steps; | |
1ae56063 | 249 | tick_info[m].steps_per_tick = TOFP((block->initial_rate * aratio) / frequency); // steps/sec / tick frequency to get steps per tick in 2.30 fixed point |
8b260c2c | 250 | tick_info[m].counter = 0; // 2.30 fixed point |
8b260c2c JM |
251 | tick_info[m].step_count = 0; |
252 | tick_info[m].next_accel_event = block->total_move_ticks + 1; | |
1598a726 JM |
253 | |
254 | float acceleration_change = 0; | |
8b260c2c JM |
255 | if(block->accelerate_until != 0) { // If the next accel event is the end of accel |
256 | tick_info[m].next_accel_event = block->accelerate_until; | |
1598a726 | 257 | acceleration_change = block->acceleration_per_tick; |
8b260c2c JM |
258 | |
259 | } else if(block->decelerate_after == 0 /*&& block->accelerate_until == 0*/) { | |
260 | // we start off decelerating | |
1598a726 | 261 | acceleration_change = -block->deceleration_per_tick; |
8b260c2c JM |
262 | |
263 | } else if(block->decelerate_after != block->total_move_ticks /*&& block->accelerate_until == 0*/) { | |
264 | // If the next event is the start of decel ( don't set this if the next accel event is accel end ) | |
265 | tick_info[m].next_accel_event = block->decelerate_after; | |
266 | } | |
1598a726 JM |
267 | |
268 | // convert to fixed point after scaling | |
1ae56063 JM |
269 | tick_info[m].acceleration_change= TOFP(acceleration_change * aratio); |
270 | tick_info[m].deceleration_change= -TOFP(block->deceleration_per_tick * aratio); | |
271 | tick_info[m].plateau_rate= TOFP((block->maximum_rate * aratio) / frequency); | |
796c9f32 | 272 | } |
8b260c2c | 273 | move_issued = true; |
a3bb687b | 274 | stepticker_debug_pin = 0; |
796c9f32 AW |
275 | } |
276 | ||
8b260c2c JM |
277 | // returns index of the stepper motor in the array and bitset |
278 | int StepTicker::register_motor(StepperMotor* m) | |
6e0063ab | 279 | { |
8b260c2c JM |
280 | motor[num_motors++] = m; |
281 | return num_motors - 1; | |
796c9f32 | 282 | } |