Changeset 224 for rtos_arduino/trunk/arduino_lib/libraries/Servo/src
- Timestamp:
- Apr 30, 2016, 11:29:25 PM (8 years ago)
- Location:
- rtos_arduino/trunk/arduino_lib/libraries/Servo/src
- Files:
-
- 2 edited
Legend:
- Unmodified
- Added
- Removed
-
rtos_arduino/trunk/arduino_lib/libraries/Servo/src/Servo.h
r136 r224 66 66 #elif defined(ARDUINO_ARCH_SAMD) 67 67 //#include "samd/ServoTimersSamd.h" 68 #define MIN_PULSE_WIDTH_SAMD 500 69 #define MAX_PULSE_WIDTH_SAMD 2100 68 #define MIN_PULSE_WIDTH_SAMD_TCC 405 69 #define MAX_PULSE_WIDTH_SAMD_TCC 2300 70 #define MIN_PULSE_WIDTH_SAMD_TC 1700 71 #define MAX_PULSE_WIDTH_SAMD_TC 9200 70 72 #else 71 73 #error "This library only supports boards with an AVR,SAM or SAMD processor." -
rtos_arduino/trunk/arduino_lib/libraries/Servo/src/samd/Servo.cpp
r136 r224 16 16 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 17 17 */ 18 //Edited by Arduino Srl development team. 18 19 19 20 #if defined(ARDUINO_ARCH_SAMD) … … 22 23 #include <Servo.h> 23 24 25 24 26 static servo_t servos[MAX_SERVOS]; // static array of servo structures 25 27 26 28 uint8_t ServoCount = 0; // the total number of attached servos 29 uint8_t isTC = 0 ; 30 Tc* TCx ; 27 31 Tcc* TCCx; 28 32 uint8_t Channelx = 0; … … 30 34 // convenience macros 31 35 32 #define SERVO_MIN() (MIN_PULSE_WIDTH_SAMD) // minimum value in uS for this servo 33 #define SERVO_MAX() (MAX_PULSE_WIDTH_SAMD) // maximum value in uS for this servo 36 #define SERVO_MIN_TCC() (MIN_PULSE_WIDTH_SAMD_TCC) // minimum value in uS for this servo if TCC timer is used 37 #define SERVO_MAX_TCC() (MAX_PULSE_WIDTH_SAMD_TCC) // maximum value in uS for this servo if TCC timer is used 38 #define SERVO_MIN_TC() (MIN_PULSE_WIDTH_SAMD_TC) // minimum value in uS for this servo if TC timer is used 39 #define SERVO_MAX_TC() (MAX_PULSE_WIDTH_SAMD_TC) // maximum value in uS for this servo if TC timer is used 34 40 35 41 /************ static functions common to all instances ***********************/ … … 46 52 if (ServoCount < MAX_SERVOS) { 47 53 this->servoIndex = ServoCount++; // assign a servo index to this instance 48 } else { //su questo costruttore forse si deve tornare49 this->servoIndex = INVALID_SERVO; // too many servos54 } else { 55 this->servoIndex = INVALID_SERVO; // too many servos 50 56 } 51 57 } … … 53 59 uint8_t Servo::attach(int pin) 54 60 { 55 return this->attach(pin, MIN_PULSE_WIDTH_SAMD, MAX_PULSE_WIDTH_SAMD); 61 if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){ 62 return this->attach(pin, SERVO_MIN_TC(), SERVO_MAX_TC()); 63 } 64 else{ 65 return this->attach(pin, SERVO_MIN_TCC(), SERVO_MAX_TCC()); 66 } 56 67 } 57 68 … … 63 74 pinMode(pin, OUTPUT); // set servo pin to output 64 75 servos[this->servoIndex].Pin.nbr = pin; 65 if(min > MIN_PULSE_WIDTH_SAMD) min = MIN_PULSE_WIDTH_SAMD; 66 if (max > MAX_PULSE_WIDTH_SAMD) max = MAX_PULSE_WIDTH_SAMD; 76 int servo_min, servo_max; 77 if(pin==4 | pin==5 | pin==10 | pin==12){ 78 servo_min=SERVO_MIN_TC(); 79 servo_max=SERVO_MAX_TC(); 80 } 81 else{ 82 servo_min=SERVO_MIN_TCC(); 83 servo_max=SERVO_MAX_TCC(); 84 } 85 if(min > servo_min) min = servo_min; 86 if (max > servo_max) max = servo_max; 67 87 this->min = min; 68 88 this->max = max; … … 75 95 TCCx=TCC0; 76 96 Channelx=0; 97 isTC=0; 77 98 } 78 99 break; … … 83 104 TCCx=TCC0; 84 105 Channelx=1; 85 } 86 break; 87 106 isTC=0; 107 } 108 break; 109 110 case 4: 111 { 112 pinPeripheral(pin, g_APinDescription[pin].ulPinType); 113 TCx=TC3; 114 Channelx=0; 115 isTC=1; 116 117 } 118 break; 119 120 case 5: 121 { 122 pinPeripheral(pin, g_APinDescription[pin].ulPinType); 123 TCx=TC3; 124 Channelx=1; 125 isTC=1; 126 127 } 128 break; 88 129 case 6: 89 130 { … … 91 132 TCCx=TCC0; 92 133 Channelx=2; 134 isTC=0; 93 135 } 94 136 break; … … 99 141 TCCx=TCC0; 100 142 Channelx=3; 143 isTC=0; 101 144 } 102 145 break; … … 107 150 TCCx=TCC1; 108 151 Channelx=0; 152 isTC=0; 109 153 } 110 154 break; … … 115 159 TCCx=TCC1; 116 160 Channelx=1; 161 isTC=0; 162 } 163 break; 164 165 case 10: 166 { 167 pinPeripheral(pin, g_APinDescription[pin].ulPinType); 168 TCx=TC3; 169 Channelx=0; 170 isTC=1; 171 117 172 } 118 173 break; … … 123 178 TCCx=TCC2; 124 179 Channelx=0; 180 isTC=0; 181 } 182 break; 183 184 case 12: 185 { 186 pinPeripheral(pin, g_APinDescription[pin].ulPinType); 187 TCx=TC3; 188 Channelx=1; 189 isTC=1; 190 125 191 } 126 192 break; … … 131 197 TCCx=TCC2; 132 198 Channelx=1; 199 isTC=0; 133 200 } 134 201 break; … … 140 207 141 208 if ((TCCx==TCC0) | (TCCx==TCC1)) GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK3 | GCLK_CLKCTRL_ID( GCM_TCC0_TCC1 )) ; 142 else if( TCCx==TCC2)GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK3 | GCLK_CLKCTRL_ID( GCM_TCC2_TC3 )) ;209 else if((TCCx==TCC2) | (TCx==TC3 ))GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK3 | GCLK_CLKCTRL_ID( GCM_TCC2_TC3 )) ; 143 210 else; 144 145 if(servos[this->servoIndex].Pin.isActive == false) 146 { 147 TCCx->CTRLA.reg &=~(TCC_CTRLA_ENABLE); //disable TCC module 211 212 if(servos[this->servoIndex].Pin.isActive == false){ 213 // Set PORT 214 if ( isTC ) 215 { 216 // -- Configure TC 217 //DISABLE TCx 218 TCx->COUNT16.CTRLA.reg &=~(TC_CTRLA_ENABLE); 219 //Set Timer counter Mode to 16 bits 220 TCx->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16; 221 //Set Prescaler to divide by 2 222 TCx->COUNT16.CTRLA.reg |= TC_CTRLA_PRESCALER_DIV2; 223 //Set TCx as normal PWM 224 TCx->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_NPWM; 225 //default value for servo position 226 TCx->COUNT16.CC[Channelx].reg = 1500; 227 //ENABLE TCx 228 TCx->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE; 229 servos[this->servoIndex].Pin.isActive = true; 230 } 231 else 232 { 233 // -- Configure TCC 234 235 TCCx->CTRLA.reg &=~(TCC_CTRLA_ENABLE); //disable TCC module 148 236 TCCx->CTRLA.reg |=TCC_CTRLA_PRESCALER_DIV8; //setting prescaler to divide by 8 149 237 TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM; //Set TCCx as normal PWM … … 151 239 TCCx->PER.reg=20000; // setting servo frequency (50 hz) 152 240 TCCx->CTRLA.reg |= TCC_CTRLA_ENABLE ; //ENABLE TCCx 153 servos[this->servoIndex].Pin.isActive = true; 154 } 155 156 //servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive 241 servos[this->servoIndex].Pin.isActive = true; 242 } 243 244 } 157 245 } 158 246 return this->servoIndex; … … 167 255 else if((servos[this->servoIndex].Pin.nbr == 8) | (servos[this->servoIndex].Pin.nbr == 9)) TCC1->CTRLA.reg &=~(TCC_CTRLA_ENABLE); 168 256 else if ((servos[this->servoIndex].Pin.nbr == 11) | (servos[this->servoIndex].Pin.nbr == 13)) TCC2->CTRLA.reg &=~(TCC_CTRLA_ENABLE); 257 else if ((servos[this->servoIndex].Pin.nbr == 4 ) | (servos[this->servoIndex].Pin.nbr == 5 ) | (servos[this->servoIndex].Pin.nbr == 10 ) | (servos[this->servoIndex].Pin.nbr == 12 ))TC3->COUNT16.CTRLA.reg &=~(TC_CTRLA_ENABLE); 169 258 } 170 259 171 260 void Servo::write(int value) 172 { 173 // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds) 174 if (value < MIN_PULSE_WIDTH) 175 { 176 if (value < 0) 177 value = 0; 178 else if (value > 180) 179 value = 180; 180 181 value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX()); 182 } 261 { 262 //select the right values for servo motor 263 int servo_min; 264 int servo_max; 265 if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){ 266 servo_min=SERVO_MIN_TC(); 267 servo_max=SERVO_MAX_TC(); 268 // treat values less than 1700 as angles in degrees (valid values in microseconds are handled as microseconds) 269 if (value < servo_min) 270 { 271 if (value < 0) 272 value = 0; 273 else if (value > 180) 274 value = 180; 275 value = map(value, 0, 180, servo_min, servo_max); 276 } 277 } 278 else{ 279 servo_min=SERVO_MIN_TCC(); 280 servo_max=SERVO_MAX_TCC(); 281 // treat values less than 400 as angles in degrees (valid values in microseconds are handled as microseconds) 282 if (value < servo_min) 283 { 284 if (value < 0) 285 value = 0; 286 else if (value > 180) 287 value = 180; 288 value = map(value, 0, 180, servo_min, servo_max); 289 } 290 } 291 183 292 writeMicroseconds(value); 184 293 } … … 190 299 if( (channel < MAX_SERVOS) ) // ensure channel is valid 191 300 { 192 if (value < SERVO_MIN()) // ensure pulse width is valid 193 value = SERVO_MIN(); 194 else if (value > SERVO_MAX()) 195 value = SERVO_MAX(); 196 servos[this->servoIndex].ticks = value; //da sistemare 301 //select the right values for servo motor 302 int servo_min; 303 int servo_max; 304 if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){ 305 servo_min=SERVO_MIN_TC(); 306 servo_max=SERVO_MAX_TC(); 307 } 308 else{ 309 servo_min=SERVO_MIN_TCC(); 310 servo_max=SERVO_MAX_TCC(); 311 } 312 if (value < servo_min) // ensure pulse width is valid 313 value = servo_min; 314 else if (value > servo_max) 315 value = servo_max; 316 servos[this->servoIndex].ticks = value; 197 317 switch(servos[this->servoIndex].Pin.nbr) 198 318 { … … 205 325 break; 206 326 327 case 4: 328 TC3->COUNT16.CC[0].reg = value; 329 break; 330 331 case 5: 332 TC3->COUNT16.CC[1].reg = value; 333 break; 334 207 335 case 6: 208 336 TCC0->CC[2].reg=value; … … 220 348 TCC1->CC[1].reg=value; 221 349 break; 350 351 case 10: 352 TC3->COUNT16.CC[0].reg = value; 353 break; 222 354 223 355 case 11: … … 225 357 break; 226 358 359 case 12: 360 TC3->COUNT16.CC[1].reg = value; 361 break; 362 227 363 case 13: 228 364 TCC2->CC[1].reg=value; … … 234 370 } 235 371 236 //servos[this->servoIndex].ticks = value; // da sistemare372 //servos[this->servoIndex].ticks = value; //to be fixed 237 373 //servos[channel].ticks = value; 238 374 } … … 241 377 int Servo::read() // return the value as degrees 242 378 { 243 return map(readMicroseconds(), SERVO_MIN(), SERVO_MAX(), 0, 180); 379 //select the right values for servo motor 380 int servo_min; 381 int servo_max; 382 if((servos[this->servoIndex].Pin.nbr==4) | (servos[this->servoIndex].Pin.nbr==5) | (servos[this->servoIndex].Pin.nbr==10) | (servos[this->servoIndex].Pin.nbr==12) ){ 383 servo_min=SERVO_MIN_TC(); 384 servo_max=SERVO_MAX_TC(); 385 } 386 else{ 387 servo_min=SERVO_MIN_TCC(); 388 servo_max=SERVO_MAX_TCC(); 389 } 390 return map(readMicroseconds(), servo_min, servo_max, 0, 180); 244 391 } 245 392
Note:
See TracChangeset
for help on using the changeset viewer.