StepTicker.cpp

00001 /*
00002       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
00003       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.
00004       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.
00005       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
00006 */
00007 
00008 
00009 #include "StepTicker.h"
00010 
00011 using namespace std;
00012 #include <vector>
00013 
00014 #include "libs/nuts_bolts.h"
00015 #include "libs/Module.h"
00016 #include "libs/Kernel.h"
00017 #include "StepperMotor.h"
00018 
00019 #include "system_LPC17xx.h" // mbed.h lib
00020 
00021 #include <mri.h>
00022 
00023 // StepTicker handles the base frequency ticking for the Stepper Motors / Actuators
00024 // It has a list of those, and calls their tick() functions at regular intervals
00025 // They then do Bresenham stuff themselves
00026 
00027 StepTicker* global_step_ticker;
00028 
00029 StepTicker::StepTicker(){
00030     global_step_ticker = this;
00031 
00032     // Configure the timer
00033     LPC_TIM0->MR0 = 10000000;       // Initial dummy value for Match Register
00034     LPC_TIM0->MCR = 3;              // Match on MR0, reset on MR0, match on MR1
00035     LPC_TIM0->TCR = 0;              // Disable interrupt
00036 
00037     LPC_SC->PCONP |= (1 << 2);      // Power Ticker ON
00038     LPC_TIM1->MR0 = 1000000;
00039     LPC_TIM1->MCR = 1;
00040     LPC_TIM1->TCR = 1;              // Enable interrupt
00041 
00042     // Default start values
00043     this->moves_finished = false;
00044     this->reset_step_pins = false;
00045     this->debug = 0;
00046     this->has_axes = 0;
00047     this->set_frequency(0.001);
00048     this->set_reset_delay(100);
00049     this->last_duration = 0;
00050     for (int i = 0; i < 12; i++){
00051         this->active_motors[i] = NULL;
00052     }
00053     this->active_motor_bm = 0;
00054 
00055     NVIC_EnableIRQ(TIMER0_IRQn);     // Enable interrupt handler
00056     NVIC_EnableIRQ(TIMER1_IRQn);     // Enable interrupt handler
00057 }
00058 
00059 // Set the base stepping frequency
00060 void StepTicker::set_frequency( float frequency ){
00061     this->frequency = frequency;
00062     this->period = int(floor((SystemCoreClock/4)/frequency));  // SystemCoreClock/4 = Timer increments in a second
00063     LPC_TIM0->MR0 = this->period;
00064     if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
00065         LPC_TIM0->TCR = 3;  // Reset
00066         LPC_TIM0->TCR = 1;  // Reset
00067     }
00068 }
00069 
00070 // Set the reset delay
00071 void StepTicker::set_reset_delay( float seconds ){
00072     this->delay = int(floor(float(SystemCoreClock/4)*( seconds )));  // SystemCoreClock/4 = Timer increments in a second
00073     LPC_TIM1->MR0 = this->delay;
00074 }
00075 
00076 // Add a stepper motor object to our list of steppers we must take care of
00077 StepperMotor* StepTicker::add_stepper_motor(StepperMotor* stepper_motor){
00078     this->stepper_motors.push_back(stepper_motor);
00079     stepper_motor->step_ticker = this;
00080     this->has_axes = true;
00081     return stepper_motor;
00082 }
00083 
00084 // Call tick() on each active motor
00085 inline void StepTicker::tick(){
00086     _isr_context = true;
00087     int i;
00088     uint32_t bm = 1;
00089     // We iterate over each active motor 
00090     for (i = 0; i < 12; i++, bm <<= 1){
00091         if (this->active_motor_bm & bm){
00092             this->active_motors[i]->tick();
00093         }
00094     }
00095     _isr_context = false;
00096 }
00097 
00098 // Call signal_mode_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that
00099 // all tick()s are called before we do the move finishing
00100 void StepTicker::signal_moves_finished(){
00101     _isr_context = true;
00102 
00103     uint16_t bitmask = 1;
00104     for ( uint8_t motor = 0; motor < 12; motor++, bitmask <<= 1){
00105         if (this->active_motor_bm & bitmask){
00106             if(this->active_motors[motor]->is_move_finished){
00107                 this->active_motors[motor]->signal_move_finished();
00108                 if(this->active_motors[motor]->moving == false){
00109                     if (motor > 0){
00110                         motor--;
00111                         bitmask >>= 1;
00112                     }
00113                 }
00114             }
00115         }
00116     }
00117     this->moves_finished = false;
00118 
00119     _isr_context = false;
00120 }
00121 
00122 // Reset step pins on all active motors
00123 inline void StepTicker::reset_tick(){
00124     _isr_context = true;
00125 
00126     int i;
00127     uint32_t bm;
00128     for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
00129     {
00130         if (this->active_motor_bm & bm)
00131             this->active_motors[i]->unstep();
00132     }
00133 
00134     _isr_context = false;
00135 }
00136 
00137 extern "C" void TIMER1_IRQHandler (void){
00138     LPC_TIM1->IR |= 1 << 0;
00139     global_step_ticker->reset_tick();
00140 }
00141 
00142 // The actual interrupt handler where we do all the work
00143 extern "C" void TIMER0_IRQHandler (void){
00144 
00145     // Reset interrupt register
00146     LPC_TIM0->IR |= 1 << 0;
00147 
00148     // Step pins
00149     uint16_t bitmask = 1;
00150     for (uint8_t motor = 0; motor < 12; motor++, bitmask <<= 1){
00151         if (global_step_ticker->active_motor_bm & bitmask){
00152             global_step_ticker->active_motors[motor]->tick();
00153         }
00154     }
00155 
00156     // We may have set a pin on in this tick, now we start the timer to set it off
00157     if( global_step_ticker->reset_step_pins ){
00158         LPC_TIM1->TCR = 3;
00159         LPC_TIM1->TCR = 1;
00160         global_step_ticker->reset_step_pins = false;
00161     }else{
00162         // Nothing happened, nothing after this really matters
00163         // TODO : This could be a problem when we use Actuators instead of StepperMotors, because this flag is specific to step generation
00164         LPC_TIM0->MR0 = global_step_ticker->period;
00165         return;
00166     }
00167     
00168     // If a move finished in this tick, we have to tell the actuator to act accordingly
00169     if( global_step_ticker->moves_finished ){ 
00170  
00171         // Do not get out of here before everything is nice and tidy
00172         LPC_TIM0->MR0 = 20000000;
00173         
00174         global_step_ticker->signal_moves_finished();
00175 
00176         // If we went over the duration an interrupt is supposed to last, we have a problem
00177         // That can happen tipically when we change blocks, where more than usual computation is done
00178         // This can be OK, if we take notice of it, which we do now
00179         if( LPC_TIM0->TC > global_step_ticker->period ){ // TODO: remove the size condition
00180 
00181             uint32_t start_tc = LPC_TIM0->TC;
00182 
00183             // 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 )
00184             uint32_t ticks_to_skip = (  ( LPC_TIM0->TC + global_step_ticker->last_duration ) / global_step_ticker->period );
00185 
00186             // Next step is now to reduce this to how many steps we can *actually* skip
00187             uint32_t ticks_we_actually_can_skip = ticks_to_skip;
00188 
00189             int i;
00190             uint32_t bm;
00191             for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
00192             {
00193                 if (global_step_ticker->active_motor_bm & bm)
00194                     ticks_we_actually_can_skip =
00195                         min(ticks_we_actually_can_skip,
00196                             (uint32_t)((uint64_t)( (uint64_t)global_step_ticker->active_motors[i]->fx_ticks_per_step - (uint64_t)global_step_ticker->active_motors[i]->fx_counter ) >> 32)
00197                             );
00198             }
00199 
00200             // Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
00201             for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
00202             {
00203                 if (global_step_ticker->active_motor_bm & bm)
00204                     global_step_ticker->active_motors[i]->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32);
00205             }
00206 
00207             // 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 )
00208             LPC_TIM0->MR0 = ( ticks_to_skip + 1 ) * global_step_ticker->period;
00209 
00210             // This is so that we know how long this computation takes, and we can take it into account next time
00211             int difference = (int)(LPC_TIM0->TC) - (int)(start_tc);
00212             if( difference > 0 ){ global_step_ticker->last_duration = (uint32_t)difference; }
00213 
00214         }else{
00215             LPC_TIM0->MR0 = global_step_ticker->period;
00216         }
00217 
00218         while( LPC_TIM0->TC > LPC_TIM0->MR0 ){
00219             LPC_TIM0->MR0 += global_step_ticker->period;
00220         }
00221 
00222     }
00223 
00224 }
00225 
00226 
00227 // We make a list of steppers that want to be called so that we don't call them for nothing
00228 void StepTicker::add_motor_to_active_list(StepperMotor* motor)
00229 {
00230     uint32_t bm;
00231     int i;
00232     for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
00233     {
00234         if (this->active_motors[i] == motor)
00235         {
00236             this->active_motor_bm |= bm;
00237             if( this->active_motor_bm != 0 ){
00238                 LPC_TIM0->TCR = 1;               // Enable interrupt
00239             }
00240             return;
00241         }
00242         if (this->active_motors[i] == NULL)
00243         {
00244             this->active_motors[i] = motor;
00245             this->active_motor_bm |= bm;
00246             if( this->active_motor_bm != 0 ){
00247                 LPC_TIM0->TCR = 1;               // Enable interrupt
00248             }
00249             return;
00250         }
00251     }
00252     return;
00253 }
00254 
00255 // Remove a stepper from the list of active motors
00256 void StepTicker::remove_motor_from_active_list(StepperMotor* motor)
00257 {
00258     uint32_t bm; int i;
00259     for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
00260     {
00261         if (this->active_motors[i] == motor)
00262         {
00263             this->active_motor_bm &= ~bm;
00264             // If we have no motor to work on, disable the whole interrupt
00265             if( this->active_motor_bm == 0 ){
00266                 LPC_TIM0->TCR = 0;               // Disable interrupt
00267             }
00268             return;
00269         }
00270     }
00271 }