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" |
da3a10b9 | 16 | #include "system_LPC17xx.h" // mbed.h lib |
61134a65 | 17 | #include <math.h> |
bd0f7508 AW |
18 | #include <mri.h> |
19 | ||
9e089978 JM |
20 | #ifdef STEPTICKER_DEBUG_PIN |
21 | #include "gpio.h" | |
22 | extern GPIO stepticker_debug_pin; | |
23 | #endif | |
24 | ||
61134a65 JM |
25 | extern bool _isr_context; |
26 | ||
921bdb42 AW |
27 | // StepTicker handles the base frequency ticking for the Stepper Motors / Actuators |
28 | // It has a list of those, and calls their tick() functions at regular intervals | |
29 | // They then do Bresenham stuff themselves | |
3b1e82d2 | 30 | |
2fa50ca0 | 31 | StepTicker* StepTicker::global_step_ticker; |
3b1e82d2 | 32 | |
3eadcfee | 33 | StepTicker::StepTicker(int nmotors){ |
2fa50ca0 | 34 | StepTicker::global_step_ticker = this; |
93694d6b AW |
35 | |
36 | // Configure the timer | |
8aea2a35 | 37 | LPC_TIM0->MR0 = 10000000; // Initial dummy value for Match Register |
813727fb | 38 | LPC_TIM0->MCR = 3; // Match on MR0, reset on MR0, match on MR1 |
8aea2a35 | 39 | LPC_TIM0->TCR = 0; // Disable interrupt |
796c9f32 | 40 | |
8aea2a35 | 41 | LPC_SC->PCONP |= (1 << 2); // Power Ticker ON |
813727fb AW |
42 | LPC_TIM1->MR0 = 1000000; |
43 | LPC_TIM1->MCR = 1; | |
8aea2a35 | 44 | LPC_TIM1->TCR = 1; // Enable interrupt |
813727fb | 45 | |
7b49793d | 46 | // Default start values |
acf766a4 | 47 | this->a_move_finished = false; |
bd0f7508 | 48 | this->reset_step_pins = false; |
650ed0a8 AW |
49 | this->set_frequency(0.001); |
50 | this->set_reset_delay(100); | |
feb204be | 51 | this->last_duration = 0; |
3680d806 | 52 | //this->overruns= 0; |
3eadcfee JM |
53 | this->num_motors= nmotors; |
54 | this->active_motors= new StepperMotor*[num_motors]; | |
55 | for (int i = 0; i < num_motors; i++){ | |
56 | this->active_motors[i] = nullptr; | |
19f961f6 | 57 | } |
6e0063ab | 58 | this->active_motor_bm = 0; |
796c9f32 | 59 | |
921bdb42 | 60 | NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler |
813727fb | 61 | NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler |
3b1e82d2 AW |
62 | } |
63 | ||
3eadcfee JM |
64 | StepTicker::~StepTicker() { |
65 | delete[] this->active_motors; | |
66 | } | |
67 | ||
921bdb42 | 68 | // Set the base stepping frequency |
1ad23cd3 | 69 | void StepTicker::set_frequency( float frequency ){ |
3b1e82d2 | 70 | this->frequency = frequency; |
9e089978 | 71 | this->period = floorf((SystemCoreClock/4.0F)/frequency); // SystemCoreClock/4 = Timer increments in a second |
feb204be | 72 | LPC_TIM0->MR0 = this->period; |
3b1e82d2 AW |
73 | if( LPC_TIM0->TC > LPC_TIM0->MR0 ){ |
74 | LPC_TIM0->TCR = 3; // Reset | |
75 | LPC_TIM0->TCR = 1; // Reset | |
76 | } | |
77 | } | |
78 | ||
921bdb42 | 79 | // Set the reset delay |
1ad23cd3 | 80 | void StepTicker::set_reset_delay( float seconds ){ |
9e089978 | 81 | this->delay = floorf((SystemCoreClock/4.0F)*seconds); // SystemCoreClock/4 = Timer increments in a second |
813727fb | 82 | LPC_TIM1->MR0 = this->delay; |
3b1e82d2 AW |
83 | } |
84 | ||
6b080aff | 85 | // Call tick() on each active motor |
7b49793d | 86 | inline void StepTicker::tick(){ |
4df07f88 | 87 | _isr_context = true; |
6e0063ab | 88 | int i; |
12aad2a0 | 89 | uint32_t bm = 1; |
61134a65 | 90 | // We iterate over each active motor |
3eadcfee | 91 | for (i = 0; i < num_motors; i++, bm <<= 1){ |
12aad2a0 | 92 | if (this->active_motor_bm & bm){ |
6e0063ab MM |
93 | this->active_motors[i]->tick(); |
94 | } | |
3b1e82d2 | 95 | } |
4df07f88 | 96 | _isr_context = false; |
3b1e82d2 AW |
97 | } |
98 | ||
acf766a4 | 99 | // Call signal_move_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that |
6b080aff | 100 | // all tick()s are called before we do the move finishing |
acf766a4 | 101 | void StepTicker::signal_a_move_finished(){ |
4df07f88 MM |
102 | _isr_context = true; |
103 | ||
39a14e08 | 104 | uint16_t bitmask = 1; |
3eadcfee | 105 | for ( uint8_t motor = 0; motor < num_motors; motor++, bitmask <<= 1){ |
12800c08 AW |
106 | if (this->active_motor_bm & bitmask){ |
107 | if(this->active_motors[motor]->is_move_finished){ | |
108 | this->active_motors[motor]->signal_move_finished(); | |
9e089978 JM |
109 | // Theoretically this does nothing and the reason for it is currently unknown and/or forgotten |
110 | // if(this->active_motors[motor]->moving == false){ | |
111 | // if (motor > 0){ | |
112 | // motor--; | |
113 | // bitmask >>= 1; | |
114 | // } | |
115 | // } | |
6e0063ab | 116 | } |
bd0f7508 | 117 | } |
bd0f7508 | 118 | } |
acf766a4 | 119 | this->a_move_finished = false; |
4df07f88 MM |
120 | |
121 | _isr_context = false; | |
bd0f7508 AW |
122 | } |
123 | ||
6b080aff | 124 | // Reset step pins on all active motors |
feb204be | 125 | inline void StepTicker::reset_tick(){ |
4df07f88 MM |
126 | _isr_context = true; |
127 | ||
6e0063ab MM |
128 | int i; |
129 | uint32_t bm; | |
3eadcfee | 130 | for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1) |
6e0063ab MM |
131 | { |
132 | if (this->active_motor_bm & bm) | |
9c5fa39a | 133 | this->active_motors[i]->unstep(); |
3b1e82d2 | 134 | } |
4df07f88 MM |
135 | |
136 | _isr_context = false; | |
3b1e82d2 AW |
137 | } |
138 | ||
813727fb | 139 | extern "C" void TIMER1_IRQHandler (void){ |
7b49793d | 140 | LPC_TIM1->IR |= 1 << 0; |
2fa50ca0 | 141 | StepTicker::global_step_ticker->reset_tick(); |
813727fb AW |
142 | } |
143 | ||
921bdb42 | 144 | // The actual interrupt handler where we do all the work |
3b1e82d2 | 145 | extern "C" void TIMER0_IRQHandler (void){ |
2fa50ca0 JM |
146 | StepTicker::global_step_ticker->TIMER0_IRQHandler(); |
147 | } | |
b2b0b56d | 148 | |
2fa50ca0 | 149 | void StepTicker::TIMER0_IRQHandler (void){ |
8aea2a35 | 150 | // Reset interrupt register |
813727fb | 151 | LPC_TIM0->IR |= 1 << 0; |
4464301d | 152 | |
7b49793d | 153 | // Step pins |
39a14e08 | 154 | uint16_t bitmask = 1; |
3eadcfee | 155 | for (uint8_t motor = 0; motor < num_motors; motor++, bitmask <<= 1){ |
2fa50ca0 JM |
156 | if (this->active_motor_bm & bitmask){ |
157 | this->active_motors[motor]->tick(); | |
12800c08 AW |
158 | } |
159 | } | |
4464301d | 160 | |
bd0f7508 | 161 | // We may have set a pin on in this tick, now we start the timer to set it off |
2fa50ca0 | 162 | if( this->reset_step_pins ){ |
bd0f7508 AW |
163 | LPC_TIM1->TCR = 3; |
164 | LPC_TIM1->TCR = 1; | |
2fa50ca0 | 165 | this->reset_step_pins = false; |
12aad2a0 AW |
166 | }else{ |
167 | // Nothing happened, nothing after this really matters | |
d337942a | 168 | // TODO : This could be a problem when we use Actuators instead of StepperMotors, because this flag is specific to step generation |
2fa50ca0 | 169 | LPC_TIM0->MR0 = this->period; |
12aad2a0 | 170 | return; |
bd0f7508 | 171 | } |
61134a65 | 172 | |
bd0f7508 | 173 | // If a move finished in this tick, we have to tell the actuator to act accordingly |
acf766a4 | 174 | if( this->a_move_finished ){ |
61134a65 | 175 | |
9e089978 JM |
176 | #ifdef STEPTICKER_DEBUG_PIN |
177 | stepticker_debug_pin= 1; | |
178 | #endif | |
179 | ||
19f961f6 | 180 | // Do not get out of here before everything is nice and tidy |
f550bd20 | 181 | LPC_TIM0->MR0 = 20000000; |
61134a65 | 182 | |
acf766a4 | 183 | this->signal_a_move_finished(); |
19f961f6 AW |
184 | |
185 | // If we went over the duration an interrupt is supposed to last, we have a problem | |
3eadcfee | 186 | // That can happen typically when we change blocks, where more than usual computation is done |
19f961f6 | 187 | // This can be OK, if we take notice of it, which we do now |
9e089978 | 188 | if(LPC_TIM0->TC > this->period ){ // TODO: remove the size condition |
3680d806 | 189 | //overruns++; |
19f961f6 | 190 | uint32_t start_tc = LPC_TIM0->TC; |
19f961f6 AW |
191 | |
192 | // How many ticks we want to skip ( this does not include the current tick, but we add the time we spent doing this computation last time ) | |
2fa50ca0 | 193 | uint32_t ticks_to_skip = ( ( LPC_TIM0->TC + this->last_duration ) / this->period ); |
19f961f6 | 194 | |
3eadcfee | 195 | // Next step is now to reduce this to how many steps we can *actually* skip, as we do not want to cause an actual step |
19f961f6 AW |
196 | uint32_t ticks_we_actually_can_skip = ticks_to_skip; |
197 | ||
198 | int i; | |
199 | uint32_t bm; | |
3eadcfee | 200 | for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1) |
19f961f6 | 201 | { |
3680d806 JM |
202 | if((this->active_motor_bm & bm) != 0) { |
203 | if(this->active_motors[i]->fx_ticks_per_step > this->active_motors[i]->fx_counter) { | |
204 | ticks_we_actually_can_skip = | |
205 | min(ticks_we_actually_can_skip, (uint32_t)((active_motors[i]->fx_ticks_per_step - active_motors[i]->fx_counter) >> active_motors[i]->fx_shift)); | |
206 | }else{ | |
207 | ticks_we_actually_can_skip= 0; | |
208 | } | |
209 | } | |
19f961f6 | 210 | } |
7b49793d | 211 | |
19f961f6 | 212 | // Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially |
3eadcfee | 213 | for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1) |
19f961f6 | 214 | { |
2fa50ca0 | 215 | if (this->active_motor_bm & bm) |
3eadcfee | 216 | this->active_motors[i]->fx_counter += ((uint64_t)ticks_we_actually_can_skip << active_motors[i]->fx_shift); |
19f961f6 | 217 | } |
650ed0a8 | 218 | |
19f961f6 | 219 | // When must we have our next MR0 ? ( +1 is here to account that we are actually doing a legit MR0 match here too, not only overtime ) |
2fa50ca0 | 220 | LPC_TIM0->MR0 = ( ticks_to_skip + 1 ) * this->period; |
650ed0a8 | 221 | |
19f961f6 AW |
222 | // This is so that we know how long this computation takes, and we can take it into account next time |
223 | int difference = (int)(LPC_TIM0->TC) - (int)(start_tc); | |
2fa50ca0 | 224 | if( difference > 0 ){ this->last_duration = (uint32_t)difference; } |
6e0063ab | 225 | |
9e089978 | 226 | |
19f961f6 | 227 | }else{ |
2fa50ca0 | 228 | LPC_TIM0->MR0 = this->period; |
813727fb | 229 | } |
650ed0a8 | 230 | |
19f961f6 | 231 | while( LPC_TIM0->TC > LPC_TIM0->MR0 ){ |
2fa50ca0 | 232 | LPC_TIM0->MR0 += this->period; |
feb204be AW |
233 | } |
234 | ||
9e089978 JM |
235 | #ifdef STEPTICKER_DEBUG_PIN |
236 | stepticker_debug_pin= 0; | |
237 | #endif | |
bd0f7508 | 238 | } |
813727fb | 239 | |
3b1e82d2 AW |
240 | } |
241 | ||
bd0f7508 | 242 | |
796c9f32 | 243 | // We make a list of steppers that want to be called so that we don't call them for nothing |
670fa10b | 244 | void StepTicker::add_motor_to_active_list(StepperMotor* motor) |
6e0063ab MM |
245 | { |
246 | uint32_t bm; | |
247 | int i; | |
3eadcfee | 248 | for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1) |
6e0063ab MM |
249 | { |
250 | if (this->active_motors[i] == motor) | |
251 | { | |
252 | this->active_motor_bm |= bm; | |
f550bd20 | 253 | if( this->active_motor_bm != 0 ){ |
8aea2a35 AW |
254 | LPC_TIM0->TCR = 1; // Enable interrupt |
255 | } | |
6e0063ab MM |
256 | return; |
257 | } | |
3eadcfee | 258 | if (this->active_motors[i] == nullptr) |
6e0063ab MM |
259 | { |
260 | this->active_motors[i] = motor; | |
261 | this->active_motor_bm |= bm; | |
f550bd20 | 262 | if( this->active_motor_bm != 0 ){ |
8aea2a35 AW |
263 | LPC_TIM0->TCR = 1; // Enable interrupt |
264 | } | |
6e0063ab | 265 | return; |
796c9f32 | 266 | } |
796c9f32 | 267 | } |
6e0063ab | 268 | return; |
796c9f32 AW |
269 | } |
270 | ||
6b080aff | 271 | // Remove a stepper from the list of active motors |
670fa10b | 272 | void StepTicker::remove_motor_from_active_list(StepperMotor* motor) |
6e0063ab MM |
273 | { |
274 | uint32_t bm; int i; | |
3eadcfee | 275 | for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1) |
6e0063ab MM |
276 | { |
277 | if (this->active_motors[i] == motor) | |
278 | { | |
279 | this->active_motor_bm &= ~bm; | |
8aea2a35 AW |
280 | // If we have no motor to work on, disable the whole interrupt |
281 | if( this->active_motor_bm == 0 ){ | |
dc4ca298 | 282 | LPC_TIM0->TCR = 0; // Disable interrupt |
8aea2a35 | 283 | } |
6e0063ab MM |
284 | return; |
285 | } | |
796c9f32 | 286 | } |
796c9f32 | 287 | } |