-         

Seite 1 von 2 12 LetzteLetzte
Ergebnis 1 bis 10 von 16

Thema: [S] Programm für 12 Servos mit Geschwindigkeitsregelung

  1. #1
    Moderator Robotik Einstein Avatar von HannoHupmann
    Registriert seit
    19.11.2005
    Ort
    München
    Alter
    34
    Beiträge
    4.528
    Blog-Einträge
    1

    [S] Programm für 12 Servos mit Geschwindigkeitsregelung

    Anzeige

    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

  2. #2
    Moderator Robotik Visionär Avatar von radbruch
    Registriert seit
    27.12.2006
    Ort
    Stuttgart
    Alter
    54
    Beiträge
    5.781
    Blog-Einträge
    8
    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

    Atmel’s products are not intended, authorized, or warranted for use
    as components in applications intended to support or sustain life!

  3. #3
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.791
    @radbruch:
    ... 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

  4. #4
    Moderator Robotik Visionär Avatar von radbruch
    Registriert seit
    27.12.2006
    Ort
    Stuttgart
    Alter
    54
    Beiträge
    5.781
    Blog-Einträge
    8
    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 :)

    Atmel’s products are not intended, authorized, or warranted for use
    as components in applications intended to support or sustain life!

  5. #5
    Erfahrener Benutzer Robotik Einstein Avatar von 021aet04
    Registriert seit
    17.01.2005
    Ort
    Niklasdorf
    Alter
    29
    Beiträge
    4.544
    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

  6. #6
    Moderator Robotik Einstein Avatar von HannoHupmann
    Registriert seit
    19.11.2005
    Ort
    München
    Alter
    34
    Beiträge
    4.528
    Blog-Einträge
    1
    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.

  7. #7
    Moderator Robotik Visionär Avatar von radbruch
    Registriert seit
    27.12.2006
    Ort
    Stuttgart
    Alter
    54
    Beiträge
    5.781
    Blog-Einträge
    8
    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

    Atmel’s products are not intended, authorized, or warranted for use
    as components in applications intended to support or sustain life!

  8. #8
    Erfahrener Benutzer Roboter Experte
    Registriert seit
    20.01.2004
    Alter
    28
    Beiträge
    645
    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.
    MfG Xtreme
    RP6 Test - alles zum Nachfolger des bekannten RP5 im neuen RP6 Forum!

  9. #9
    Moderator Robotik Visionär Avatar von radbruch
    Registriert seit
    27.12.2006
    Ort
    Stuttgart
    Alter
    54
    Beiträge
    5.781
    Blog-Einträge
    8
    Frage: Warum kann das ein 8MHz PIC und ein Mega32 nicht? Antwort: Weil es der PIC auch nicht kann ;)

    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

    Atmel’s products are not intended, authorized, or warranted for use
    as components in applications intended to support or sustain life!

  10. #10
    Erfahrener Benutzer Roboter Experte
    Registriert seit
    20.01.2004
    Alter
    28
    Beiträge
    645
    Meiner Erfahrung nach hängt es sehr vom verwendeten Servo ab wie empfindlich dieses auf eine Verlängerung der Wiederholrate reagiert...
    MfG Xtreme
    RP6 Test - alles zum Nachfolger des bekannten RP5 im neuen RP6 Forum!

Seite 1 von 2 12 LetzteLetzte

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •