Diff: src/SimpleRCservos.cpp

Revision:

0:c680385286e3

Child:

2:d6a018232650

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/SimpleRCservos.cpp	Mon May 02 18:38:45 2011 +0000
@@ -0,0 +1,226 @@
+
+
+#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.