Hallo
Programme für so viele Servos steuern meist die Servos nacheinander an (günstig wegen Anlaufströmen) und scheitern meist an den 20ms Wiederholungen (Stochri, Dirk). Geschwindigkeitsregelungen können sie normalerweise auch nicht (wenn man vom unschönen weil stottrigem Verlängern der 20ms absieht). Gleich vorweg, ich kann das natürlich auch nicht, aber ich habe einen netten Ansatz für viele Servos am Mega32:
Eher durch Zufall habe ich eine nette Methode gefunden um viele Servos mit wenig Overhead zu betreiben. Ich nutze einen Timer mit günstigen Prescaler und erzeuge die Impulse in zwei Teilen: Ein Grundimpuls der für alle Servos gleich ist (kann man vielleicht später zur Nullstellungsjustage verwenden) und einem variablen Anteil der den Drehwinkel des Servos einstellt. Hier eine Variante mit dem 8Bit-Timer 2 im Overflowmode, Stellbereich der Servopositionen ist bytefreundlich von ca. 0-255:
// Servos ansteuern mit 8MHz Mega32 und 8-Bit Timer2 Overflow-ISR 22.3.2009 mic
// Die Servosimpulse werden nacheinander erzeugt. Die Impulsdauer jedes Servos
// setzt sich aus einem Grundimpuls (der für alle Servos gleich ist) und seinem
// Positionswert zwischen 0 und 255 zusammen.
// In der ISR werden im Wechsel ein Grundimpuls und ein Positionswert erzeugt
// und zum jeweiligen Servo gesendet. Nach den Servoimpulsen wird eine
// Pause eingefügt um die 50Hz Wiederholfrequenz (20ms) zu erzeugen.
// Diese auf acht Servos aufgebohrte Version scheint zu funktionieren,
// ich habe es allerdings nur mit angeschlossenen Servos 1-4 ausprobiert.
#include <avr/io.h>
#include <avr/interrupt.h>
// Servoausgänge 1-8
#define servoinit {DDRB |= (1<<PB7); PORTB &= ~(1<<PB7); DDRC |= 0b01110000; PORTC &= ~0b01110000;}
#define servo1on PORTC |= (1<<PC4)
#define servo1off PORTC &= ~(1<<PC4)
#define servo2on PORTC |= (1<<PC5)
#define servo2off PORTC &= ~(1<<PC5)
#define servo3on PORTC |= (1<<PC6)
#define servo3off PORTC &= ~(1<<PC6)
#define servo4on PORTB |= (1<<PB7)
#define servo4off PORTB &= ~(1<<PB7)
#define servo5on PORTB |= (1<<PB0) // Dummyservos 4-8 an SL6
#define servo5off PORTB &= ~(1<<PB0)
#define servo6on PORTB |= (1<<PB0)
#define servo6off PORTB &= ~(1<<PB0)
#define servo7on PORTB |= (1<<PB0)
#define servo7off PORTB &= ~(1<<PB0)
#define servo8on PORTB |= (1<<PB0)
#define servo8off PORTB &= ~(1<<PB0)
uint8_t servo1, servo2, servo3, servo4, servo5, servo6, servo7, servo8;
int main(void)
{
servoinit; // Datenrichtung der Servopins einstellen
//Timer2 Initialisierung
// für 8MHz Takt:
TCCR2 = (0 << WGM21) | (0 << COM20) | (1 << CS22); // Normal Mode, prescaler /64
// für 16MHz Takt:
//TCCR2 = (0 << WGM21) | (0 << COM20) | (1 << CS22) | (1 << CS20); // /128
TIMSK |= (1 << TOIE2); // Timer2 Overflow-Interrupt erlauben -> Servos an
//TIMSK &= ~(1 << TOIE2); // Timer2 Overflow-Interrupt verbieten -> Servos aus
sei();
servo1=125; // Mittelposition, Drehbereich ist von 0-255!
servo2=125;
servo3=125;
servo4=125;
servo5=125;
servo6=125;
servo7=125;
servo8=125;
while(1) // Hauptschleife
{
}
return(0);
}
ISR (TIMER2_OVF_vect)
{
static uint8_t servo_nr=0, grundimpuls=0; // Gestartet wird am Ende der Pause
static uint16_t impulspause;
if(servo_nr)
{
// Endweder wird hier der Grundimpuls erzeugt (Länge 56 Einheiten)
if(grundimpuls++ & 1) { TCNT2=200; impulspause-=256-200; } else
// Oder der zur Servoposition gehörende Impuls (0-255, 0 ist der längste Impuls!)
{
if(servo_nr==1) {TCNT2=servo1; servo1on; impulspause-=servo1;}
if(servo_nr==2) {TCNT2=servo2; servo1off; servo2on; impulspause-=servo2;}
if(servo_nr==3) {TCNT2=servo3; servo2off; servo3on; impulspause-=servo3;}
if(servo_nr==4) {TCNT2=servo4; servo3off; servo4on; impulspause-=servo4;}
if(servo_nr==5) {TCNT2=servo5; servo4off; servo5on; impulspause-=servo5;}
if(servo_nr==6) {TCNT2=servo6; servo5off; servo6on; impulspause-=servo6;}
if(servo_nr==7) {TCNT2=servo7; servo6off; servo7on; impulspause-=servo7;}
if(servo_nr==8) {TCNT2=servo8; servo7off; servo8on; impulspause-=servo8;}
if(servo_nr==9) {servo8off; servo_nr=0;}
if(servo_nr) servo_nr++;
}
}
else
// Anschliessend wird die Impulspuse erzeugt. Sie ergibt sich aus der Startlänge-
// der Summe der einzelnen Impulslängen. Bei acht Servos errechnet sich der
// kleinste benötigte Startwert für Impulspause etwa so:
// 8*56 + 8*256 = 2496 (Summe der Grundimpulse + Summe der Positionsimpulse)
{
if(impulspause>256) impulspause-=256; // Gesamtpause in 256er-Schritten
else {TCNT2=-impulspause; servo_nr++; impulspause=3000;} // die Restpause
}
}
Das funktioniert recht gut. Diese weitere Variante mit 18 Servos verwendet Timer1 im CTC-Mode und funktioniert nur bedingt:
// 18 Servos ansteuern mit 8MHz-Mega32 und 16-Bit Timer1 24.3.2009 mic
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdlib.h>
#define systemtakt 1 // 1 bei 8MHz, 2 bei 16MHz-Prozessortakt
#define grundimpuls 47 // grundimpuls + 125 sollte Servomitte sein
// Servoausgänge A 1-9
#define servoainit {DDRB |= (1<<PB7); PORTB &= ~(1<<PB7);}
#define servoa1on PORTB |= (1<<PB7)
#define servoa1off PORTB &= ~(1<<PB7)
#define servoa2on PORTB |= (1<<PB0)
#define servoa2off PORTB &= ~(1<<PB0)
#define servoa3on PORTB |= (1<<PB0)
#define servoa3off PORTB &= ~(1<<PB0)
#define servoa4on PORTB |= (1<<PB0)
#define servoa4off PORTB &= ~(1<<PB0)
#define servoa5on PORTB |= (1<<PB0) // Dummyservoas 4-9 an SL6
#define servoa5off PORTB &= ~(1<<PB0)
#define servoa6on PORTB |= (1<<PB0)
#define servoa6off PORTB &= ~(1<<PB0)
#define servoa7on PORTB |= (1<<PB0)
#define servoa7off PORTB &= ~(1<<PB0)
#define servoa8on PORTB |= (1<<PB0)
#define servoa8off PORTB &= ~(1<<PB0)
#define servoa9on PORTB |= (1<<PB0)
#define servoa9off PORTB &= ~(1<<PB0)
// Servoausgänge B 1-9
#define servobinit {DDRC |= 0b01110000; PORTC &= ~0b01110000;}
#define servob1on PORTC |= (1<<PC4)
#define servob1off PORTC &= ~(1<<PC4)
#define servob2on PORTC |= (1<<PC5)
#define servob2off PORTC &= ~(1<<PC5)
#define servob3on PORTC |= (1<<PC6)
#define servob3off PORTC &= ~(1<<PC6)
#define servob4on PORTB |= (1<<PB0)
#define servob4off PORTB &= ~(1<<PB0)
#define servob5on PORTB |= (1<<PB0) // Dummyservobs 4-9 an SL6
#define servob5off PORTB &= ~(1<<PB0)
#define servob6on PORTB |= (1<<PB0)
#define servob6off PORTB &= ~(1<<PB0)
#define servob7on PORTB |= (1<<PB0)
#define servob7off PORTB &= ~(1<<PB0)
#define servob8on PORTB |= (1<<PB0)
#define servob8off PORTB &= ~(1<<PB0)
#define servob9on PORTB |= (1<<PB0)
#define servob9off PORTB &= ~(1<<PB0)
volatile uint8_t p; // 20ms-Timer
uint16_t servoa1, servoa2, servoa3, servoa4, servoa5, servoa6, servoa7, servoa8, servoa9;
uint16_t servob1, servob2, servob3, servob4, servob5, servob6, servob7, servob8, servob9;
/************************* Ausgabe an Terminal ********************************/
void writeChar(char ch) {while (!(UCSRA & (1<<UDRE))); UDR = (uint8_t)ch;}
void writeString(char *string) {while(*string) writeChar(*string++);}
void writeInteger(int16_t number, uint8_t base)
{char buffer[17]; itoa(number, &buffer[0], base); writeString(&buffer[0]);}
/************************************************** ****************************/
int main(void)
{
/************************ UART-Setup für RP6 *******************************/
#define BAUD_LOW 38400 //Low speed - 38.4 kBaud
#define UBRR_BAUD_LOW ((F_CPU/(16*BAUD_LOW))-1)
UBRRH = UBRR_BAUD_LOW >> 8; // Baudrate is Low Speed
UBRRL = (uint8_t) UBRR_BAUD_LOW;
UCSRA = 0x00;
UCSRC = (1<<URSEL)|(1<<UCSZ1)|(1<<UCSZ0);
UCSRB = (1 << TXEN) | (1 << RXEN) | (1 << RXCIE);
/************************************************** *************************/
servoa1=125; // Drehbereich ist ca. 10-245!
servoa2=125;
servoa3=125;
servoa4=125;
servoa5=125;
servoa6=125;
servoa7=125;
servoa8=125;
servoa9=125;
servob1=125;
servob2=125;
servob3=125;
servob4=125;
servob5=125;
servob6=125;
servob7=125;
servob8=125;
servob9=125;
//servoa1=servoa2=servoa3=servoa4=servoa5=servoa6=se rvoa7=servoa8=servoa9=60; // Test
//servob1=servob2=servob3=servob4=servob5=servob6=se rvob7=servob8=servob9=60; // Test
servoainit; // Datenrichtung der Servopins A einstellen
servobinit; // Datenrichtung der Servopins B einstellen
//Timer1 Initialisierung
TCCR1A = 0;
TCCR1B = (0<<CS12) | (1<<CS11) | (1<<CS10); // Prescaler /64
TCCR1B|= (1<<WGM12); // CTC-Mode
OCR1A=100; // 100*64 Takte bis zum ersten Interrupt
OCR1B=100;
TIMSK |= (1 << OCIE1A);
TIMSK |= (1 << OCIE1B);
sei(); // ... und los!
while(1) // Hauptschleife
{
writeChar('*');
writeChar('\n');
servoa1=115;
servob1=115;
p=50; while(p); // Das sollte ungefähr 50*20ms=1 Sekunde verzögern
servoa1=135;
servob1=135;
p=50; while(p);
}
return(0);
}
ISR (TIMER1_COMPA_vect)
{
uint16_t temp=grundimpuls;
static uint8_t servob_nr=1;
static uint16_t impulspause=3000;
if(servob_nr==1) {temp+=servob1; servob1on;}
if(servob_nr==2) {temp+=servob2; servob1off; servob2on;}
if(servob_nr==3) {temp+=servob3; servob2off; servob3on;}
if(servob_nr==4) {temp+=servob4; servob3off; servob4on;}
if(servob_nr==5) {temp+=servob5; servob4off; servob5on;}
if(servob_nr==6) {temp+=servob6; servob5off; servob6on;}
if(servob_nr==7) {temp+=servob7; servob6off; servob7on;}
if(servob_nr==8) {temp+=servob8; servob7off; servob8on;}
if(servob_nr==9) {temp+=servob9; servob8off; servob9on;}
if(servob_nr >9) {temp =impulspause; servob9off; servob_nr=0;}
OCR1A=temp*systemtakt;
if(servob_nr) impulspause-=temp; else impulspause=3000;
servob_nr++;
}
ISR (TIMER1_COMPB_vect)
{
uint16_t temp=grundimpuls;
static uint8_t servoa_nr=1;
static uint16_t impulspause=3000;
switch(servoa_nr)
{
case 1: temp+=servoa1; servoa1on; if(p) p--; break;
case 2: temp+=servoa2; servoa1off; servoa2on; break;
case 3: temp+=servoa3; servoa2off; servoa3on; break;
case 4: temp+=servoa4; servoa3off; servoa4on; break;
case 5: temp+=servoa5; servoa4off; servoa5on; break;
case 6: temp+=servoa6; servoa5off; servoa6on; break;
case 7: temp+=servoa7; servoa6off; servoa7on; break;
case 8: temp+=servoa8; servoa7off; servoa8on; break;
case 9: temp+=servoa9; servoa8off; servoa9on; break;
default:temp =impulspause; servoa9off; servoa_nr=0; break;
}
OCR1B=temp*systemtakt;
if(servoa_nr) impulspause-=temp; else impulspause=3000;
servoa_nr++;
}
Das Problem hierbei ist dass nur eine der beiden ISR ausgeführt wird, Ursache habe ich noch nicht gefunden (weil ich grad mal wieder was anderes spannender finde). An der if/case-Variante oder am TimerA/B liegt es scheinbar nicht, wenn man die ISR über Kreuz umbenennt wandert das Problem mit. Vielleicht mag das ja jemand aufgreifen und verbessern.
Alternativ würde sich ein fertiges Servoboard wie das Micro Servoboard (http://www.shop.robotikhardware.de/shop/catalog/product_info.php?cPath=67&products_id=118) anbieten. Die Investition würde sich durch kompakte Bauweise und weniger Nervenaufwand rentieren, möglicherweise findet sich sogar ein Sponsor dafür ;)
Gruß
mic