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/>.
12 #include "libs/nuts_bolts.h"
13 #include "libs/Module.h"
14 #include "libs/Kernel.h"
15 #include "StepTicker.h"
16 #include "system_LPC17xx.h" // mbed.h lib
20 // StepTicker handles the base frequency ticking for the Stepper Motors / Actuators
21 // It has a list of those, and calls their tick() functions at regular intervals
22 // They then do Bresenham stuff themselves
24 StepTicker
* global_step_ticker
;
26 StepTicker::StepTicker(){
27 global_step_ticker
= this;
28 LPC_TIM0
->MR0
= 10000000; // Initial dummy value for Match Register
29 LPC_TIM0
->MCR
= 3; // Match on MR0, reset on MR0, match on MR1
30 LPC_TIM0
->TCR
= 1; // Enable interrupt
32 LPC_SC
->PCONP
|= (1 << 2); // Power Ticker ON
33 LPC_TIM1
->MR0
= 1000000;
35 LPC_TIM1
->TCR
= 1; // Enable interrupt
37 // Default start values
38 this->moves_finished
= false;
39 this->reset_step_pins
= false;
42 this->set_frequency(0.001);
43 this->set_reset_delay(100);
44 this->last_duration
= 0;
45 this->active_motors
[0] = NULL
;
47 NVIC_EnableIRQ(TIMER0_IRQn
); // Enable interrupt handler
48 NVIC_EnableIRQ(TIMER1_IRQn
); // Enable interrupt handler
51 // Set the base stepping frequency
52 void StepTicker::set_frequency( double frequency
){
53 this->frequency
= frequency
;
54 this->period
= int(floor((SystemCoreClock
/4)/frequency
)); // SystemCoreClock/4 = Timer increments in a second
55 LPC_TIM0
->MR0
= this->period
;
56 if( LPC_TIM0
->TC
> LPC_TIM0
->MR0
){
57 LPC_TIM0
->TCR
= 3; // Reset
58 LPC_TIM0
->TCR
= 1; // Reset
62 // Set the reset delay
63 void StepTicker::set_reset_delay( double seconds
){
64 this->delay
= int(floor(double(SystemCoreClock
/4)*( seconds
))); // SystemCoreClock/4 = Timer increments in a second
65 LPC_TIM1
->MR0
= this->delay
;
68 // Add a stepper motor object to our list of steppers we must take care of
69 StepperMotor
* StepTicker::add_stepper_motor(StepperMotor
* stepper_motor
){
70 this->stepper_motors
.push_back(stepper_motor
);
71 stepper_motor
->step_ticker
= this;
72 this->has_axes
= true;
76 // Call tick() on each active motor
77 inline void StepTicker::tick(){
78 uint8_t current_id
= 0;
79 StepperMotor
* current
= this->active_motors
[0];
80 while(current
!= NULL
){
83 current
= this->active_motors
[current_id
];
87 // Call signal_mode_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that
88 // all tick()s are called before we do the move finishing
89 void StepTicker::signal_moves_finished(){
90 uint8_t current_id
= 0;
91 StepperMotor
* current
= this->active_motors
[0];
92 while(current
!= NULL
){
93 if( current
->is_move_finished
){
94 current
->signal_move_finished();
95 if( current
->moving
== false ){ current_id
--; }
98 current
= this->active_motors
[current_id
];
100 this->moves_finished
= false;
103 // Reset step pins on all active motors
104 inline void StepTicker::reset_tick(){
105 uint8_t current_id
= 0;
106 StepperMotor
* current
= this->active_motors
[0];
107 while(current
!= NULL
){
108 current
->step_pin
->set(0);
110 current
= this->active_motors
[current_id
];
114 extern "C" void TIMER1_IRQHandler (void){
115 LPC_TIM1
->IR
|= 1 << 0;
116 global_step_ticker
->reset_tick();
120 //#pragma GCC push_options
121 //#pragma GCC optimize ("O0")
124 // The actual interrupt handler where we do all the work
125 extern "C" void TIMER0_IRQHandler (void){
127 LPC_GPIO1
->FIODIR
|= 1<<18;
128 LPC_GPIO1
->FIOSET
= 1<<18;
130 uint32_t initial_tc
= LPC_TIM0
->TC
;
132 LPC_TIM0
->IR
|= 1 << 0;
134 // If no axes enabled, just ignore for now
135 if( global_step_ticker
->active_motors
[0] == NULL
){
136 LPC_GPIO1
->FIOCLR
= 1<<18;
140 // Do not get out of here before everything is nice and tidy
141 LPC_TIM0
->MR0
= 2000000;
144 global_step_ticker
->tick();
146 // We may have set a pin on in this tick, now we start the timer to set it off
147 if( global_step_ticker
->reset_step_pins
){
150 global_step_ticker
->reset_step_pins
= false;
153 // If a move finished in this tick, we have to tell the actuator to act accordingly
154 if( global_step_ticker
->moves_finished
){ global_step_ticker
->signal_moves_finished(); }
156 // If we went over the duration an interrupt is supposed to last, we have a problem
157 // That can happen tipically when we change blocks, where more than usual computation is done
158 // This can be OK, if we take notice of it, which we do now
159 if( LPC_TIM0
->TC
> global_step_ticker
->period
){ // TODO : remove the size condition
161 LPC_GPIO1
->FIODIR
|= 1<<19;
162 LPC_GPIO1
->FIOSET
= 1<<19;
164 uint32_t start_tc
= LPC_TIM0
->TC
;
166 // 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 )
167 uint32_t ticks_to_skip
= ( ( LPC_TIM0
->TC
+ global_step_ticker
->last_duration
) / global_step_ticker
->period
);
169 // Next step is now to reduce this to how many steps we can *actually* skip
170 uint32_t ticks_we_actually_can_skip
= ticks_to_skip
;
171 uint8_t current_id
= 0; StepperMotor
* current
= global_step_ticker
->active_motors
[0];
172 while(current
!= NULL
){ // For each active stepper
173 ticks_we_actually_can_skip
= min( ticks_we_actually_can_skip
, (uint32_t)((uint64_t)( (uint64_t)current
->fx_ticks_per_step
- (uint64_t)current
->fx_counter
) >> 32) );
175 current
= global_step_ticker
->active_motors
[current_id
];
178 // Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
179 current_id
= 0; current
= global_step_ticker
->active_motors
[0];
180 while(current
!= NULL
){ // For each active stepper
181 current
->fx_counter
+= (uint64_t)((uint64_t)(ticks_we_actually_can_skip
)<<32);
183 current
= global_step_ticker
->active_motors
[current_id
];
186 // 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 )
187 // LPC_TIM0->MR0 = ( ticks_we_actually_can_skip + 1 ) * global_step_ticker->period;
188 LPC_TIM0
->MR0
= ( ticks_to_skip
+ 1 ) * global_step_ticker
->period
;
190 // This is so that we know how long this computation takes, and we can take it into account next time
191 int difference
= (int)(LPC_TIM0
->TC
) - (int)(start_tc
);
192 if( difference
> 0 ){ global_step_ticker
->last_duration
= (uint32_t)difference
; }
194 //if( global_step_ticker->last_duration > 2000 || LPC_TIM0->MR0 > 2000 || LPC_TIM0->TC > 2000 || initial_tc > 2000 ){ __debugbreak(); }
196 LPC_GPIO1
->FIOCLR
= 1<<19;
199 LPC_TIM0
->MR0
= global_step_ticker
->period
;
202 while( LPC_TIM0
->TC
> LPC_TIM0
->MR0
){
203 LPC_TIM0
->MR0
+= global_step_ticker
->period
;
206 LPC_GPIO1
->FIOCLR
= 1<<18;
210 //#pragma GCC pop_options
212 // We make a list of steppers that want to be called so that we don't call them for nothing
213 void StepTicker::add_motor_to_active_list(StepperMotor
* motor
){
214 uint8_t current_id
= 0;
215 StepperMotor
* current
= this->active_motors
[0];
216 while(current
!= NULL
){
217 if( current
== motor
){
218 return; // Motor already in list
221 current
= this->active_motors
[current_id
];
223 this->active_motors
[current_id
] = motor
;
224 this->active_motors
[current_id
+1] = NULL
;
228 // Remove a stepper from the list of active motors
229 void StepTicker::remove_motor_from_active_list(StepperMotor
* motor
){
230 uint8_t current_id
= 0;
232 StepperMotor
* current
= this->active_motors
[0];
233 while( current
!= NULL
){
234 if( current
== motor
){ offset
++; }
235 this->active_motors
[current_id
] = this->active_motors
[current_id
+offset
];
237 current
= this->active_motors
[current_id
+offset
];
239 this->active_motors
[current_id
] = NULL
;