File content as of revision 0:c680385286e3:

#include "mbed.h"
#include "SimpleRCservos.h"

namespace AjK {

void 
SimpleRCservos::init(uint32_t duty)
{
    _duty = duty;
    
    for (int i = 0; i < NumOfServos; i++) {
        _limitMin[i] = -1.0;
        _limitMax[i] = +1.0;
        
        // The following would give 1ms to 2ms
        setRange((Servo)i, 1000, 2000);
        
        // The following would give 900us to 2.1ms as required Hitec HS-322HD servo module.        
        //setRange((Servo)i, 900, 2100);
        
        // The following would give 600us to 2.4ms
        //setRange((Servo)i, 600, 2400);
        
        _mid[i] = duty * 0.075f; // 1.5ms
        
    }
}

void
SimpleRCservos::setDuty(uint32_t duty)
{
    // Ensure powered up (default is 1)  
    LPC_SC->PCONP |= (1UL << 6);  
        
    // CCLK/4 = 24MHz
    LPC_SC->PCLKSEL0 &= ~(3UL << 12);
    
    // Reset.
    LPC_PWM1->TCR   = 2; 
    
    _duty = duty;
    
    LPC_PWM1->PR    = 0;
    LPC_PWM1->MR0   = _duty;
    LPC_PWM1->MR1   = 0;
    LPC_PWM1->MR2   = 0;
    LPC_PWM1->MR3   = 0;
    LPC_PWM1->MR4   = 0;
    LPC_PWM1->MR5   = 0;
    LPC_PWM1->MR6   = 0;
        
    LPC_PWM1->MCR   = 2; // MR0 resets TC.
        
    LPC_PWM1->TCR   = 9; // Enable.        
}

double
SimpleRCservos::position(Servo ch, double pos)
{
    pos = limit(ch, pos);    
    double a = pos - _limitMin[ch];
    double set = a / (_limitMax[ch] - _limitMin[ch]);
    uint32_t ps = _msMax[ch] * set;
    setMRx(ch, ps + _msMin[ch]);   
    return pos; 
}

void
SimpleRCservos::neutral(Servo ch)
{
    setMRx(ch, _mid[ch]);   
}

double 
SimpleRCservos::limit(Servo ch, double pos)
{
    if (pos >= _limitMin[ch] && pos <= _limitMax[ch]) return pos;
    if (pos < _limitMin[ch]) return _limitMin[ch];
    if (pos > _limitMax[ch]) return _limitMax[ch];
    return 0.0; // Keep the compiler happy.
}

void 
SimpleRCservos::setMRx(Servo ch, uint32_t value) 
{
    switch(ch) {
        case Servo1: LPC_PWM1->MR1 = value; break;
        case Servo2: LPC_PWM1->MR2 = value; break;
        case Servo3: LPC_PWM1->MR3 = value; break;
        case Servo4: LPC_PWM1->MR4 = value; break;
        case Servo5: LPC_PWM1->MR5 = value; break;
        case Servo6: LPC_PWM1->MR6 = value; break;        
    }
    LPC_PWM1->LER |= (1UL << ch);
}

void
SimpleRCservos::enable(Servo ch)
{
    switch(ch) {
        case 1: enable1(); break;
        case 2: enable2(); break;
        case 3: enable3(); break;
        case 4: enable4(); break;
        case 5: enable5(); break;
        case 6: enable6(); break;
    }
}

void
SimpleRCservos::enable1(PinName pin)
{
    setMRx(Servo1, _mid[1]);
    
    switch(pin) {
        case P2_0:    
            LPC_PINCON->PINSEL4 &= ~(3UL << 0); // Mbed p26 P2.0 clr bits
            LPC_PINCON->PINSEL4 |=  (1UL << 0); // Mbed p26 P2.0 set bits
            break;
        case P1_18: // Mbed LED1
            LPC_PINCON->PINSEL3 &= ~(3UL << 4); // Mbed LED2 P1.18 clr bits
            LPC_PINCON->PINSEL3 |=  (2UL << 4); // Mbed LED2 P1.18 set bits
            break;
    }
    
    LPC_PWM1->PCR |= (1UL << 9);
}

void
SimpleRCservos::enable2(PinName pin)
{
    setMRx(Servo2, _mid[2]);
    
    switch(pin) {
        case P2_1:
            LPC_PINCON->PINSEL4 &= ~(3UL << 2); // Mbed p25 P2.1 clr bits
            LPC_PINCON->PINSEL4 |=  (1UL << 2); // Mbed p25 P2.1 set bits
            break;
        case P1_20: // Mbed LED2
            LPC_PINCON->PINSEL3 &= ~(3UL << 8); // Mbed LED2 P1.20 clr bits
            LPC_PINCON->PINSEL3 |=  (2UL << 8); // Mbed LED2 P1.20 set bits
            break;
    }
    
    LPC_PWM1->PCR |= (1UL << 10);
}

void
SimpleRCservos::enable3(PinName pin)
{
    setMRx(Servo3, _mid[3]);
    
    switch(pin) {
        case P2_2:
            LPC_PINCON->PINSEL4 &= ~(3UL << 4); // Mbed p24 P2.2 clr bits
            LPC_PINCON->PINSEL4 |=  (1UL << 4); // Mbed p24 P2.2 set bits
            break;
        case P1_21: // Mbed LED3
            LPC_PINCON->PINSEL3 &= ~(3UL << 10); // Mbed LED3 P1.21 clr bits
            LPC_PINCON->PINSEL3 |=  (2UL << 10); // Mbed LED3 P1.21 set bits
            break;
    }
    
    LPC_PWM1->PCR |= (1UL << 11);
}

void
SimpleRCservos::enable4(PinName pin)
{
    setMRx(Servo4, _mid[4]);
    
    switch(pin) {
        case P2_3:
            LPC_PINCON->PINSEL4 &= ~(3UL << 6); // Mbed p23 P2.3 clr bits
            LPC_PINCON->PINSEL4 |=  (1UL << 6); // Mbed p23 P2.3 set bits
            break;
        case P1_23: // Mbed LED4
            LPC_PINCON->PINSEL3 &= ~(3UL << 14); // Mbed LED4 P1.23 clr bits
            LPC_PINCON->PINSEL3 |=  (2UL << 14); // Mbed LED4 P1.23 set bits
            break;
    }
    
    LPC_PWM1->PCR |= (1UL << 12);
}

void
SimpleRCservos::enable5(PinName pin)
{
    setMRx(Servo5, _mid[5]);
    
    switch(pin) {
        case P2_4: // Mbed p22
            LPC_PINCON->PINSEL4 &= ~(3UL << 8); // Mbed p22 P2.4 clr bits
            LPC_PINCON->PINSEL4 |=  (1UL << 8); // Mbed p22 P2.4 set bits
            break;
        case P1_24:            
            LPC_PINCON->PINSEL3 &= ~(3UL << 16); // P1.24 clr bits
            LPC_PINCON->PINSEL3 |=  (2UL << 16); // P1.24 set bits
            break;
    }
    
    LPC_PWM1->PCR |= (1UL << 13);
}

void
SimpleRCservos::enable6(PinName pin)
{
    setMRx(Servo6, _mid[6]);
    
    switch(pin) {
        case P2_5: // Mbed p21
            LPC_PINCON->PINSEL4 &= ~(3UL << 10); // Mbed p21 P2.5 clr bits
            LPC_PINCON->PINSEL4 |=  (1UL << 10); // Mbed p21 P2.5 set bits
            break;
        case P1_26:
            LPC_PINCON->PINSEL3 &= ~(3UL << 20); // P1.26 clr bits
            LPC_PINCON->PINSEL3 |=  (2UL << 20); // P1.26 set bits
            break;
    }
    
    LPC_PWM1->PCR |= (1UL << 14);
}

}; // namespace AjK ends.