SimpleRCservos.cpp

00001 /*
00002     Copyright (c) 2011 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 
00024 #include "mbed.h"
00025 #include "SimpleRCservos.h"
00026 
00027 namespace AjK {
00028 
00029 void 
00030 SimpleRCservos::init(uint32_t duty)
00031 {
00032     _duty = duty;
00033     
00034     for (int i = 0; i < NumOfServos; i++) {
00035         _limitMin[i] = -1.0;
00036         _limitMax[i] = +1.0;
00037         
00038         // The following would give 1ms to 2ms
00039         setRange((Servo)i, 1000, 2000);
00040         
00041         // The following would give 900us to 2.1ms as required Hitec HS-322HD servo module.        
00042         //setRange((Servo)i, 900, 2100);
00043         
00044         // The following would give 600us to 2.4ms
00045         //setRange((Servo)i, 600, 2400);
00046         
00047         _mid[i] = duty * 0.075f; // 1.5ms
00048         
00049     }
00050 }
00051 
00052 void
00053 SimpleRCservos::setDuty(uint32_t duty)
00054 {
00055     // Ensure powered up (default is 1)  
00056     LPC_SC->PCONP |= (1UL << 6);  
00057         
00058     // CCLK/4 = 24MHz
00059     LPC_SC->PCLKSEL0 &= ~(3UL << 12);
00060     
00061     // Reset.
00062     LPC_PWM1->TCR   = 2; 
00063     
00064     _duty = duty;
00065     
00066     LPC_PWM1->PR    = 0;
00067     LPC_PWM1->MR0   = _duty;
00068     LPC_PWM1->MR1   = 0;
00069     LPC_PWM1->MR2   = 0;
00070     LPC_PWM1->MR3   = 0;
00071     LPC_PWM1->MR4   = 0;
00072     LPC_PWM1->MR5   = 0;
00073     LPC_PWM1->MR6   = 0;
00074         
00075     LPC_PWM1->MCR   = 2; // MR0 resets TC.
00076         
00077     LPC_PWM1->TCR   = 9; // Enable.        
00078 }
00079 
00080 double
00081 SimpleRCservos::position(Servo ch, double pos)
00082 {
00083     pos = limit(ch, pos);    
00084     double a = pos - _limitMin[ch];
00085     double set = a / (_limitMax[ch] - _limitMin[ch]);
00086     uint32_t ps = _msMax[ch] * set;
00087     setMRx(ch, ps + _msMin[ch]);   
00088     return pos; 
00089 }
00090 
00091 void
00092 SimpleRCservos::neutral(Servo ch)
00093 {
00094     setMRx(ch, _mid[ch]);   
00095 }
00096 
00097 double 
00098 SimpleRCservos::limit(Servo ch, double pos)
00099 {
00100     if (pos >= _limitMin[ch] && pos <= _limitMax[ch]) return pos;
00101     if (pos < _limitMin[ch]) return _limitMin[ch];
00102     if (pos > _limitMax[ch]) return _limitMax[ch];
00103     return 0.0; // Keep the compiler happy.
00104 }
00105 
00106 void 
00107 SimpleRCservos::setMRx(Servo ch, uint32_t value) 
00108 {
00109     switch(ch) {
00110         case Servo1: LPC_PWM1->MR1 = value; break;
00111         case Servo2: LPC_PWM1->MR2 = value; break;
00112         case Servo3: LPC_PWM1->MR3 = value; break;
00113         case Servo4: LPC_PWM1->MR4 = value; break;
00114         case Servo5: LPC_PWM1->MR5 = value; break;
00115         case Servo6: LPC_PWM1->MR6 = value; break;        
00116     }
00117     LPC_PWM1->LER |= (1UL << ch);
00118 }
00119 
00120 void
00121 SimpleRCservos::enable(Servo ch)
00122 {
00123     switch(ch) {
00124         case 1: enable1(); break;
00125         case 2: enable2(); break;
00126         case 3: enable3(); break;
00127         case 4: enable4(); break;
00128         case 5: enable5(); break;
00129         case 6: enable6(); break;
00130     }
00131 }
00132 
00133 void
00134 SimpleRCservos::enable1(PinName pin)
00135 {
00136     setMRx(Servo1, _mid[1]);
00137     
00138     switch(pin) {
00139         case P2_0:    
00140             LPC_PINCON->PINSEL4 &= ~(3UL << 0); // Mbed p26 P2.0 clr bits
00141             LPC_PINCON->PINSEL4 |=  (1UL << 0); // Mbed p26 P2.0 set bits
00142             break;
00143         case P1_18: // Mbed LED1
00144             LPC_PINCON->PINSEL3 &= ~(3UL << 4); // Mbed LED2 P1.18 clr bits
00145             LPC_PINCON->PINSEL3 |=  (2UL << 4); // Mbed LED2 P1.18 set bits
00146             break;
00147     }
00148     
00149     LPC_PWM1->PCR |= (1UL << 9);
00150 }
00151 
00152 void
00153 SimpleRCservos::enable2(PinName pin)
00154 {
00155     setMRx(Servo2, _mid[2]);
00156     
00157     switch(pin) {
00158         case P2_1:
00159             LPC_PINCON->PINSEL4 &= ~(3UL << 2); // Mbed p25 P2.1 clr bits
00160             LPC_PINCON->PINSEL4 |=  (1UL << 2); // Mbed p25 P2.1 set bits
00161             break;
00162         case P1_20: // Mbed LED2
00163             LPC_PINCON->PINSEL3 &= ~(3UL << 8); // Mbed LED2 P1.20 clr bits
00164             LPC_PINCON->PINSEL3 |=  (2UL << 8); // Mbed LED2 P1.20 set bits
00165             break;
00166     }
00167     
00168     LPC_PWM1->PCR |= (1UL << 10);
00169 }
00170 
00171 void
00172 SimpleRCservos::enable3(PinName pin)
00173 {
00174     setMRx(Servo3, _mid[3]);
00175     
00176     switch(pin) {
00177         case P2_2:
00178             LPC_PINCON->PINSEL4 &= ~(3UL << 4); // Mbed p24 P2.2 clr bits
00179             LPC_PINCON->PINSEL4 |=  (1UL << 4); // Mbed p24 P2.2 set bits
00180             break;
00181         case P1_21: // Mbed LED3
00182             LPC_PINCON->PINSEL3 &= ~(3UL << 10); // Mbed LED3 P1.21 clr bits
00183             LPC_PINCON->PINSEL3 |=  (2UL << 10); // Mbed LED3 P1.21 set bits
00184             break;
00185     }
00186     
00187     LPC_PWM1->PCR |= (1UL << 11);
00188 }
00189 
00190 void
00191 SimpleRCservos::enable4(PinName pin)
00192 {
00193     setMRx(Servo4, _mid[4]);
00194     
00195     switch(pin) {
00196         case P2_3:
00197             LPC_PINCON->PINSEL4 &= ~(3UL << 6); // Mbed p23 P2.3 clr bits
00198             LPC_PINCON->PINSEL4 |=  (1UL << 6); // Mbed p23 P2.3 set bits
00199             break;
00200         case P1_23: // Mbed LED4
00201             LPC_PINCON->PINSEL3 &= ~(3UL << 14); // Mbed LED4 P1.23 clr bits
00202             LPC_PINCON->PINSEL3 |=  (2UL << 14); // Mbed LED4 P1.23 set bits
00203             break;
00204     }
00205     
00206     LPC_PWM1->PCR |= (1UL << 12);
00207 }
00208 
00209 void
00210 SimpleRCservos::enable5(PinName pin)
00211 {
00212     setMRx(Servo5, _mid[5]);
00213     
00214     switch(pin) {
00215         case P2_4: // Mbed p22
00216             LPC_PINCON->PINSEL4 &= ~(3UL << 8); // Mbed p22 P2.4 clr bits
00217             LPC_PINCON->PINSEL4 |=  (1UL << 8); // Mbed p22 P2.4 set bits
00218             break;
00219         case P1_24:            
00220             LPC_PINCON->PINSEL3 &= ~(3UL << 16); // P1.24 clr bits
00221             LPC_PINCON->PINSEL3 |=  (2UL << 16); // P1.24 set bits
00222             break;
00223     }
00224     
00225     LPC_PWM1->PCR |= (1UL << 13);
00226 }
00227 
00228 void
00229 SimpleRCservos::enable6(PinName pin)
00230 {
00231     setMRx(Servo6, _mid[6]);
00232     
00233     switch(pin) {
00234         case P2_5: // Mbed p21
00235             LPC_PINCON->PINSEL4 &= ~(3UL << 10); // Mbed p21 P2.5 clr bits
00236             LPC_PINCON->PINSEL4 |=  (1UL << 10); // Mbed p21 P2.5 set bits
00237             break;
00238         case P1_26:
00239             LPC_PINCON->PINSEL3 &= ~(3UL << 20); // P1.26 clr bits
00240             LPC_PINCON->PINSEL3 |=  (2UL << 20); // P1.26 set bits
00241             break;
00242     }
00243     
00244     LPC_PWM1->PCR |= (1UL << 14);
00245 }
00246 
00247 }; // namespace AjK ends.