Servo motor vezérlés
2014-08-30
Nemrég, hazafelé a buszról felfedeztem, hogy útközben van egy modellezéssel foglalkozó bolt. Korábban is piszkálta a fantáziámat a servo motorok vezérlése, meg mostanában vacakoltam a pwm vezérléssel, hát leszálltam és fejlesztési célból megvettem a legolcsóbb (1100HUF) motort. Nézegettem, hogyan kell pwm jelel vezéreln a motort. Nálam jobban hozzáértők azt írták, hogy 20 msec (50Hz) sűrűségű impulzusok kellenek hozzá. Az impulzus hossza határozza meg a motorra szerelt kar elfordulását. Hozzávetőleg 1,5msec a közép állás, azután motorfüggő melyik motor milyen hosszú impulzusra mennyit fordul el. Kicsit részletesebben, aki még nem foglalkozott vele, itt egy olyan motorról van szó, amelyik egybe van szerelve egy jó nagy fogaskerék áttétellel, amiből kilóg egy tengely. Erre egy kart lehet szerelni, ami azután mozgathatja a repülőnk kormánylapját, a hajónk kormányát, vagy amivel összeszerkesztjük. A tengelyre belül rá van szerelve egy potméter, és ez alapján a kis készülékben lévő szabályzó áramkör, a vezérlő impulzusoknak megfelelő szögig (+/-90) fok forgatja el a tengelyt.
Nem nagyon fért a fejembe, minek kell ehhez pwm. Csinál az ember az egyik számlálóval egy 50Hz-es megszakítást, azután az egyes csatornákra sorra kiadja megfelelő hosszúságú impulzust, arra meg ott vannak a _delay_us() fuggvény. Itt történt az első tévedésem. Ez nem függvény, hanem makro. Lehet függvényként is használni, de akkor nem fordítás alatt értékelődik ki, hanem futás alatt, amihez a fordító hozzávag a kódhoz kb, 4kB lebegőpontos aritmetikát. Kellet írni egy saját időzítőt, amihez T1-t használtam fel. A T0 valójában 1000Hz-es megszakításra lett beálítva, és számlálja, hogy mikor telt le 20 msec.
A második tévedésem az volt, hogy a globálisan definiált a..d_usec változókat nem volatile-nek definiáltam. Így utólag, sok óra kinlódás után már csak kicsit görcsösen somolyog az ember... :).
Az egyes servo motorokhoz meg lehet adni a két impulzushossz szélső értékét. Szebb megoldás lett volna eeprom-ba letárolni őket, de hát nem lőhetek le minden feladatot a követőim elöl :).
Az impulzus hossz kiszámolása azért lett egy kicsit komplikáltabb, mert így benne lehet maradni a unsigned int tartományban.
Így néz ki a deszka modell. Vannak itt ide nem kellő alkatrészek is. A motorok 5V környékéről szeretnek járni. A középső piros vezeték +5V, a szélső fekete (az enyémen barna) a GND, és a sárga a vezérlés.
A programban benne hagytam a debugg-oláshoz betett részeket (A servo állítása). Ehhez a még készülő sajat printf() fuggvényből is be kellet emelnem a számból string-re alakító függvényemet. De ezek kihagyhatók belőle. Az egyes csatornákat is az éppen szabad lábakra tettem, természetesen ezek is áthelyezhetőek, valamint ezt a feladatot egy kisebb ATMEL uC-vel is meg lehet oldani :).
/*******************************************************************************
* Author - Kiraly Tibor
* http://www.tkiraaly.hu
* Date - 2014.08.29.
* Chip - Atmel ATmega8
* Compiler - avr-gcc (WinAVR)
*
* 4 csatornas servo vezerlo
*
* Soros vonalon az alabbi parancsokkal vezerelheto:
* A0, Enter - A servo 0 fokba
* A90, Enter - A servo 90 fokba
* B157, Enter - B servo 157 fokba
* ...
* A servok A..D kozott, az elfordulas 0..180 fok kozott adhato meg
* X - osszes servo kozepre (90 fok) all
* +/- - bemereshez A servo impulzus szelesseget 50us-kent allitja
*
********************************************************************************
* PonyProg Configuration and Security Bits (bepipalva):
*
* CKSEL3, CKSEL2, CKSEL1, CKSEL0
* P - P P 0100 Calibrated Internal RC Oscillator 8MHz
*
* Calibrated Internal RC Oscillator
* SUT1 SUT0
* - P Slowly rising power
*
********************************************************************************
*
* Soros port:
* 1 VCC
* 2 X
* 3 RX - AVR RXD - 2
* 4 TX - ACR TXD - 3
* 5 GND
*
* Programozo fej:
* 1 RST - AVR NRES - 1
* 2 X
* 3 SCK - AVR SCK - 19
* 4 MISO - AVR MISO - 18
* 5 MOSI - AVR MOSI - 17
* 6 GND
*
* Egyeb:
* LED - AVR PB0 - 14
* A_SERVO - AVR PB1 - 15
* B_SERVO - AVR PB2 - 16
* C_SERVO - AVR PB6 - 9
* D_SERVO - AVR PB7 - 10
*
*******************************************************************************/
#define F_CPU _8MHZ
#include "tkiraaly_atmega8.h"
#define T0_F(hz,osztas) TCNT0=F_CPU/hz/osztas-1
// Servo kimenetek
#define A_SERVO_ENABLE PB1_OUT
#define A_SERVO_BE BS( PORTB, 1)
#define A_SERVO_KI BC( PORTB, 1)
#define A_SERVO_MIN 750
#define A_SERVO_MAX 2500
#define B_SERVO_ENABLE PB2_OUT
#define B_SERVO_BE BS( PORTB, 2)
#define B_SERVO_KI BC( PORTB, 2)
#define B_SERVO_MIN 750
#define B_SERVO_MAX 2500
#define C_SERVO_ENABLE PB6_OUT
#define C_SERVO_BE BS( PORTB, 6)
#define C_SERVO_KI BC( PORTB, 6)
#define C_SERVO_MIN 750
#define C_SERVO_MAX 2500
#define D_SERVO_ENABLE PB7_OUT
#define D_SERVO_BE BS( PORTB, 7)
#define D_SERVO_KI BC( PORTB, 7)
#define D_SERVO_MIN 750
#define D_SERVO_MAX 2500
// LED - PB0
#define LED_ENABLE PB0_OUT
#define LED_BE BC( PORTB, 0)
#define LED_KI BS( PORTB, 0)
volatile U8 milisec= 0; // milisec szamlalo
volatile U8 T1_flag= 0; // T1 letelt
volatile U16 a_usec= 1500; // A servo impulzus szelessege usec-ben
volatile U16 b_usec= 1500; // B servo impulzus szelessege usec-ben
volatile U16 c_usec= 1500; // C servo impulzus szelessege usec-ben
volatile U16 d_usec= 1500; // D servo impulzus szelessege usec-ben
void usart_putc( U8); // USART egy karakter kuldese
void usartra( U8 *); // string kiiras USART-ra
void cmd( U8); // bejovo char ertelmezese
void kwait_usec( U16); // usec hosszu varakozas
U8 * kpri_U16( U16, U16, U8, U8); // szam -> string
ISR( USART_RXC_vect) // USART megszakitas kezelese
{
cmd( UDR); // bejovo char feldolgozasa
}
ISR( TIMER0_OVF_vect) // masodpercenkent 1000*
{
T0_F( 1000, 64); // 1000 Hz
milisec++;
}
ISR( TIMER1_OVF_vect) // usec meres, idozies letelt
{
T1_STOP;
T1_flag++;
}
int main( void)
{
U8 periodusido= 20; // servo-k frissitesenek surusege msec-ben
LED_ENABLE;
LED_BE;
A_SERVO_KI;
A_SERVO_ENABLE;
B_SERVO_KI;
B_SERVO_ENABLE;
C_SERVO_KI;
C_SERVO_ENABLE;
D_SERVO_KI;
D_SERVO_ENABLE;
T0_FP_64;
T0_F( 1000, 64); // 10000 Hz
IT_TIMER0_OVF_ENABLE;
IT_TIMER1_OVF_ENABLE; // usec merese
IT_ENABLE;
USART_SET_9600_8N1;
for(;;)
{
if( milisec > periodusido)
{
milisec-= periodusido;
A_SERVO_BE;
kwait_usec( a_usec);
A_SERVO_KI;
B_SERVO_BE;
kwait_usec( b_usec);
B_SERVO_KI;
C_SERVO_BE;
kwait_usec( c_usec);
C_SERVO_KI;
D_SERVO_BE;
kwait_usec( d_usec);
D_SERVO_KI;
}
}
return 0;
}
void usart_putc( U8 c) // USART egy karakter kuldese
{
while( BTC( UCSRA, UDRE)); // 1, ha kesz az adat fogadasara
UDR= c;
}
void usartra( U8 *s) // string kiiras USART-ra
{
while( *s) usart_putc( *s++);
}
void kwait_usec( U16 usec) // usec hosszu varakozas
{
TCNT1= 0xFFFF- usec;
T1_flag= 0;
T1_FP_8; // 1 MHz - 1 usec
while( !T1_flag);
}
void cmd( U8 c) // parancs feldolgozo
{
static U8 servo= 0; // melyik servo A..D
static U8 fok= 0; // hany fokra all 0..180
static U16 t= 0; // impulzus szelesseg kiszamitasahoz
switch( c) // soros vonalon beerkezett betu
{
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
if( fok < 20)
{
fok*= 10;
fok+= c- '0';
}
if( fok > 180) fok= 180; // max
break;
case 'A':
case 'B':
case 'C':
case 'D':
case 'X':
servo= c;
break;
case 'a':
case 'b':
case 'c':
case 'd':
case 'x':
servo= c- 'a'+ 'A';
break;
case '-':
if( a_usec > 100) a_usec-= 50;
usart_putc( 'A');
usart_putc( '=');
usartra( kpri_U16( a_usec, 10, 0, 0));
usart_putc( 13);
break;
case '+':
if( a_usec < 5000) a_usec+= 50;
usart_putc( 'A');
usart_putc( '=');
usartra( kpri_U16( a_usec, 10, 0, 0));
usart_putc( 13);
break;
case 13: // Enter
t= ((100* fok)/180)*10+ 750;
switch( servo)
{
case 'A':
a_usec= ((((A_SERVO_MAX- A_SERVO_MIN)/ 10) * fok) /180)* 10+ A_SERVO_MIN;
usart_putc( 'A');
usart_putc( '=');
usartra( kpri_U16( fok, 10, 0, 0));
usart_putc( '-');
usartra( kpri_U16( a_usec, 10, 0, 0));
usart_putc( 13);
break;
case 'B':
b_usec= ((((B_SERVO_MAX- B_SERVO_MIN)/ 10) * fok) /180)* 10+ B_SERVO_MIN;
break;
case 'C':
c_usec= ((((C_SERVO_MAX- C_SERVO_MIN)/ 10) * fok) /180)* 10+ C_SERVO_MIN;
break;
case 'D':
d_usec= ((((D_SERVO_MAX- D_SERVO_MIN)/ 10) * fok) /180)* 10+ D_SERVO_MIN;
break;
case 'X': // az osszes alapra, kozepre
a_usec= ( A_SERVO_MAX- A_SERVO_MIN)/ 2+ A_SERVO_MIN;
b_usec= ( B_SERVO_MAX- B_SERVO_MIN)/ 2+ B_SERVO_MIN;
c_usec= ( C_SERVO_MAX- C_SERVO_MIN)/ 2+ C_SERVO_MIN;
d_usec= ( D_SERVO_MAX- D_SERVO_MIN)/ 2+ D_SERVO_MIN;
break;
}
servo= 0;
fok= 0;
break;
}
}
/*******************************************************************************
* Author - Kiraly Tibor
* http://www.tkiraaly.hu
* Date - 2014.08.29.
*
* unsigned int szam konvertalasa a kivant alapu szamrendszerbeli string-re
*
* elojel_betu - ha 0, nincs elojel
* tizedesjegy - tizedespont utani szamjegyek szama, ha 0, nincs tizedespont
*
*******************************************************************************/
U8 * kpri_U16(
U16 szam,
U16 alap,
U8 elojel_betu,
U8 tizedesjegy
) // szam -> string
{
static U8 buffer[20];
U8 n= 0; // betuk szama
U8 * p;
p= &buffer[ sizeof( buffer)- 1];
*p= '\0'; // string vege, hatulrol elore
do
{
n++;
p--;
if( n == tizedesjegy) *p= '.'; // ha tizedespont
else
{
*p= szam% alap; // maradek a szamjegy
if( *p > 9) *p+= '@'; // 9-nel nagyobb, akkor ABC..
else *p+= '0'; // 9-ig szamjegy
szam/= alap;
}
} while( szam != 0);
if( elojel_betu) *--p= elojel_betu; // ha van elojel
return p; // mutato a stringre
}
Itt a vége, fussatok el vélem (én nem futok), legyetek az én vendégeim, innen letölthetitek a hozzávalókat összecsomagolva.
