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@0:c680385286e3, 2011-05-02 (annotated)

Committer:
AjK
Date:
Mon May 02 18:38:45 2011 +0000
Revision:
0:c680385286e3
Child:
2:d6a018232650
1.0 See ChangeLog.h

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AjK 0:c680385286e3 1
AjK 0:c680385286e3 2
AjK 0:c680385286e3 3 #include "mbed.h"
AjK 0:c680385286e3 4 #include "SimpleRCservos.h"
AjK 0:c680385286e3 5
AjK 0:c680385286e3 6 namespace AjK {
AjK 0:c680385286e3 7
AjK 0:c680385286e3 8 void
AjK 0:c680385286e3 9 SimpleRCservos::init(uint32_t duty)
AjK 0:c680385286e3 10 {
AjK 0:c680385286e3 11 _duty = duty;
AjK 0:c680385286e3 12
AjK 0:c680385286e3 13 for (int i = 0; i < NumOfServos; i++) {
AjK 0:c680385286e3 14 _limitMin[i] = -1.0;
AjK 0:c680385286e3 15 _limitMax[i] = +1.0;
AjK 0:c680385286e3 16
AjK 0:c680385286e3 17 // The following would give 1ms to 2ms
AjK 0:c680385286e3 18 setRange((Servo)i, 1000, 2000);
AjK 0:c680385286e3 19
AjK 0:c680385286e3 20 // The following would give 900us to 2.1ms as required Hitec HS-322HD servo module.
AjK 0:c680385286e3 21 //setRange((Servo)i, 900, 2100);
AjK 0:c680385286e3 22
AjK 0:c680385286e3 23 // The following would give 600us to 2.4ms
AjK 0:c680385286e3 24 //setRange((Servo)i, 600, 2400);
AjK 0:c680385286e3 25
AjK 0:c680385286e3 26 _mid[i] = duty * 0.075f; // 1.5ms
AjK 0:c680385286e3 27
AjK 0:c680385286e3 28 }
AjK 0:c680385286e3 29 }
AjK 0:c680385286e3 30
AjK 0:c680385286e3 31 void
AjK 0:c680385286e3 32 SimpleRCservos::setDuty(uint32_t duty)
AjK 0:c680385286e3 33 {
AjK 0:c680385286e3 34 // Ensure powered up (default is 1)
AjK 0:c680385286e3 35 LPC_SC->PCONP |= (1UL << 6);
AjK 0:c680385286e3 36
AjK 0:c680385286e3 37 // CCLK/4 = 24MHz
AjK 0:c680385286e3 38 LPC_SC->PCLKSEL0 &= ~(3UL << 12);
AjK 0:c680385286e3 39
AjK 0:c680385286e3 40 // Reset.
AjK 0:c680385286e3 41 LPC_PWM1->TCR = 2;
AjK 0:c680385286e3 42
AjK 0:c680385286e3 43 _duty = duty;
AjK 0:c680385286e3 44
AjK 0:c680385286e3 45 LPC_PWM1->PR = 0;
AjK 0:c680385286e3 46 LPC_PWM1->MR0 = _duty;
AjK 0:c680385286e3 47 LPC_PWM1->MR1 = 0;
AjK 0:c680385286e3 48 LPC_PWM1->MR2 = 0;
AjK 0:c680385286e3 49 LPC_PWM1->MR3 = 0;
AjK 0:c680385286e3 50 LPC_PWM1->MR4 = 0;
AjK 0:c680385286e3 51 LPC_PWM1->MR5 = 0;
AjK 0:c680385286e3 52 LPC_PWM1->MR6 = 0;
AjK 0:c680385286e3 53
AjK 0:c680385286e3 54 LPC_PWM1->MCR = 2; // MR0 resets TC.
AjK 0:c680385286e3 55
AjK 0:c680385286e3 56 LPC_PWM1->TCR = 9; // Enable.
AjK 0:c680385286e3 57 }
AjK 0:c680385286e3 58
AjK 0:c680385286e3 59 double
AjK 0:c680385286e3 60 SimpleRCservos::position(Servo ch, double pos)
AjK 0:c680385286e3 61 {
AjK 0:c680385286e3 62 pos = limit(ch, pos);
AjK 0:c680385286e3 63 double a = pos - _limitMin[ch];
AjK 0:c680385286e3 64 double set = a / (_limitMax[ch] - _limitMin[ch]);
AjK 0:c680385286e3 65 uint32_t ps = _msMax[ch] * set;
AjK 0:c680385286e3 66 setMRx(ch, ps + _msMin[ch]);
AjK 0:c680385286e3 67 return pos;
AjK 0:c680385286e3 68 }
AjK 0:c680385286e3 69
AjK 0:c680385286e3 70 void
AjK 0:c680385286e3 71 SimpleRCservos::neutral(Servo ch)
AjK 0:c680385286e3 72 {
AjK 0:c680385286e3 73 setMRx(ch, _mid[ch]);
AjK 0:c680385286e3 74 }
AjK 0:c680385286e3 75
AjK 0:c680385286e3 76 double
AjK 0:c680385286e3 77 SimpleRCservos::limit(Servo ch, double pos)
AjK 0:c680385286e3 78 {
AjK 0:c680385286e3 79 if (pos >= _limitMin[ch] && pos <= _limitMax[ch]) return pos;
AjK 0:c680385286e3 80 if (pos < _limitMin[ch]) return _limitMin[ch];
AjK 0:c680385286e3 81 if (pos > _limitMax[ch]) return _limitMax[ch];
AjK 0:c680385286e3 82 return 0.0; // Keep the compiler happy.
AjK 0:c680385286e3 83 }
AjK 0:c680385286e3 84
AjK 0:c680385286e3 85 void
AjK 0:c680385286e3 86 SimpleRCservos::setMRx(Servo ch, uint32_t value)
AjK 0:c680385286e3 87 {
AjK 0:c680385286e3 88 switch(ch) {
AjK 0:c680385286e3 89 case Servo1: LPC_PWM1->MR1 = value; break;
AjK 0:c680385286e3 90 case Servo2: LPC_PWM1->MR2 = value; break;
AjK 0:c680385286e3 91 case Servo3: LPC_PWM1->MR3 = value; break;
AjK 0:c680385286e3 92 case Servo4: LPC_PWM1->MR4 = value; break;
AjK 0:c680385286e3 93 case Servo5: LPC_PWM1->MR5 = value; break;
AjK 0:c680385286e3 94 case Servo6: LPC_PWM1->MR6 = value; break;
AjK 0:c680385286e3 95 }
AjK 0:c680385286e3 96 LPC_PWM1->LER |= (1UL << ch);
AjK 0:c680385286e3 97 }
AjK 0:c680385286e3 98
AjK 0:c680385286e3 99 void
AjK 0:c680385286e3 100 SimpleRCservos::enable(Servo ch)
AjK 0:c680385286e3 101 {
AjK 0:c680385286e3 102 switch(ch) {
AjK 0:c680385286e3 103 case 1: enable1(); break;
AjK 0:c680385286e3 104 case 2: enable2(); break;
AjK 0:c680385286e3 105 case 3: enable3(); break;
AjK 0:c680385286e3 106 case 4: enable4(); break;
AjK 0:c680385286e3 107 case 5: enable5(); break;
AjK 0:c680385286e3 108 case 6: enable6(); break;
AjK 0:c680385286e3 109 }
AjK 0:c680385286e3 110 }
AjK 0:c680385286e3 111
AjK 0:c680385286e3 112 void
AjK 0:c680385286e3 113 SimpleRCservos::enable1(PinName pin)
AjK 0:c680385286e3 114 {
AjK 0:c680385286e3 115 setMRx(Servo1, _mid[1]);
AjK 0:c680385286e3 116
AjK 0:c680385286e3 117 switch(pin) {
AjK 0:c680385286e3 118 case P2_0:
AjK 0:c680385286e3 119 LPC_PINCON->PINSEL4 &= ~(3UL << 0); // Mbed p26 P2.0 clr bits
AjK 0:c680385286e3 120 LPC_PINCON->PINSEL4 |= (1UL << 0); // Mbed p26 P2.0 set bits
AjK 0:c680385286e3 121 break;
AjK 0:c680385286e3 122 case P1_18: // Mbed LED1
AjK 0:c680385286e3 123 LPC_PINCON->PINSEL3 &= ~(3UL << 4); // Mbed LED2 P1.18 clr bits
AjK 0:c680385286e3 124 LPC_PINCON->PINSEL3 |= (2UL << 4); // Mbed LED2 P1.18 set bits
AjK 0:c680385286e3 125 break;
AjK 0:c680385286e3 126 }
AjK 0:c680385286e3 127
AjK 0:c680385286e3 128 LPC_PWM1->PCR |= (1UL << 9);
AjK 0:c680385286e3 129 }
AjK 0:c680385286e3 130
AjK 0:c680385286e3 131 void
AjK 0:c680385286e3 132 SimpleRCservos::enable2(PinName pin)
AjK 0:c680385286e3 133 {
AjK 0:c680385286e3 134 setMRx(Servo2, _mid[2]);
AjK 0:c680385286e3 135
AjK 0:c680385286e3 136 switch(pin) {
AjK 0:c680385286e3 137 case P2_1:
AjK 0:c680385286e3 138 LPC_PINCON->PINSEL4 &= ~(3UL << 2); // Mbed p25 P2.1 clr bits
AjK 0:c680385286e3 139 LPC_PINCON->PINSEL4 |= (1UL << 2); // Mbed p25 P2.1 set bits
AjK 0:c680385286e3 140 break;
AjK 0:c680385286e3 141 case P1_20: // Mbed LED2
AjK 0:c680385286e3 142 LPC_PINCON->PINSEL3 &= ~(3UL << 8); // Mbed LED2 P1.20 clr bits
AjK 0:c680385286e3 143 LPC_PINCON->PINSEL3 |= (2UL << 8); // Mbed LED2 P1.20 set bits
AjK 0:c680385286e3 144 break;
AjK 0:c680385286e3 145 }
AjK 0:c680385286e3 146
AjK 0:c680385286e3 147 LPC_PWM1->PCR |= (1UL << 10);
AjK 0:c680385286e3 148 }
AjK 0:c680385286e3 149
AjK 0:c680385286e3 150 void
AjK 0:c680385286e3 151 SimpleRCservos::enable3(PinName pin)
AjK 0:c680385286e3 152 {
AjK 0:c680385286e3 153 setMRx(Servo3, _mid[3]);
AjK 0:c680385286e3 154
AjK 0:c680385286e3 155 switch(pin) {
AjK 0:c680385286e3 156 case P2_2:
AjK 0:c680385286e3 157 LPC_PINCON->PINSEL4 &= ~(3UL << 4); // Mbed p24 P2.2 clr bits
AjK 0:c680385286e3 158 LPC_PINCON->PINSEL4 |= (1UL << 4); // Mbed p24 P2.2 set bits
AjK 0:c680385286e3 159 break;
AjK 0:c680385286e3 160 case P1_21: // Mbed LED3
AjK 0:c680385286e3 161 LPC_PINCON->PINSEL3 &= ~(3UL << 10); // Mbed LED3 P1.21 clr bits
AjK 0:c680385286e3 162 LPC_PINCON->PINSEL3 |= (2UL << 10); // Mbed LED3 P1.21 set bits
AjK 0:c680385286e3 163 break;
AjK 0:c680385286e3 164 }
AjK 0:c680385286e3 165
AjK 0:c680385286e3 166 LPC_PWM1->PCR |= (1UL << 11);
AjK 0:c680385286e3 167 }
AjK 0:c680385286e3 168
AjK 0:c680385286e3 169 void
AjK 0:c680385286e3 170 SimpleRCservos::enable4(PinName pin)
AjK 0:c680385286e3 171 {
AjK 0:c680385286e3 172 setMRx(Servo4, _mid[4]);
AjK 0:c680385286e3 173
AjK 0:c680385286e3 174 switch(pin) {
AjK 0:c680385286e3 175 case P2_3:
AjK 0:c680385286e3 176 LPC_PINCON->PINSEL4 &= ~(3UL << 6); // Mbed p23 P2.3 clr bits
AjK 0:c680385286e3 177 LPC_PINCON->PINSEL4 |= (1UL << 6); // Mbed p23 P2.3 set bits
AjK 0:c680385286e3 178 break;
AjK 0:c680385286e3 179 case P1_23: // Mbed LED4
AjK 0:c680385286e3 180 LPC_PINCON->PINSEL3 &= ~(3UL << 14); // Mbed LED4 P1.23 clr bits
AjK 0:c680385286e3 181 LPC_PINCON->PINSEL3 |= (2UL << 14); // Mbed LED4 P1.23 set bits
AjK 0:c680385286e3 182 break;
AjK 0:c680385286e3 183 }
AjK 0:c680385286e3 184
AjK 0:c680385286e3 185 LPC_PWM1->PCR |= (1UL << 12);
AjK 0:c680385286e3 186 }
AjK 0:c680385286e3 187
AjK 0:c680385286e3 188 void
AjK 0:c680385286e3 189 SimpleRCservos::enable5(PinName pin)
AjK 0:c680385286e3 190 {
AjK 0:c680385286e3 191 setMRx(Servo5, _mid[5]);
AjK 0:c680385286e3 192
AjK 0:c680385286e3 193 switch(pin) {
AjK 0:c680385286e3 194 case P2_4: // Mbed p22
AjK 0:c680385286e3 195 LPC_PINCON->PINSEL4 &= ~(3UL << 8); // Mbed p22 P2.4 clr bits
AjK 0:c680385286e3 196 LPC_PINCON->PINSEL4 |= (1UL << 8); // Mbed p22 P2.4 set bits
AjK 0:c680385286e3 197 break;
AjK 0:c680385286e3 198 case P1_24:
AjK 0:c680385286e3 199 LPC_PINCON->PINSEL3 &= ~(3UL << 16); // P1.24 clr bits
AjK 0:c680385286e3 200 LPC_PINCON->PINSEL3 |= (2UL << 16); // P1.24 set bits
AjK 0:c680385286e3 201 break;
AjK 0:c680385286e3 202 }
AjK 0:c680385286e3 203
AjK 0:c680385286e3 204 LPC_PWM1->PCR |= (1UL << 13);
AjK 0:c680385286e3 205 }
AjK 0:c680385286e3 206
AjK 0:c680385286e3 207 void
AjK 0:c680385286e3 208 SimpleRCservos::enable6(PinName pin)
AjK 0:c680385286e3 209 {
AjK 0:c680385286e3 210 setMRx(Servo6, _mid[6]);
AjK 0:c680385286e3 211
AjK 0:c680385286e3 212 switch(pin) {
AjK 0:c680385286e3 213 case P2_5: // Mbed p21
AjK 0:c680385286e3 214 LPC_PINCON->PINSEL4 &= ~(3UL << 10); // Mbed p21 P2.5 clr bits
AjK 0:c680385286e3 215 LPC_PINCON->PINSEL4 |= (1UL << 10); // Mbed p21 P2.5 set bits
AjK 0:c680385286e3 216 break;
AjK 0:c680385286e3 217 case P1_26:
AjK 0:c680385286e3 218 LPC_PINCON->PINSEL3 &= ~(3UL << 20); // P1.26 clr bits
AjK 0:c680385286e3 219 LPC_PINCON->PINSEL3 |= (2UL << 20); // P1.26 set bits
AjK 0:c680385286e3 220 break;
AjK 0:c680385286e3 221 }
AjK 0:c680385286e3 222
AjK 0:c680385286e3 223 LPC_PWM1->PCR |= (1UL << 14);
AjK 0:c680385286e3 224 }
AjK 0:c680385286e3 225
AjK 0:c680385286e3 226 }; // namespace AjK ends.