[S] Programm für 12 Servos mit Geschwindigkeitsregelung

Hallo,

für meinen Wall E such ich ein Programm mit dem ich 12 Servos ansteuern kann, möglichst auch noch mit Geschwindigkeitskontrolle, für einen Mega32.

Falls jemand so einen Code schon geschrieben hat würde ich mich freuen wenn er mir diesen zur Verfügung stellt. Sonst muss ich mir wirklich selbst was stricken.

viele Grüße Hanno

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:

Code:

// 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:

Code:

// 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=servoa7=servoa8=servoa9=60; // Test //servob1=servob2=servob3=servob4=servob5=servob6=servob7=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 anbieten. Die Investition würde sich durch kompakte Bauweise und weniger Nervenaufwand rentieren, möglicherweise findet sich sogar ein Sponsor dafür ;)

Gruß

mic

@radbruch:

Zitat:

... und scheitern meist an den 20ms Wiederholungen (Stochri, Dirk).

[-X [-X [-X
??? Bei meinen verschiedenen Lösungen für den RP6 scheitert gar nichts, sondern auch die Impulswiederholung funktioniert gut. Das alles mit recht wenig Interrupt-Last und unter Nutzung der Original-Libraries.

Gruß Dirk

Ich meinte mehr als 8-10 Servos scheitern daran. Man muss dann eben zwei "Kanäle" zusammenbasteln. Und zu kompliziert sollte es auch nicht sein :)

Es geht auch extern mit dem SD21 Board. Dieser wird über I2C angesteuert. Dieses Board kann bis zu 21 Servos ansteuern. Es werden die Daten für Geschwindigkeit und Position gesendet. Datenblatt gibt es hier: http://www.robotikhardware.de/download/sd21.pdf

Ich hab bisher auch nen Algo gehabt der eben 10 Servos ansteuern kann aber auch nicht mehr leider. Für die Geschwindigkeitskontrolle dachte ich daran die Bewegung zur Zielposition in viele Zwischenschritte zu unterteilen und dazwischen ein sleep intervall definierter Länge zu legen.

Von einem Servoboard würde ich gerne absehen auch wenn es vermutlich die geschmeidigste Lösung meines Problems wäre. Aber das sind auch wieder 40€ und ich hab schon 95€ für die Servos hingelegt.

Hallo,

ich habe mal aufgeschnappt, dass die Servos einen "Totpunkt" haben. Dieser bewirkt, dass nur Impulslängenänderungen die größer als dieser Totpunkt sind, auch vom Servo nachgefahren werden. Das beschränkt die Schrittweite wenn du den Gesamtweg in kleine Einzelschritte zerlegen möchtest. Hier wird dieser Totpunkt vergrößert um das Servo unempfindlicher zu machen, für unsere Anwendungen müßte der Kondensator wohl verkleinert werden:
http://www.goetzbirkner.de/tipps_und...servoumbau.htm
(Der Frame stammt von hier: http://www.goetzbirkner.de)

Ich habe meine Servos bisher im orginalen Zustand betrieben und das Zerlegen des Weges in kleine Einzelschritte hat leidlich gut funktioniert. Damit kann man recht hübsche Effekte erzielen, z.B. kann man mehrere Servos bei verschieden langen Stellwegen gleichzeitig im Ziel ankommen lasssen. So habe ich die Wege meines Deltas vorgegeben (Link in Signatur)

Gruß

mic

Wenn es eine günstigere Fertiglösung als das DS21 Board sein darf:
http://www.roboter-teile.de/Shop/the...id=24&source=2

Ist nichts anderes als ein programmierter PIC.

Frage: Warum kann das ein 8MHz PIC und ein Mega32 nicht? Antwort: Weil es der PIC auch nicht kann ;)

Zitat:

Wiederholrate

Die Wiederholrate ergibt sich aus der Zeit zwischen den Impulsen für jeden Servo. Die Servoausgänge sind inaktiv solange bis die entsprechende Position gesetzt wird. Die Wiederholzeit ist abhängig von der Anzahl der angeschlossenen Servos und beträgt 20ms, sofern die Summe der angeschlossenen Ausgänge (Zeiten) kleiner / gleich 20ms ist. Anderenfalls erhöht sich die Wiederholzeit um den entsprechenden Betrag.

(Aus Seite 4 der Doku: http://www.roboter-teile.de/datasheets/sd20.pdf)

Variable Verstellgeschwindigkeit hat er auch nicht und die feste I2C-Adresse könnte möglicherweise stören. Trotz des niedrigen Preises finde ich hier selbercoden sportlicher.

Gruß

mic

Meiner Erfahrung nach hängt es sehr vom verwendeten Servo ab wie empfindlich dieses auf eine Verlängerung der Wiederholrate reagiert...

... zumal ja beim WallE-Nachbau vermutlich nie alle Servos gleichzeitig laufen müssen. Dann allerdings stellt sich die Frage ob der PIC ein Servo auch wieder aus der Kette entfernen kann wenn er gerade nicht aktiv sein muss: "- Servos sind inaktiv nach einschalten bis zur Übertragung der ersten Position"

"Da der Pin 6 (Servo5) des SD20 keinen internen Pull-Up Widerstand besitzt, muss dieser extern bestückt werden, dazu einen Widerstand mit 4,7K von Pin6 nach +5V schalten"
Warum verwenden die wohl die PullUps? Mein Bauchgefühl würde eher zu PullDowns tendieren.

Ich würde auch ganz gerne davon absehen noch einen weiteren µC oder Pic nur für die Servos zu verwenden wenn ich eh schon einen Mega32 dafür reserviere.

Wunschvorstellung wäre ein Programm bei dem ich immer 4 Servos gleichzeitig bewegen kann (entspricht einem Arm oder dem Kopf). Nur ganz wenige Prozesse erfordern es, dass sich beide Arme gleichzeitig bewegen müssten.

Ich kann mir nicht vorstellen, dass sowas nicht möglich ist, aber ich kann es selber nicht, da ich in C für den µC nicht so fit bin.

Jemand eine Idee wie sowas zu realisieren wäre?

Hmmm, vielleicht die Servosignale per 16-Bit Timer generieren, in einer Routine dann einfach die Servowerte entsprechend der gewünschten Geschwindigkeit bearbeiten (soferns keine Brems/Beschleunigungsrampen sein sollen, dürfte doch einfaches Addieren/Subtrahieren reichen?)...

So hätte ich's vermutlich gemacht, allerdings nicht in C, sondern Bascom.

Viele Grüße

Hallo,

also ohne Beschleunigungs/Bremsrampen sollte das mit einem Timerinterrupt problemlos möglich sein. Kommt hald darauf an was der Mega32 sonnst noch macht bzw. wo er die Daten für die Servos herbekommt.
Du kannst auch mal n bisschen mit google suchen, da gibts mehrere Projekte wo man viele Servos mit nem AVR ansteuert. Hab nur kein link zur Hand, sonnst würd ich ihn hier posten!
MFG Mixxer

@HannoHupmann:

Zitat:

Für die Geschwindigkeitskontrolle dachte ich daran die Bewegung zur Zielposition in viele Zwischenschritte zu unterteilen und dazwischen ein sleep intervall definierter Länge zu legen.

Ich würde die Funktion, die die Bewegungen machen soll, in einem festen "Takt" aufrufen. Der Takt könnte z.B. auch Interrupt-getriggert sein.
Das ist besser, als Wartezeiten zwischen den Servopositionen einzulegen.
Eine weitere Feinabstimmung der Servogeschwindigkeit ist durch die Abstufung der Servopositionen möglich, d.h. ich kann die Positionen (im Takt, s.o.) jeweils um 1 verändern, natürlich aber auch um 2 oder 5 oder 10 und damit eine höhere Geschwindigkeit erreichen. Damit lassen sich Servos (bei gleichem Takt) unterschiedlich schnell zur Zielposition bewegen.

Gruß Dirk

Der MEGA32 hat sonst nicht viel zu tun, ab und an mal seinen AD Wandler auslesen und den Wert über I²C rausgeben. Was er mit den Servos machen soll bekommt er auch über I²C gesagt. D.h. es zum Beispiel dann: "Arm hoch" oder "Kopf drehen" und dann wird ein festes Programm ausgeführt.

Für mehr Aktionen müsste ich vermutlich noch mehr µC einbauen. Vielleicht ist das auch besser da ich mit den Ports langsam knapp werde und doch noch so einiges zu tun ist.