Controlant un servomotor

Un servomotor és un dispositiu electromecànic de control senzill. La majoria d’ells son rotatius i permeten controlar l’angle d’actuació del motor amb un senyal digital, normalment un senyal PWM.

Així, el que se sol necessitar és un senyal PWM a una freqüència determinada i un temps actiu entre certs valors que provocaran un moviment proporcional al motor. En el cas del servo que tinc entre mans, un Parallax Standard Servo (#900-00005) (link al seu DataSheet), cal un PWM a 50 Hz i uns temps mínims de 0.75 ms i màxim de 2.25 ms. Aquests temps faran que el servo es mogui entre els 0º i els 180º de rotació.

Servomotor parallax

Per tant, el que caldrà serà preparar el nostre codi per generar un PWM a 50 Hz i després controlar que es generi un pols d’entre 0.75 i 2.25 mili-segons. Anem a fer els càlculs.

Com ja vem fer al post sobre el PWM, programarem un Timer amb una sortida directe a un pin que serà on generarem la senyal de control. Com que volem una freqüència de treball de 50 Hz (20 ms), hem de treballar la formula 17.4 del datasheet per obtenir el valor TOP a partir d’una freqüència desitjada i un valor del prescaler fixat. Així, l’expressió 17.4 ens queda com segueix:

Tenint en compte que el comptador TOP és de 16 bits (en altres famílies de EFM32 els timers son de 32 bits, però no per la versió Tiny) i que quan més alt el valor, millor, podem provar diferents valors de PRESCALER i obtenir la següent taula:

F CPU (Hz)F PWM (Hz)PRESCALER (counts)TOP (counts)
14_000_000502139_999
14_000_00050469_999
14_000_00050834_999
14_000_000501617_499
14_000_00050328_740
Càlculs del valor TOP pel Timer

Com que la resolució depèn del valor de TOP, hem d’agafar el valor més alt que càpiga en 16 bits, per tant en aquest cas, serà el valor 34_999 amb PRESCALER a 8.

Seguint la fórmula 17.3 del Datasheet, el nombre de bits de resolució és 15, que no està gens malament per un comptador de 16 bits com el nostre.

Ara, cal tenir en compte que el duty cycle el PWM no pot anar el 0 al 100%, ja que el màxim de nivell alt que pot rebre el servo és de 2.25 ms i el 100% de duty cycle serien 20 ms. Llavors el valor màxim (corresponent als 180º del servo) del PWM han de ser 2.25 ms que serà (en counts) el valor 34_999 * 2.25 / 20 = 3937,38 -> 3937 . Pel valor mínim (corresponent a 0º) ha de ser 0,75 ms que correspon al valor 34_999 * 0,75 / 20 = 1312,46 -> 1312.

PWM per 180º del servomotor
PWM per 0º del servomotor

Amb això ja podem fer una funció que ens passi de graus de rotació a counts del PWM per simplificar el codi. La funció queda així:

#define PWM_FREQ (34999)
#define PWM_0 (1312)
#define PWM_180 (3937)

uint32_t degrees_to_pwm(uint32_t degrees) {
    uint32_t ret_value = 0;

    if (degrees < 180) {
        ret_value = (degree * (PWM_180 - PWM_0) / 180 ) + PWM_0;
    } else {
        ret_value = PWM_180;
    }

    return ret_value;
}

I posem un codi a les ISR dels dos botons perquè incrementi (decrementi) el valor en graus on volem situar el servo:

void GPIO_EVEN_IRQHandler(void) {
    uint32_t aux;

    aux = GPIO_IntGet();

    /* clear flags */
    GPIO_IntClear(aux);

    degree += 30;

    if (degree > 180) {
        degree = 180;
    }

    pwm_value = degrees_to_pwm(degree);

    TIMER_CompareBufSet(TIMER1, 1, pwm_value);

}

Amb això ja tenim quasi acabat el nostre codi de control d’un servomotor. El projecte complet, com sempre està al repositori del curs.

Comentaris

En el meu cas i amb el servomotor que tinc, s’observa que quan esta en repòs de tant en tant se sent com el servo fa algun moviment o “crec”. No n’estic segur, però pot ser que sigui perquè la freqüència del PWM no sigui prou estable i s’escurci un pel i se surti d’especificacions. En aquest cas millora si s’allarga el període del PWM augmentant una mica el valor de PWM_FREQ.

També veig que si el mínim el poso a 0.75 ms el servo no arriba a fer 180º de rotació i puc baixar el mínim fins a 0.5ms i que tot segueixi funcionat. En aquest cas el valor de PWM_0 és 875.

Deixa un comentari

Fill in your details below or click an icon to log in:

WordPress.com Logo

Esteu comentant fent servir el compte WordPress.com. Log Out /  Canvia )

Google photo

Esteu comentant fent servir el compte Google. Log Out /  Canvia )

Twitter picture

Esteu comentant fent servir el compte Twitter. Log Out /  Canvia )

Facebook photo

Esteu comentant fent servir el compte Facebook. Log Out /  Canvia )

S'està connectant a %s

Aquest lloc utilitza Akismet per reduir els comentaris brossa. Apreneu com es processen les dades dels comentaris.