Andy K / SimpleRCservos

A simple API/Software interface to the LPC17xx PWM peripheral to provide control to up to six RC type servo controllers.

src/SimpleRCservos.cpp@3:1232505e7206, 2011-05-19 (annotated)

Committer:
AjK
Date:
Thu May 19 22:48:17 2011 +0000
Revision:
3:1232505e7206
Parent:
2:d6a018232650 
1.3 See ChangeLog.h

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AjK 2:d6a018232650 1 /*
AjK 2:d6a018232650 2 Copyright (c) 2011 Andy Kirkham
AjK 2:d6a018232650 3
AjK 2:d6a018232650 4 Permission is hereby granted, free of charge, to any person obtaining a copy
AjK 2:d6a018232650 5 of this software and associated documentation files (the "Software"), to deal
AjK 2:d6a018232650 6 in the Software without restriction, including without limitation the rights
AjK 2:d6a018232650 7 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
AjK 2:d6a018232650 8 copies of the Software, and to permit persons to whom the Software is
AjK 2:d6a018232650 9 furnished to do so, subject to the following conditions:
AjK 2:d6a018232650 10
AjK 2:d6a018232650 11 The above copyright notice and this permission notice shall be included in
AjK 2:d6a018232650 12 all copies or substantial portions of the Software.
AjK 2:d6a018232650 13
AjK 2:d6a018232650 14 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
AjK 2:d6a018232650 15 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
AjK 2:d6a018232650 16 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AjK 2:d6a018232650 17 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
AjK 2:d6a018232650 18 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
AjK 2:d6a018232650 19 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
AjK 2:d6a018232650 20 THE SOFTWARE.
AjK 2:d6a018232650 21 */
AjK 2:d6a018232650 22
AjK 2:d6a018232650 23
AjK 2:d6a018232650 24 #include "mbed.h"
AjK 2:d6a018232650 25 #include "SimpleRCservos.h"
AjK 2:d6a018232650 26
AjK 2:d6a018232650 27 namespace AjK {
AjK 2:d6a018232650 28
AjK 2:d6a018232650 29 void
AjK 2:d6a018232650 30 SimpleRCservos::init(uint32_t duty)
AjK 2:d6a018232650 31 {
AjK 2:d6a018232650 32 _duty = duty;
AjK 2:d6a018232650 33
AjK 2:d6a018232650 34 for (int i = 0; i < NumOfServos; i++) {
AjK 2:d6a018232650 35 _limitMin[i] = -1.0;
AjK 2:d6a018232650 36 _limitMax[i] = +1.0;
AjK 2:d6a018232650 37
AjK 2:d6a018232650 38 // The following would give 1ms to 2ms
AjK 2:d6a018232650 39 setRange((Servo)i, 1000, 2000);
AjK 2:d6a018232650 40
AjK 2:d6a018232650 41 // The following would give 900us to 2.1ms as required Hitec HS-322HD servo module.
AjK 2:d6a018232650 42 //setRange((Servo)i, 900, 2100);
AjK 2:d6a018232650 43
AjK 2:d6a018232650 44 // The following would give 600us to 2.4ms
AjK 2:d6a018232650 45 //setRange((Servo)i, 600, 2400);
AjK 2:d6a018232650 46
AjK 2:d6a018232650 47 _mid[i] = duty * 0.075f; // 1.5ms
AjK 2:d6a018232650 48
AjK 2:d6a018232650 49 }
AjK 2:d6a018232650 50 }
AjK 2:d6a018232650 51
AjK 2:d6a018232650 52 void
AjK 2:d6a018232650 53 SimpleRCservos::setDuty(uint32_t duty)
AjK 2:d6a018232650 54 {
AjK 2:d6a018232650 55 // Ensure powered up (default is 1)
AjK 2:d6a018232650 56 LPC_SC->PCONP |= (1UL << 6);
AjK 2:d6a018232650 57
AjK 2:d6a018232650 58 // CCLK/4 = 24MHz
AjK 2:d6a018232650 59 LPC_SC->PCLKSEL0 &= ~(3UL << 12);
AjK 2:d6a018232650 60
AjK 2:d6a018232650 61 // Reset.
AjK 2:d6a018232650 62 LPC_PWM1->TCR = 2;
AjK 2:d6a018232650 63
AjK 2:d6a018232650 64 _duty = duty;
AjK 2:d6a018232650 65
AjK 2:d6a018232650 66 LPC_PWM1->PR = 0;
AjK 2:d6a018232650 67 LPC_PWM1->MR0 = _duty;
AjK 2:d6a018232650 68 LPC_PWM1->MR1 = 0;
AjK 2:d6a018232650 69 LPC_PWM1->MR2 = 0;
AjK 2:d6a018232650 70 LPC_PWM1->MR3 = 0;
AjK 2:d6a018232650 71 LPC_PWM1->MR4 = 0;
AjK 2:d6a018232650 72 LPC_PWM1->MR5 = 0;
AjK 2:d6a018232650 73 LPC_PWM1->MR6 = 0;
AjK 2:d6a018232650 74
AjK 2:d6a018232650 75 LPC_PWM1->MCR = 2; // MR0 resets TC.
AjK 2:d6a018232650 76
AjK 2:d6a018232650 77 LPC_PWM1->TCR = 9; // Enable.
AjK 2:d6a018232650 78 }
AjK 2:d6a018232650 79
AjK 2:d6a018232650 80 double
AjK 2:d6a018232650 81 SimpleRCservos::position(Servo ch, double pos)
AjK 2:d6a018232650 82 {
AjK 2:d6a018232650 83 pos = limit(ch, pos);
AjK 2:d6a018232650 84 double a = pos - _limitMin[ch];
AjK 2:d6a018232650 85 double set = a / (_limitMax[ch] - _limitMin[ch]);
AjK 2:d6a018232650 86 uint32_t ps = _msMax[ch] * set;
AjK 2:d6a018232650 87 setMRx(ch, ps + _msMin[ch]);
AjK 2:d6a018232650 88 return pos;
AjK 2:d6a018232650 89 }
AjK 2:d6a018232650 90
AjK 2:d6a018232650 91 void
AjK 2:d6a018232650 92 SimpleRCservos::neutral(Servo ch)
AjK 2:d6a018232650 93 {
AjK 2:d6a018232650 94 setMRx(ch, _mid[ch]);
AjK 2:d6a018232650 95 }
AjK 2:d6a018232650 96
AjK 2:d6a018232650 97 double
AjK 2:d6a018232650 98 SimpleRCservos::limit(Servo ch, double pos)
AjK 2:d6a018232650 99 {
AjK 2:d6a018232650 100 if (pos >= _limitMin[ch] && pos <= _limitMax[ch]) return pos;
AjK 2:d6a018232650 101 if (pos < _limitMin[ch]) return _limitMin[ch];
AjK 2:d6a018232650 102 if (pos > _limitMax[ch]) return _limitMax[ch];
AjK 2:d6a018232650 103 return 0.0; // Keep the compiler happy.
AjK 2:d6a018232650 104 }
AjK 2:d6a018232650 105
AjK 2:d6a018232650 106 void
AjK 2:d6a018232650 107 SimpleRCservos::setMRx(Servo ch, uint32_t value)
AjK 2:d6a018232650 108 {
AjK 2:d6a018232650 109 switch(ch) {
AjK 2:d6a018232650 110 case Servo1: LPC_PWM1->MR1 = value; break;
AjK 2:d6a018232650 111 case Servo2: LPC_PWM1->MR2 = value; break;
AjK 2:d6a018232650 112 case Servo3: LPC_PWM1->MR3 = value; break;
AjK 2:d6a018232650 113 case Servo4: LPC_PWM1->MR4 = value; break;
AjK 2:d6a018232650 114 case Servo5: LPC_PWM1->MR5 = value; break;
AjK 2:d6a018232650 115 case Servo6: LPC_PWM1->MR6 = value; break;
AjK 2:d6a018232650 116 }
AjK 2:d6a018232650 117 LPC_PWM1->LER |= (1UL << ch);
AjK 2:d6a018232650 118 }
AjK 2:d6a018232650 119
AjK 2:d6a018232650 120 void
AjK 2:d6a018232650 121 SimpleRCservos::enable(Servo ch)
AjK 2:d6a018232650 122 {
AjK 2:d6a018232650 123 switch(ch) {
AjK 2:d6a018232650 124 case 1: enable1(); break;
AjK 2:d6a018232650 125 case 2: enable2(); break;
AjK 2:d6a018232650 126 case 3: enable3(); break;
AjK 2:d6a018232650 127 case 4: enable4(); break;
AjK 2:d6a018232650 128 case 5: enable5(); break;
AjK 2:d6a018232650 129 case 6: enable6(); break;
AjK 2:d6a018232650 130 }
AjK 2:d6a018232650 131 }
AjK 2:d6a018232650 132
AjK 2:d6a018232650 133 void
AjK 2:d6a018232650 134 SimpleRCservos::enable1(PinName pin)
AjK 2:d6a018232650 135 {
AjK 2:d6a018232650 136 setMRx(Servo1, _mid[1]);
AjK 2:d6a018232650 137
AjK 2:d6a018232650 138 switch(pin) {
AjK 2:d6a018232650 139 case P2_0:
AjK 2:d6a018232650 140 LPC_PINCON->PINSEL4 &= ~(3UL << 0); // Mbed p26 P2.0 clr bits
AjK 2:d6a018232650 141 LPC_PINCON->PINSEL4 |= (1UL << 0); // Mbed p26 P2.0 set bits
AjK 2:d6a018232650 142 break;
AjK 2:d6a018232650 143 case P1_18: // Mbed LED1
AjK 2:d6a018232650 144 LPC_PINCON->PINSEL3 &= ~(3UL << 4); // Mbed LED2 P1.18 clr bits
AjK 2:d6a018232650 145 LPC_PINCON->PINSEL3 |= (2UL << 4); // Mbed LED2 P1.18 set bits
AjK 2:d6a018232650 146 break;
AjK 2:d6a018232650 147 }
AjK 2:d6a018232650 148
AjK 2:d6a018232650 149 LPC_PWM1->PCR |= (1UL << 9);
AjK 2:d6a018232650 150 }
AjK 2:d6a018232650 151
AjK 2:d6a018232650 152 void
AjK 2:d6a018232650 153 SimpleRCservos::enable2(PinName pin)
AjK 2:d6a018232650 154 {
AjK 2:d6a018232650 155 setMRx(Servo2, _mid[2]);
AjK 2:d6a018232650 156
AjK 2:d6a018232650 157 switch(pin) {
AjK 2:d6a018232650 158 case P2_1:
AjK 2:d6a018232650 159 LPC_PINCON->PINSEL4 &= ~(3UL << 2); // Mbed p25 P2.1 clr bits
AjK 2:d6a018232650 160 LPC_PINCON->PINSEL4 |= (1UL << 2); // Mbed p25 P2.1 set bits
AjK 2:d6a018232650 161 break;
AjK 2:d6a018232650 162 case P1_20: // Mbed LED2
AjK 2:d6a018232650 163 LPC_PINCON->PINSEL3 &= ~(3UL << 8); // Mbed LED2 P1.20 clr bits
AjK 2:d6a018232650 164 LPC_PINCON->PINSEL3 |= (2UL << 8); // Mbed LED2 P1.20 set bits
AjK 2:d6a018232650 165 break;
AjK 2:d6a018232650 166 }
AjK 2:d6a018232650 167
AjK 2:d6a018232650 168 LPC_PWM1->PCR |= (1UL << 10);
AjK 2:d6a018232650 169 }
AjK 2:d6a018232650 170
AjK 2:d6a018232650 171 void
AjK 2:d6a018232650 172 SimpleRCservos::enable3(PinName pin)
AjK 2:d6a018232650 173 {
AjK 2:d6a018232650 174 setMRx(Servo3, _mid[3]);
AjK 2:d6a018232650 175
AjK 2:d6a018232650 176 switch(pin) {
AjK 2:d6a018232650 177 case P2_2:
AjK 2:d6a018232650 178 LPC_PINCON->PINSEL4 &= ~(3UL << 4); // Mbed p24 P2.2 clr bits
AjK 2:d6a018232650 179 LPC_PINCON->PINSEL4 |= (1UL << 4); // Mbed p24 P2.2 set bits
AjK 2:d6a018232650 180 break;
AjK 2:d6a018232650 181 case P1_21: // Mbed LED3
AjK 2:d6a018232650 182 LPC_PINCON->PINSEL3 &= ~(3UL << 10); // Mbed LED3 P1.21 clr bits
AjK 2:d6a018232650 183 LPC_PINCON->PINSEL3 |= (2UL << 10); // Mbed LED3 P1.21 set bits
AjK 2:d6a018232650 184 break;
AjK 2:d6a018232650 185 }
AjK 2:d6a018232650 186
AjK 2:d6a018232650 187 LPC_PWM1->PCR |= (1UL << 11);
AjK 2:d6a018232650 188 }
AjK 2:d6a018232650 189
AjK 2:d6a018232650 190 void
AjK 2:d6a018232650 191 SimpleRCservos::enable4(PinName pin)
AjK 2:d6a018232650 192 {
AjK 2:d6a018232650 193 setMRx(Servo4, _mid[4]);
AjK 2:d6a018232650 194
AjK 2:d6a018232650 195 switch(pin) {
AjK 2:d6a018232650 196 case P2_3:
AjK 2:d6a018232650 197 LPC_PINCON->PINSEL4 &= ~(3UL << 6); // Mbed p23 P2.3 clr bits
AjK 2:d6a018232650 198 LPC_PINCON->PINSEL4 |= (1UL << 6); // Mbed p23 P2.3 set bits
AjK 2:d6a018232650 199 break;
AjK 2:d6a018232650 200 case P1_23: // Mbed LED4
AjK 2:d6a018232650 201 LPC_PINCON->PINSEL3 &= ~(3UL << 14); // Mbed LED4 P1.23 clr bits
AjK 2:d6a018232650 202 LPC_PINCON->PINSEL3 |= (2UL << 14); // Mbed LED4 P1.23 set bits
AjK 2:d6a018232650 203 break;
AjK 2:d6a018232650 204 }
AjK 2:d6a018232650 205
AjK 2:d6a018232650 206 LPC_PWM1->PCR |= (1UL << 12);
AjK 2:d6a018232650 207 }
AjK 2:d6a018232650 208
AjK 2:d6a018232650 209 void
AjK 2:d6a018232650 210 SimpleRCservos::enable5(PinName pin)
AjK 2:d6a018232650 211 {
AjK 2:d6a018232650 212 setMRx(Servo5, _mid[5]);
AjK 2:d6a018232650 213
AjK 2:d6a018232650 214 switch(pin) {
AjK 2:d6a018232650 215 case P2_4: // Mbed p22
AjK 2:d6a018232650 216 LPC_PINCON->PINSEL4 &= ~(3UL << 8); // Mbed p22 P2.4 clr bits
AjK 2:d6a018232650 217 LPC_PINCON->PINSEL4 |= (1UL << 8); // Mbed p22 P2.4 set bits
AjK 2:d6a018232650 218 break;
AjK 2:d6a018232650 219 case P1_24:
AjK 2:d6a018232650 220 LPC_PINCON->PINSEL3 &= ~(3UL << 16); // P1.24 clr bits
AjK 2:d6a018232650 221 LPC_PINCON->PINSEL3 |= (2UL << 16); // P1.24 set bits
AjK 2:d6a018232650 222 break;
AjK 2:d6a018232650 223 }
AjK 2:d6a018232650 224
AjK 2:d6a018232650 225 LPC_PWM1->PCR |= (1UL << 13);
AjK 2:d6a018232650 226 }
AjK 2:d6a018232650 227
AjK 2:d6a018232650 228 void
AjK 2:d6a018232650 229 SimpleRCservos::enable6(PinName pin)
AjK 2:d6a018232650 230 {
AjK 2:d6a018232650 231 setMRx(Servo6, _mid[6]);
AjK 2:d6a018232650 232
AjK 2:d6a018232650 233 switch(pin) {
AjK 2:d6a018232650 234 case P2_5: // Mbed p21
AjK 2:d6a018232650 235 LPC_PINCON->PINSEL4 &= ~(3UL << 10); // Mbed p21 P2.5 clr bits
AjK 2:d6a018232650 236 LPC_PINCON->PINSEL4 |= (1UL << 10); // Mbed p21 P2.5 set bits
AjK 2:d6a018232650 237 break;
AjK 2:d6a018232650 238 case P1_26:
AjK 2:d6a018232650 239 LPC_PINCON->PINSEL3 &= ~(3UL << 20); // P1.26 clr bits
AjK 2:d6a018232650 240 LPC_PINCON->PINSEL3 |= (2UL << 20); // P1.26 set bits
AjK 2:d6a018232650 241 break;
AjK 2:d6a018232650 242 }
AjK 2:d6a018232650 243
AjK 2:d6a018232650 244 LPC_PWM1->PCR |= (1UL << 14);
AjK 2:d6a018232650 245 }
AjK 2:d6a018232650 246
AjK 2:d6a018232650 247 }; // namespace AjK ends.