SimpleSteppers.cpp

00001 /*
00002     Copyright (c) 2010 Andy Kirkham
00003  
00004     Permission is hereby granted, free of charge, to any person obtaining a copy
00005     of this software and associated documentation files (the "Software"), to deal
00006     in the Software without restriction, including without limitation the rights
00007     to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008     copies of the Software, and to permit persons to whom the Software is
00009     furnished to do so, subject to the following conditions:
00010  
00011     The above copyright notice and this permission notice shall be included in
00012     all copies or substantial portions of the Software.
00013  
00014     THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00015     IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00016     FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00017     AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00018     LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00019     OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00020     THE SOFTWARE.
00021 */
00022 
00023 #include <math.h>
00024 #include "SimpleSteppers.h"
00025 #include "IOmacros.h"
00026 
00027 #define _PMR    *((uint32_t *)_pMR)
00028 
00029 namespace AjK {
00030 
00031 extern SimpleStepperController __simpleStepperController;
00032 
00033 void TIMER2_IRQHandler(void);
00034 
00035 SimpleStepper::SimpleStepper(PinName pulse, SimpleStepperOutput *direction)
00036 {
00037     _simpleStepperController = &__simpleStepperController;
00038     _pulse     = pulse;
00039     _direction = direction;
00040     init();
00041 }
00042 
00043 SimpleStepper::SimpleStepper(SimpleStepperController *con, PinName pulse, SimpleStepperOutput *direction)
00044 {
00045     _simpleStepperController = con;
00046     _pulse     = pulse;
00047     _direction = direction;
00048     init();
00049 }
00050 
00051 void
00052 SimpleStepper::init(void)
00053 {
00054     _pulseCounter = 0;
00055     _pulseWrapPos = 0;
00056     _pulseWrapNeg = 0;
00057     
00058     _maintainPositionData = true; 
00059     
00060     _steps_per_second = 0;
00061     
00062     setPulseSense(1);
00063     setDirectionSense(1);
00064     
00065     switch (_pulse) {
00066         case P0_6: // Mbed p8
00067             _pulseMAT = MAT2_0;
00068             _operShift = 4;
00069             _pMR = 0x40090018; // T2MR0
00070             _match_shift = 0;
00071             _EMmask = 1;
00072             _EMCshift = 4;
00073             _simpleStepperController->_stepper[0] = this;
00074             if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask; 
00075             LPC_PINCON->PINSEL0 |= (3UL << 12);
00076             break;
00077         case P0_7: // Mbed p7
00078             _pulseMAT = MAT2_1;
00079             _operShift = 6;
00080             _pMR = 0x4009001C; // T2MR1
00081             _match_shift = 3;
00082             _EMmask = 2;
00083             _EMCshift = 6;
00084             _simpleStepperController->_stepper[1] = this;
00085             if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask; 
00086             LPC_PINCON->PINSEL0 |= (3UL << 14);
00087             break;        
00088         case P0_8: // Mbed p6
00089             _pulseMAT = MAT2_2;
00090             _operShift = 8;
00091             _pMR = 0x40090020; // T2MR2
00092             _match_shift = 6;
00093             _EMmask = 4;
00094             _EMCshift = 8;
00095             _simpleStepperController->_stepper[2] = this;
00096             if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask; 
00097             LPC_PINCON->PINSEL0 |= (3UL << 16);
00098             break;        
00099         case P0_9: // Mbed p5
00100             _pulseMAT = MAT2_3;
00101             _operShift = 10;
00102             _pMR = 0x40090024; // T2MR3
00103             _match_shift = 9;
00104             _EMmask = 8;
00105             _EMCshift = 10;
00106             _simpleStepperController->_stepper[3] = this;
00107             if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask; 
00108             LPC_PINCON->PINSEL0 |= (3UL << 18);
00109             break;
00110         default:
00111             while(1); /* ToDo, proper error reporting. */
00112     } 
00113     
00114     
00115     // The default pulse length. 5 * (1/SIMPLESTEPPER_F) = 100us.
00116     _pulseLen = 5;
00117     
00118     // Initial state machine conditions.
00119     _pulseState = PulseIdle;    
00120     _stepperState = Stopped;       
00121 }
00122 
00123 void
00124 SimpleStepper::setInterval(int interval, uint32_t raw)
00125 {    
00126     
00127     if (interval == 0) { setSpeed((int)0); }
00128     else {
00129         int n = interval;
00130         if (n < 0) n *= -1;
00131         if (_direction) {
00132             if (_directionSense) {
00133                 if (interval >= 0.0) _direction->write(1);
00134                 if (interval <  0.0) _direction->write(0);
00135             }
00136             else {
00137                 if (interval >= 0.0) _direction->write(0);
00138                 if (interval <  0.0) _direction->write(1);        
00139             }
00140         }        
00141         _commandSpeed = n - _pulseLen; 
00142         _pulseState = PulseAssert;
00143         _stepperState = ConstantSpeed;
00144         if (raw) {
00145             _PMR = raw + _commandSpeed;
00146         }
00147         else {
00148             _PMR = LPC_TIM2->TC + _commandSpeed;
00149         }                
00150         if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
00151         LPC_TIM2->MCR &= ~(7UL << _match_shift);
00152         LPC_TIM2->MCR |=  (1UL << _match_shift);
00153     }
00154 }
00155 
00156 void
00157 SimpleStepper::setSpeed(double steps_per_second, uint32_t raw)
00158 {    
00159     int i = 0;
00160     
00161     if (steps_per_second == 0.0) { setSpeed(i); }
00162     else {
00163         double fractpart, intpart;
00164         fractpart = modf (steps_per_second , &intpart);
00165         if (fractpart >= 0.50) intpart++;
00166         double n = intpart;
00167         if (n < 0.0) n *= -1.0;
00168         if (_direction) {
00169             if (_directionSense) {
00170                 if (steps_per_second >= 0.0) _direction->write(1);
00171                 if (steps_per_second <  0.0) _direction->write(0);
00172             }
00173             else {
00174                 if (steps_per_second >= 0.0) _direction->write(0);
00175                 if (steps_per_second <  0.0) _direction->write(1);        
00176             }
00177         }        
00178         _commandSpeed = (int)(((double)SIMPLESTEPPER_F / n)) - _pulseLen; 
00179         _pulseState = PulseAssert;
00180         _stepperState = ConstantSpeed;
00181         if (raw) {
00182             _PMR = raw + _commandSpeed;
00183         }
00184         else {
00185             _PMR = LPC_TIM2->TC + _commandSpeed;
00186         }                
00187         if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
00188         LPC_TIM2->MCR &= ~(7UL << _match_shift);
00189         LPC_TIM2->MCR |=  (1UL << _match_shift);
00190     }
00191 }
00192 
00193 void
00194 SimpleStepper::setSpeed(int steps_per_second, uint32_t raw)
00195 {
00196     if (steps_per_second == 0) {
00197         LPC_TIM2->EMR |=  (NothingOnMatch << _EMCshift);
00198         LPC_TIM2->MCR &= ~(7UL << _match_shift);
00199         if (_pulseSense) LPC_TIM2->EMR |= _EMmask; else LPC_TIM2->EMR &= ~_EMmask; 
00200         _stepperState = Stopped;
00201         _pulseState = PulseIdle;
00202         _commandSpeed = 0;
00203         _steps_per_second = 0;
00204     }
00205     else {
00206         _steps_per_second = steps_per_second;
00207         int n = steps_per_second;
00208         if (n < 0) n *= -1;
00209         if (_direction) {
00210             if (_directionSense) {
00211                 if (steps_per_second >= 0) _direction->write(1);
00212                 if (steps_per_second <  0) _direction->write(0);
00213             }
00214             else {
00215                 if (steps_per_second >= 0) _direction->write(0);
00216                 if (steps_per_second <  0) _direction->write(1);        
00217             }
00218         }        
00219         _commandSpeed = (SIMPLESTEPPER_F / n) - _pulseLen;        
00220         _pulseState = PulseAssert;
00221         _stepperState = ConstantSpeed;        
00222         if (raw) {
00223             _PMR = raw + _commandSpeed;
00224         }
00225         else {
00226             _PMR = LPC_TIM2->TC + _commandSpeed;
00227         }
00228         if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
00229         LPC_TIM2->MCR &= ~(7UL << _match_shift);
00230         LPC_TIM2->MCR |=  (1UL << _match_shift);
00231     }
00232 }
00233 
00234 void 
00235 SimpleStepper::next_step(void) 
00236 {    
00237     _PMR += _commandSpeed;
00238         
00239     switch(_pulseSense) {
00240         case 1:
00241             LPC_TIM2->EMR &= ~(3UL << _EMCshift); 
00242             LPC_TIM2->EMR |=  (SetOnMatch << _EMCshift); 
00243             break;
00244         case 0:
00245             LPC_TIM2->EMR &= ~(3UL << _EMCshift); 
00246             LPC_TIM2->EMR |=  (ClrOnMatch << _EMCshift); 
00247             break;
00248     }
00249     
00250     // Next IRQ will automatically assert the pulse.
00251     _pulseState = PulseAssert;
00252 }
00253 
00254 void 
00255 SimpleStepper::isr(uint32_t ir) 
00256 {
00257     // Test to see if this interrupt is for us.
00258     if (ir & _EMmask) { 
00259     
00260         if (_stepperState == Stopped) {
00261             if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask; 
00262             return;
00263         }
00264        
00265         switch(_pulseState) {
00266             case PulseIdle:
00267                 if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
00268                 _currentMatch = _PMR; // Store the current match value for profile().
00269                 next_step();
00270                 break;
00271                 
00272             case PulseAssert: // Pulse has just been asserted on MAT2_x. 
00273                 _PMR += _pulseLen; // We now program for the deassert IRQ to occur at the required time.            
00274                 LPC_TIM2->EMR &= ~(ToggleOnMatch << _EMCshift); // At next IRQ we want to switch the pulse off...
00275                 LPC_TIM2->EMR |=  (ToggleOnMatch << _EMCshift); // ... we do this by toggling it.
00276                 _pulseState = PulseDeassert; // Set state for next interrupt.
00277                 
00278                 if (_maintainPositionData) {
00279                     if (_directionSense) {
00280                         if (_commandSpeed > 0) {
00281                             _pulseCounter++;
00282                             if (_pulseWrapPos && _pulseCounter >= _pulseWrapPos) {
00283                                 _pulseCounter = _pulseWrapNeg + 1;
00284                             }
00285                         }                        
00286                         if (_commandSpeed < 0) {
00287                             _pulseCounter--;
00288                             if (_pulseWrapNeg && _pulseCounter <= _pulseWrapNeg) {
00289                                 _pulseCounter = _pulseWrapPos - 1;
00290                             }
00291                         }
00292                     }
00293                     else {
00294                         if (_commandSpeed > 0) {
00295                             _pulseCounter--;
00296                             if (_pulseWrapNeg && _pulseCounter <= _pulseWrapNeg) {
00297                                 _pulseCounter = _pulseWrapPos - 1;
00298                             }
00299                         }                        
00300                         if (_commandSpeed < 0) {
00301                             _pulseCounter++;
00302                             if (_pulseWrapPos && _pulseCounter >= _pulseWrapPos) {
00303                                 _pulseCounter = _pulseWrapNeg + 1;
00304                             }
00305                         }
00306                     }
00307                 }
00308                 break;
00309                 
00310             case PulseDeassert: // Pulse has just been deasserted on MAT2_x.
00311                 LPC_TIM2->EMR &= ~(NothingOnMatch << _EMCshift); // No further action...
00312                 LPC_TIM2->EMR |=  (NothingOnMatch << _EMCshift); // ... just in case.
00313                 _pulseState = PulseIdle; // Set state for next interrupt (profile() may change this).
00314                 _currentMatch = _PMR; // Store the current match value for profile().
00315                 next_step();
00316                 break;
00317         }    
00318     }
00319 }
00320 
00321 // **************************************************
00322 // * Controller class SimpleStepperController code. *
00323 // **************************************************
00324 
00325 SimpleStepperController::SimpleStepperController()
00326 {
00327     for (int i = 0; i < 4; i++) _stepper[i] = 0;
00328     
00329     uint32_t pr = (uint32_t)(SystemCoreClock / 8 / SIMPLESTEPPER_F);
00330     
00331     LPC_SC->PCONP    |= (1UL << 22); // TIM2 On
00332     LPC_SC->PCLKSEL1 |= (3UL << 12); // CCLK/8 = 12MHz
00333     LPC_TIM2->PR      = pr - 1;      // TC clocks at SIMPLESTEPPER_F hz.
00334     LPC_TIM2->MR0     = 0;
00335     LPC_TIM2->MR1     = 0;
00336     LPC_TIM2->MR2     = 0;
00337     LPC_TIM2->MR3     = 0;
00338     LPC_TIM2->MCR     = 0;          
00339     
00340     // Enable interrupts
00341     NVIC_EnableIRQ(TIMER2_IRQn);  
00342     
00343     // Start timer operations.
00344     LPC_TIM2->TCR = 2; // reset 
00345     LPC_TIM2->TCR = 1; // enable
00346 }
00347 
00348 SimpleStepperController::~SimpleStepperController()
00349 {
00350     NVIC_DisableIRQ(TIMER2_IRQn);    
00351     LPC_SC->PCONP &= ~(1UL << 22); // TIM2 Off.
00352 }
00353 
00354 void
00355 SimpleStepperController::isr(void)
00356 {
00357     uint32_t ir = LPC_TIM2->IR & 0xF;
00358     if (_stepper[0] && ir & 1) _stepper[0]->isr(ir);
00359     if (_stepper[1] && ir & 2) _stepper[1]->isr(ir);
00360     if (_stepper[2] && ir & 4) _stepper[2]->isr(ir);
00361     if (_stepper[3] && ir & 8) _stepper[3]->isr(ir);
00362 }
00363 
00364 }; // namespace AjK ends.