Servo-ohjain

Monikanavainen servo-ohjain 8051:lle

Esimerkkiohjelma, miten voi generoida servopulsseja usealle RC-servolle käyttäen vain yhtä kontrollerin ajastinta.

/***************************************************************************
 
 servocont.c
 
 Monikanavainen servo-ohjain 8051:lle.
 
 Ohjelma generoi servopulssia 1...8 servolle käyttäen vain yhtä timeria (T0).
 Kunkin pulssin leveyttä voi säätää toisistaan riippumatta. Ohjelma ei käytä
 mitään perus-8051:n ulkopuolisia resursseja.
 
 Varsinainen pulssien generointi tapahtuu Timer0-keskeytyksessä. Pulssien 
 leveydet ovat pulse_len-taulukossa. Tässä esimerkissä on mukana yksinkertainen 
 pääohjelma, joka lukee sarjaportista komentoja, joilla servojen asentoja voi 
 asetella.
 
 Komennot ovat muotoa 
 
    <servon nro>,<pulssin leveys>
 
    servon nro = 0...4
    pulssin leveys = mikrosekunteja, 1000...2000
 
 esim. komento 1,1200 asettaa servon 1 pulssin leveydeksi 1.2 ms.
 
 Sarjaportin nopeus on 9600 b/s 24 MHz kiteellä.
 
 Servojen määrää voi säätää vakiolla MAX_SERVO_NR. Tämä on suurimman servon numero, 
 numerointi alkaa nollasta. Koodissa arvo  on 4, eli servoja on 5.
 Pulssin leveyden rajat asetellaan vakioilla SERVO_MIN ja SERVO_MAX (mikrosekunteja). 
 SERVO_MAX pitää olla alle 2500!
 
 Servojen ohjaukset lähtevät vakion SERVO_PORT ilmoittaman portin pinneistä (koodissa P2),
 servo 0 = P2.0, servo 1 = P2.1 jne.
 
 Vakiolla CYCLES_PER_US valitaan, onko käytössä 12 vain 24 MHz kide. Muut taajuudet 
 eivät käy. Huom. 12 MHz kiteellä sarjaportin nopeus putoaa 4800 b/s:iin!
 
 Ohjelma kääntyy SDCC:llä, komennolla
 
   sdcc servocont.c
 
 Testattu NXP:n P89CV51RD2:lla 24 MHz kiteellä. 12 MHz kiteellä ei testattu.
 
 JK 2.11.2008
 
 ***************************************************************************/
 
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include <8051.h>
 
/* =====================================================================
------------------------ Vakiot ja makrot --------------------------- */
 
/* --- Tyypit */
typedef unsigned char BYTE;
typedef unsigned short WORD;
typedef __bit BIT;
 
// Tämän vakion määrittelyllä ohjelman saa toimimaan joko 12 tai 24 MHz kiteellä
//#define CYCLES_PER_US       1          // 12 MHz kide
#define CYCLES_PER_US       2          // 24 MHz kide
 
#define MAX_SERVO_NR        4          // ohjattavia servoja 5 kpl
#define SERVO_MIN           1000       // min servon ohjausarvo (us)
#define SERVO_CENTER        1500       // servon keskiasennon ohjausarvo (us)
#define SERVO_MAX           2000       // max servon ohjausarvo (us)
#define SERVO_INTERVAL      2500       // peräkkäisten pulssien väli (us)
 
#define INBUF_LEN           10
 
/* =======================================================================
------------------------ I/O ------------------------------------------ */
 
/* Servot portin P2 pinneissä P2.0 .. P2.MAX_SERVO_NR */
#define SERVO_PORT    P2
 
/* Timerit 16-bittisinä rekistereinä, helpompi käsitellä */
__sfr16 __at (0x8C8A) TMR0;
__sfr16 __at (0x8D8B) TMR1;
 
/* =====================================================================
------------------------ Tietotyypit -------------------------------- */
 
/* =====================================================================
------------------------ Globaalit muuttujat ------------------------ */
 
/* Pulssien leveydet mikrosekunteina */
int pulse_len[MAX_SERVO_NR+1];
 
/* Sarjaportin vastaanottopuskuri */
char inbuf[INBUF_LEN];
BYTE buf_idx = 0;
 
/* =====================================================================
------------------------ Funktioiden prototyypit -------------------- */
 
char getchar( void );
void putchar( char ch );
void putstr( char *p );
void init_hw( void );
 
/* =====================================================================
Pääohjelma
--------------------------------------------------------------------- */
 
void main( void )
{
    __data char *p;
    char ch;
    BYTE servo_nr;
    int plen;
 
    /* Laitteiston alustukset */
    init_hw();
 
    /* Kaikki servot keskelle */
    for (servo_nr = 0; servo_nr <= MAX_SERVO_NR; servo_nr++)
    {
        pulse_len[servo_nr] = SERVO_CENTER;
    }
 
    EA = 1;          // Keskeytysten sallinta
 
    /* --- Pääsilmukka --- */
    while (1) 
    {
        /* Odotetaan merkkiä sarjaliitännästä ja tulkitaan se */
        ch = getchar();
 
        if (ch == '\r')
        {
            /* Komento saatu, tulkitaan */
 
            inbuf[buf_idx] = '\0';   // loppumerkki puskuriin
 
            /* Etsitään pilkku */
            p = (__data char *)strchr(inbuf,',');
            if (p == NULL)
            {
                putstr("Syntax error\r\n");
                buf_idx = 0;
                continue;
            }
 
            /* Erotellaan servon numero ja pulssin pituus */
            *p = '\0';
            servo_nr = (BYTE)atoi(inbuf);
            plen = atoi(p+1);
 
            if (servo_nr > MAX_SERVO_NR)
            {
                putstr("Illegal servo nr\r\n");
                buf_idx = 0;
                continue;
            }
 
            if (plen < SERVO_MIN || plen > SERVO_MAX) 
            {
                putstr("Illegal pulse length\r\n");
                buf_idx = 0;
                continue;
            }
 
            /* Kielletään timer-keskeytys taulukkoon kirjoittamisen ajaksi, 
               jotta mahdollinen timer-keskeytys ei näe keskeneräistä dataa. */
            ET0 = 0;
            pulse_len[servo_nr] = plen;
            ET0 = 1;
 
            buf_idx = 0;  // puskuri tyhjäksi
        }
        else if (isdigit(ch) || ch == ',')  // hyväksytään vain numerot ja pilkku
        {   
            /* Merkki puskuriin */
            if (buf_idx < sizeof(inbuf))
            {
                inbuf[buf_idx] = ch;   
                buf_idx++;
            }
        }
    }
}
 
/* =======================================================================
Lukee merkin sarjaliitännältä.
----------------------------------------------------------------------- */
 
char getchar( void )
{
    char ch;
 
    while (!RI);
    RI = 0;
 
    ch = SBUF & 0x7F;
 
    /* Kaiutus */
    putchar(ch);
 
    if (ch == '\r')
        putchar('\n');
 
   return ch;
}
 
/* =======================================================================
Lähettää merkin sarjaliitäntään.
----------------------------------------------------------------------- */
 
void putchar( char ch )
{
   TI = 0;
   SBUF = ch;
 
   while (!TI);
}
 
/* =======================================================================
Lähettää merkkijonon sarjaliitäntään.
----------------------------------------------------------------------- */
 
void putstr( char *p )
{
    while (*p)
    {
        putchar(*p);
        p++;
    }
}
 
/* =======================================================================
Alustaa laitteiston
----------------------------------------------------------------------- */
 
void init_hw( void )
{
    /* --- Ajastimet --- */
    /* Timer 0: servojen pulssitus */
    /* Timer 1: baudigeneraattori 9600 b/s tai 4800 b/s */
    TMOD = 0x21;    // Tmr 0 = mode 1, timer; Tmr 1 = mode 2, timer
    TCON = 0x00;    // IEx = 0, TRx = 0, TFx = 0 
    TMR0 = 0xFFFF;  // keskeytys heti
    TR0 = 1;        // Timer 0 käyntiin
 
    TH1 = 0xF3;     // 9600 b/s @ 24 MHz tai 4800 b/s @ 12 MHz 
    PCON = 0x80;    // SMOD = 1
    TR1 = 1;        // Timer 1 käyntiin
 
    /* --- Sarjaportti --- */
    SM1 = 1;        // Mode 1
    REN = 1;        // Receive enable
 
    /* --- Keskeytykset --- */
    IP = 0x00;      // Prior.0: kaikki
    IE = 0x02;      // Enable: Tmr0; Disable: muut  (EA=1 vasta pääohjelmassa)
}
 
/* ======================================================================= */
/* =====================       KESKEYTYSPALVELUT      ==================== */
/* ======================================================================= */
 
/* =======================================================================
 Timer 0: generoi servopulssit.
 
 Keskeytyksessä käydään joka kerta, kun jonkun servon pulssin tilaa pitää
 vaihtaa, eli 2 kertaa per servo. Samalla timeriin alustetaan aika seuraavaan
 tapahtumaan.
 
 Halutut pulssien leveydet mikrosekunteina ovat taulukossa pulse_len[]. 
 Taulukkoa indeksoidaan servon numerolla. 
 
 Keskeytyspalvelussa lasketaan valmiiksi seuraavaa keskeytyskertaa
 varten timeriin ladattava arvo sekä menossa olevaa servoa vastaava maski.
 Näin pulssin aloitus/lopetus ja timerin uudelleen lataus saadaan tehtyä 
 heti, kun tullaan keskeytyspalveluun, tarvitsematta käyttää aikaa arvojen 
 laskemiseen.
 
 Kahden eri servon pulssien alkuhetkien väli on SERVO_INTERVAL (2.5 ms).
 Tällöin jää aikaa tehdä laskenta silloinkin, kun edelliselle servolle 
 generoidaan maksimittaista pulssia (2 ms). 
 
 Viimeisen servon jälkeen pidetään pitempi väli, jotta yhden servon pulssien
 alkuhetkien väliksi tulee 20 ms.
 
 Lipulla pulse_start pidetään lukua, ollaanko generoimassa pulssin alkua
 vai loppua. Joka toinen kesksytyskerta on aina pulssin alku, joka toinen 
 loppu. Servo vaihtuu kahden keskeytyksen välein.
 
 Keskeytyspalvelun suoritusaika 24 MHz kiteellä on n. 30 us.
 
----------------------------------------------------------------------- */
 
void timer0_isr(void) __interrupt(TF0_VECTOR) __using (1) 
{
    static BIT pulse_start = 1;
    static int next_timer_val = -(SERVO_CENTER * CYCLES_PER_US);
    static BYTE next_mask = 0x01;
    static BYTE cur_servo = 0;
    int last_interval;
 
    /* Servopulssi päälle tai pois */
    if (pulse_start)
        SERVO_PORT |= next_mask;
    else
        SERVO_PORT &= ~next_mask;
 
    /* Alustetaan timer 0:aan aiemmin laskettu arvo */
    TF0 = 0;
    TMR0 = next_timer_val;
    TR0 = 1;
 
    /* Uusi pulssi tai pulssien väli on käynnissä, joten nyt on hyvää
       aikaa laskea seuraava maski ja ajastimen arvo */
 
    pulse_start = !pulse_start;  // vaihdetaan lipun tila
 
    if (pulse_start)
    {
        /* Seuraavaksi tulee pulssin alku, joten seuraava servo käsittelyyn. */
        cur_servo++;
        if (cur_servo > MAX_SERVO_NR)
            cur_servo = 0;
 
        /* Lasketaan seuraavan servon pulssin pituus ja servoa vastaava maski */
        next_timer_val = -(pulse_len[cur_servo] * CYCLES_PER_US);
 
        next_mask = 1 << cur_servo;
    }
    else
    {
        /* Seuraavaksi tulee pulssin loppu, joten pysytään samassa servossa. 
           Lasketaan väli pulssin lopusta seuraavan pulssin alkuun. */
 
        /* Jos ollaan viimeisessä servossa, tulee pitempi väli, jotta saadaan
           20 ms täyteen */
        if (cur_servo == MAX_SERVO_NR)
            last_interval = SERVO_INTERVAL*(7-MAX_SERVO_NR);
        else
            last_interval = 0;
 
        next_timer_val = -((SERVO_INTERVAL - pulse_len[cur_servo] + last_interval) * CYCLES_PER_US);  
 
        /* Maski pysyy samana, koska servo ei vaihtunut */
    }
}
 
/* ============================ EOF ====================================== */