File content as of revision 4:7b7940df7865:

/*
    Copyright (c) 2010 Andy Kirkham
 
    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:
 
    The above copyright notice and this permission notice shall be included in
    all copies or substantial portions of the Software.
 
    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
    THE SOFTWARE.
*/

#include <math.h>
#include "SimpleSteppers.h"
#include "IOmacros.h"

#define _PMR    *((uint32_t *)_pMR)

namespace AjK {

extern SimpleStepperController __simpleStepperController;

void TIMER2_IRQHandler(void);

SimpleStepper::SimpleStepper(PinName pulse, SimpleStepperOutput *direction)
{
    _simpleStepperController = &__simpleStepperController;
    _pulse     = pulse;
    _direction = direction;
    init();
}

SimpleStepper::SimpleStepper(SimpleStepperController *con, PinName pulse, SimpleStepperOutput *direction)
{
    _simpleStepperController = con;
    _pulse     = pulse;
    _direction = direction;
    init();
}

void
SimpleStepper::init(void)
{
    _pulseCounter = 0;
    _pulseWrapPos = 0;
    _pulseWrapNeg = 0;
    
    _maintainPositionData = true; 
    
    _steps_per_second = 0;
    
    setPulseSense(1);
    setDirectionSense(1);
    
    switch (_pulse) {
        case P0_6: // Mbed p8
            _pulseMAT = MAT2_0;
            _operShift = 4;
            _pMR = 0x40090018; // T2MR0
            _match_shift = 0;
            _EMmask = 1;
            _EMCshift = 4;
            _simpleStepperController->_stepper[0] = this;
            if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask; 
            LPC_PINCON->PINSEL0 |= (3UL << 12);
            break;
        case P0_7: // Mbed p7
            _pulseMAT = MAT2_1;
            _operShift = 6;
            _pMR = 0x4009001C; // T2MR1
            _match_shift = 3;
            _EMmask = 2;
            _EMCshift = 6;
            _simpleStepperController->_stepper[1] = this;
            if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask; 
            LPC_PINCON->PINSEL0 |= (3UL << 14);
            break;        
        case P0_8: // Mbed p6
            _pulseMAT = MAT2_2;
            _operShift = 8;
            _pMR = 0x40090020; // T2MR2
            _match_shift = 6;
            _EMmask = 4;
            _EMCshift = 8;
            _simpleStepperController->_stepper[2] = this;
            if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask; 
            LPC_PINCON->PINSEL0 |= (3UL << 16);
            break;        
        case P0_9: // Mbed p5
            _pulseMAT = MAT2_3;
            _operShift = 10;
            _pMR = 0x40090024; // T2MR3
            _match_shift = 9;
            _EMmask = 8;
            _EMCshift = 10;
            _simpleStepperController->_stepper[3] = this;
            if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask; 
            LPC_PINCON->PINSEL0 |= (3UL << 18);
            break;
        default:
            while(1); /* ToDo, proper error reporting. */
    } 
    
    
    // The default pulse length. 5 * (1/SIMPLESTEPPER_F) = 100us.
    _pulseLen = 5;
    
    // Initial state machine conditions.
    _pulseState = PulseIdle;    
    _stepperState = Stopped;       
}

void
SimpleStepper::setInterval(int interval, uint32_t raw)
{    
    
    if (interval == 0) { setSpeed((int)0); }
    else {
        int n = interval;
        if (n < 0) n *= -1;
        if (_direction) {
            if (_directionSense) {
                if (interval >= 0.0) _direction->write(1);
                if (interval <  0.0) _direction->write(0);
            }
            else {
                if (interval >= 0.0) _direction->write(0);
                if (interval <  0.0) _direction->write(1);        
            }
        }        
        _commandSpeed = n - _pulseLen; 
        _pulseState = PulseAssert;
        _stepperState = ConstantSpeed;
        if (raw) {
            _PMR = raw + _commandSpeed;
        }
        else {
            _PMR = LPC_TIM2->TC + _commandSpeed;
        }                
        if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
        LPC_TIM2->MCR &= ~(7UL << _match_shift);
        LPC_TIM2->MCR |=  (1UL << _match_shift);
    }
}

void
SimpleStepper::setSpeed(double steps_per_second, uint32_t raw)
{    
    int i = 0;
    
    if (steps_per_second == 0.0) { setSpeed(i); }
    else {
        double fractpart, intpart;
        fractpart = modf (steps_per_second , &intpart);
        if (fractpart >= 0.50) intpart++;
        double n = intpart;
        if (n < 0.0) n *= -1.0;
        if (_direction) {
            if (_directionSense) {
                if (steps_per_second >= 0.0) _direction->write(1);
                if (steps_per_second <  0.0) _direction->write(0);
            }
            else {
                if (steps_per_second >= 0.0) _direction->write(0);
                if (steps_per_second <  0.0) _direction->write(1);        
            }
        }        
        _commandSpeed = (int)(((double)SIMPLESTEPPER_F / n)) - _pulseLen; 
        _pulseState = PulseAssert;
        _stepperState = ConstantSpeed;
        if (raw) {
            _PMR = raw + _commandSpeed;
        }
        else {
            _PMR = LPC_TIM2->TC + _commandSpeed;
        }                
        if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
        LPC_TIM2->MCR &= ~(7UL << _match_shift);
        LPC_TIM2->MCR |=  (1UL << _match_shift);
    }
}

void
SimpleStepper::setSpeed(int steps_per_second, uint32_t raw)
{
    if (steps_per_second == 0) {
        LPC_TIM2->EMR |=  (NothingOnMatch << _EMCshift);
        LPC_TIM2->MCR &= ~(7UL << _match_shift);
        if (_pulseSense) LPC_TIM2->EMR |= _EMmask; else LPC_TIM2->EMR &= ~_EMmask; 
        _stepperState = Stopped;
        _pulseState = PulseIdle;
        _commandSpeed = 0;
        _steps_per_second = 0;
    }
    else {
        _steps_per_second = steps_per_second;
        int n = steps_per_second;
        if (n < 0) n *= -1;
        if (_direction) {
            if (_directionSense) {
                if (steps_per_second >= 0) _direction->write(1);
                if (steps_per_second <  0) _direction->write(0);
            }
            else {
                if (steps_per_second >= 0) _direction->write(0);
                if (steps_per_second <  0) _direction->write(1);        
            }
        }        
        _commandSpeed = (SIMPLESTEPPER_F / n) - _pulseLen;        
        _pulseState = PulseAssert;
        _stepperState = ConstantSpeed;        
        if (raw) {
            _PMR = raw + _commandSpeed;
        }
        else {
            _PMR = LPC_TIM2->TC + _commandSpeed;
        }
        if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
        LPC_TIM2->MCR &= ~(7UL << _match_shift);
        LPC_TIM2->MCR |=  (1UL << _match_shift);
    }
}

void 
SimpleStepper::next_step(void) 
{    
    _PMR += _commandSpeed;
        
    switch(_pulseSense) {
        case 1:
            LPC_TIM2->EMR &= ~(3UL << _EMCshift); 
            LPC_TIM2->EMR |=  (SetOnMatch << _EMCshift); 
            break;
        case 0:
            LPC_TIM2->EMR &= ~(3UL << _EMCshift); 
            LPC_TIM2->EMR |=  (ClrOnMatch << _EMCshift); 
            break;
    }
    
    // Next IRQ will automatically assert the pulse.
    _pulseState = PulseAssert;
}

void 
SimpleStepper::isr(uint32_t ir) 
{
    // Test to see if this interrupt is for us.
    if (ir & _EMmask) { 
    
        if (_stepperState == Stopped) {
            if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask; 
            return;
        }
       
        switch(_pulseState) {
            case PulseIdle:
                if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
                _currentMatch = _PMR; // Store the current match value for profile().
                next_step();
                break;
                
            case PulseAssert: // Pulse has just been asserted on MAT2_x. 
                _PMR += _pulseLen; // We now program for the deassert IRQ to occur at the required time.            
                LPC_TIM2->EMR &= ~(ToggleOnMatch << _EMCshift); // At next IRQ we want to switch the pulse off...
                LPC_TIM2->EMR |=  (ToggleOnMatch << _EMCshift); // ... we do this by toggling it.
                _pulseState = PulseDeassert; // Set state for next interrupt.
                
                if (_maintainPositionData) {
                    if (_directionSense) {
                        if (_commandSpeed > 0) {
                            _pulseCounter++;
                            if (_pulseWrapPos && _pulseCounter >= _pulseWrapPos) {
                                _pulseCounter = _pulseWrapNeg + 1;
                            }
                        }                        
                        if (_commandSpeed < 0) {
                            _pulseCounter--;
                            if (_pulseWrapNeg && _pulseCounter <= _pulseWrapNeg) {
                                _pulseCounter = _pulseWrapPos - 1;
                            }
                        }
                    }
                    else {
                        if (_commandSpeed > 0) {
                            _pulseCounter--;
                            if (_pulseWrapNeg && _pulseCounter <= _pulseWrapNeg) {
                                _pulseCounter = _pulseWrapPos - 1;
                            }
                        }                        
                        if (_commandSpeed < 0) {
                            _pulseCounter++;
                            if (_pulseWrapPos && _pulseCounter >= _pulseWrapPos) {
                                _pulseCounter = _pulseWrapNeg + 1;
                            }
                        }
                    }
                }
                break;
                
            case PulseDeassert: // Pulse has just been deasserted on MAT2_x.
                LPC_TIM2->EMR &= ~(NothingOnMatch << _EMCshift); // No further action...
                LPC_TIM2->EMR |=  (NothingOnMatch << _EMCshift); // ... just in case.
                _pulseState = PulseIdle; // Set state for next interrupt (profile() may change this).
                _currentMatch = _PMR; // Store the current match value for profile().
                next_step();
                break;
        }    
    }
}

// **************************************************
// * Controller class SimpleStepperController code. *
// **************************************************

SimpleStepperController::SimpleStepperController()
{
    for (int i = 0; i < 4; i++) _stepper[i] = 0;
    
    uint32_t pr = (uint32_t)(SystemCoreClock / 8 / SIMPLESTEPPER_F);
    
    LPC_SC->PCONP    |= (1UL << 22); // TIM2 On
    LPC_SC->PCLKSEL1 |= (3UL << 12); // CCLK/8 = 12MHz
    LPC_TIM2->PR      = pr - 1;      // TC clocks at SIMPLESTEPPER_F hz.
    LPC_TIM2->MR0     = 0;
    LPC_TIM2->MR1     = 0;
    LPC_TIM2->MR2     = 0;
    LPC_TIM2->MR3     = 0;
    LPC_TIM2->MCR     = 0;          
    
    // Enable interrupts
    NVIC_EnableIRQ(TIMER2_IRQn);  
    
    // Start timer operations.
    LPC_TIM2->TCR = 2; // reset 
    LPC_TIM2->TCR = 1; // enable
}

SimpleStepperController::~SimpleStepperController()
{
    NVIC_DisableIRQ(TIMER2_IRQn);    
    LPC_SC->PCONP &= ~(1UL << 22); // TIM2 Off.
}

void
SimpleStepperController::isr(void)
{
    uint32_t ir = LPC_TIM2->IR & 0xF;
    if (_stepper[0] && ir & 1) _stepper[0]->isr(ir);
    if (_stepper[1] && ir & 2) _stepper[1]->isr(ir);
    if (_stepper[2] && ir & 4) _stepper[2]->isr(ir);
    if (_stepper[3] && ir & 8) _stepper[3]->isr(ir);
}

}; // namespace AjK ends.