A simple API/Software interface to the LPC17xx PWM peripheral to provide control to up to six RC type servo controllers.
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.
Generated on Fri Jul 15 2022 02:13:19 by 1.7.2