bis zu 12 Servos gleichzeitig ansteuern

**blenderkid** · 20.03.2008, 19:51

Hi,

ich habe zwar oft einen Servo zum laufen gekriegt, aber jetzt möchte ich bis zu 12 Servos gleichzeitig laufen lassen. Die Funktionen der Servos sollten nicht blockierend sein. Weiß jemand wie das zu bewerkstelligen ist ?

Ich brauche es dafür:
Ich habe dem RP6 4 Beine mit je 3 gelenken verpasst. Ich probiere jetzt seit min 3 Tagen an dem Programm aber kriegs nicht hin. Es gibt immer Probleme mit der Zeit des PWM.

DIV blenderkid

**thietho** · 20.03.2008, 22:03

Da habe ich was für Dich!
Bin allerdings heute nicht zu Hause, werde Dir aber den Code dann senden

**radbruch** · 20.03.2008, 22:16

Acht Servos an den LEDs und ADC0/1:

Code:

// Vierbeiniger RP6 (erste Tests des neuen Beines)                24.2.2008  mic

#include "rblib.h"
#include "rblib.c"

#define vli_on  (PORTC|=SL1)
#define vli_off (PORTC&=~SL1)
#define vri_on  (PORTA |= 1)
#define vri_off (PORTA &= ~1)
#define hli_on  (PORTC|=SL2)
#define hli_off (PORTC&=~SL2)
#define hri_on  (PORTC|=SL3)
#define hri_off (PORTC&=~SL3)
#define vla_on  (PORTB|=SL4)
#define vla_off (PORTB&=~SL4)
#define vra_on  (PORTA |= 2)
#define vra_off (PORTA &= ~2)
#define hla_on  (PORTB|=SL5)
#define hla_off (PORTB&=~SL5)
#define hra_on  (PORTB|=SL6)
#define hra_off (PORTB&=~SL6)

uint8_t vli, vri, hli, hri, vla, vra, hla, hra; // Positionen der Servos
uint8_t i;

int main(void)
{
	rblib_init();
	cli();
	TCCR0 =  (0 << WGM00) | (1 << WGM01);					// CTC-Mode
	TCCR0 |= (0 << COM00) | (0 << COM01);					// ohne OCR-Pin
	TCCR0 |=	(0 << CS02)  | (1 << CS01) | (0 << CS00);	// prescaler /8
	TIMSK =  (1 << OCIE0); 										// Interrupt ein
	OCR0  = 9; // 100kHz?

	DDRC |= 0x70;		// LED1-3 auf Ausgang und low
	PORTC &= ~0x70;
	DDRB |= 0x83;		// LED4-6 auf Ausgang und low
	PORTB &= ~0x83;
	//DDRC |= 0x3;		// SCL und SDA auf Ausgang und low
	//PORTC &= ~0x3;
	DDRA |= 3;        // ADC0 und ADC1 auf Ausgang und low
   PORTA &= ~3;
   vli=vla=vri=vra=hli=hla=hri=hra=100;
	sei();
	delay(150);

		vri=120;delay(20);
		vli=80;delay(20);
		hri=120;delay(20);
		hli=80;delay(20);

		vri=80;delay(20);
		vli=120;delay(20);
		hri=80;delay(20);
		hli=120;delay(20);

	while(1){
		vla=90; vra=110; hla=110; hra=90; delay(5);
		vla=70; vra=130; hla=140; hra=60; delay(20);
		//for(i=0; i<40; i++) { vli=120-i; vri=80+i; hli=120-i; hri=80+i; delay(5); }
		vli=80; vri=120; hli=80; hri=120; delay(30);
		vla=90; vra=110; hla=110; hra=90; delay(10);
   	vla=vra=hla=hra=100; delay(5);
		vri=80;delay(5);
		vli=120;delay(5);
		hri=80;delay(5);
		hli=120;delay(5);
	}
	return 0;
}
ISR (TIMER0_COMP_vect)
{
	static uint16_t count=1; // Servoansteuerung

	(count>vli)?vli_off:vli_on;
	(count>vla)?vla_off:vla_on;
	(count>vri)?vri_off:vri_on;
	(count>vra)?vra_off:vra_on;
	(count>hli)?hli_off:hli_on;
	(count>hla)?hla_off:hla_on;
	(count>hri)?hri_off:hri_on;
	(count>hra)?hra_off:hra_on;

	if(count<2000) count++; else count=0;
}

Die Libs dienen nur zum debuggen, mit io.h und interrupt.h sollte es auch funktionieren. Mit der RP6-Lib ist kein Timer mehr frei :(

Ach, fast vergessen: delay() ist 'ne Zählschleife, die Servos brauchen ja auch etwas Zeit um die Positionen anzufahren:

Code:

void delay(uint8_t d)
{
	uint16_t d1, dummy;
	for (d1=d*255; d1>0; d1--) dummy^=d1;
}

Gruß

mic

**blenderkid** · 21.03.2008, 10:03

Hi radbruch,

ich glaube ich verstehe den Code so halb. Was macht die Zeile:
(count>vli)?vli_off:vli_on;
Und ist das mit dem delay eine blockierende Funktion?
Wie lange blockiert delay() die schleife?

Und wann wird ISR aufgerufen?

DIV blenderkid

**radbruch** · 21.03.2008, 11:09

Hallo

(count>vli)?vli_off:vli_on;

bedeutet if(count>vli) vli_off; else vli_on; Es wird vom Kompiler aber zum selben Code übersetzt.

Count wird in der ISR von 0 bis 1999 hochgezählt, das ergibt den 20ms-Intervall. Solange der Positionswert des Servos kleiner als count ist wird der Impuls ausgegeben, bei Position==100 sind das 1ms. (Wenn man den 20ms-Intervall verkürzt, werden die Servos "agressiver", verlängern macht sie träge)

Und ist das mit dem delay eine blockierende Funktion?

Ja. Anstelle der delays kannst du aber auch Berechnungen machen. Ich schreibe die Abläufe erst mit langen delays und kürze dann die Verzögerungen bis es "flüssig" aussieht. (Durch Änderungen innerhalb der delay()-Funktion kann man ALLE Bewegungen beeinflussen!)

Wie lange blockiert delay() die schleife?

Da es eine Zählschleife ist hängt das von der Taktgeschwindigkeit des Kontrollers ab.

Und wann wird ISR aufgerufen?

Alle 10 us, dann ergeben 100 Aufrufe 1ms, das sollte Servo-Mitte sein.

Meine "Beine" haben je zwei Servos, in den defines sind die Pins nach dem Schema vorne-links-innen, vorne-links-aussen, vorne-rechts-innen, vorne-rechts-aussen.... definiert. Bei 6 Beinen würde sich eine Erweiterung mit mitte-links-innen, mitte-links-aussen... anbieten.

Das sind auch meine ersten Gehversuche. Ich definiere feste Abläufe für die Schritte und rufe diese dann nacheinander ab. Z.B. für ein einzelnes Bein: vla anheben, vli nach vorne schwenken, vla absenken. Für zwei Beine gleichzeitig: vla+hra anheben, schwenken, absenken... Dabei gebe ich immer die Zielpositionen der Servos an und warte kurz bis sie die Position erreicht haben. Bei zwei gleichzeitig bewegten Beinen starte ich das Zweite etwas verzögert wegen den Stromspitzen. Während zwei Beine vorgesetzt werden, schwenken die anderen vier ein Stück nach hinten. btw sollten die Servos eine eigene Stromversorgung haben!

Ich werde mir übrigends auch noch zwei weitere Beine basteln, denn mit vier Beinen mit je zwei Bewegungsebenen fällt er um...

Gruß

mic

[Edit]
Ich habe erst jetzt gelesen dass du vier Beine mit je drei Gelenken gebaut hast. Ich glaube, der RP6 kann mit vier Beinen nicht gehen weil er ein relativ hohes Gewicht hat und sein Schwerpunkt beim Anheben eines Beines ja über den anderen Beinen, die dann alles tragen müssen, sein muss damit er nicht umfällt. Meine Beschreibungen oben bezogen sich auf sechs Beine, sorry.

**blenderkid** · 21.03.2008, 15:09

Danke radbruch,
ich werde es einfach mal mit 4 beinen probieren.
Ich kann anstatt der LEDs aber auch die IOs des M32 benutzen oder?

danke blenderkid

**blenderkid** · 21.03.2008, 15:52

Code:

#define vli_on  (PORTC|=IO_PD5)
#define vli_off (PORTC&=~IO_PD5)
#define vlm_on  (PORTC |= IO_PC2)
#define vlm_off (PORTC &= ~IO_PC2)
#define vla_on  (PORTC|=IO_PC3)
#define vla_off (PORTC&=~IO_PC3)
#define vri_on  (PORTC|=IO_PC5)
#define vri_off (PORTC&=~IO_PC5)
#define vrm_on  (PORTC|=IO_PC7)
#define vrm_off (PORTC&=~IO_PC7)
#define vra_on  (PORTC |= IO_PD6)
#define vra_off (PORTC &= ~IO_PD6)


uint8_t vli, vlm, vla, vri, vrm, vra; // Positionen der Servos
uint8_t i;


  
  
 


/*****************************************************************************/
// Main function - The program starts here:


void delay(uint8_t d)
{
   uint16_t d1, dummy;
   for (d1=d*255; d1>0; d1--) dummy^=d1;
} 


int16_t main(void)
{
	initRP6Control();  
	initLCD();
    
	writeString_P("\n\nRP6 CONTROL M32 I2C Master Example Program!\n"); 
    writeString_P("\nMoving...\n"); 

	// ---------------------------------------
	WDT_setRequestHandler(watchDogRequest); 
	BUMPERS_setStateChangedHandler(bumpersStateChanged);
	ACS_setStateChangedHandler(acsStateChanged);
	BATTERY_setLowVoltageHandler(batteryVoltageLow);

	// ---------------------------------------
	I2CTWI_initMaster(100);  
	I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
	I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);

	sound(180,80,25);
	sound(220,80,25);

	setLEDs(0b1111);

	showScreenLCD("################", "################");
	mSleep(500);
	showScreenLCD("I2C-Master", "Behaviours");
	mSleep(1000);
	setLEDs(0b0000);
	
	// ---------------------------------------
	// Setup ACS power:
	I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_ACS_POWER, ACS_PWR_HIGH);
	// Enable Watchdog for Interrupt requests:
	I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT, true);
	// Enable timed watchdog requests:
	I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT_RQ, true);

	startStopwatch1();
	startStopwatch2();
	startStopwatch4();
	

	showScreenLCD("Active Behaviour", "");
	
	
	
	 cli();
    TCCR1B =  (0 << WGM00) | (1 << WGM01);               // CTC-Mode
    TCCR1B |= (0 << COM00) | (0 << COM01);               // ohne OCR-Pin
    TCCR1B |=   (0 << CS02)  | (1 << CS01) | (0 << CS00);   // prescaler /8
    TIMSK =  (1 << OCIE0);                               // Interrupt ein
    OCR1A  = 9; // 100kHz?
	
	
	
	
	DDRD |= IO_PD5;					// SERVO_OUT -> OUTPUT
	PORTD &= ~IO_PD5;				// SERVO_OUT -> LOW
	DDRD |= IO_PC2;					// SERVO_OUT -> OUTPUT
	PORTD &= ~IO_PC2;				// SERVO_OUT -> LOW
	DDRD |= IO_PC3;					// SERVO_OUT -> OUTPUT
	PORTD &= ~IO_PC3;				// SERVO_OUT -> LOW
	DDRD |= IO_PC5;					// SERVO_OUT -> OUTPUT
	PORTD &= ~IO_PC5;				// SERVO_OUT -> LOW
	DDRD |= IO_PC7;					// SERVO_OUT -> OUTPUT
	PORTD &= ~IO_PC7;				// SERVO_OUT -> LOW
	DDRD |= IO_PD6;					// SERVO_OUT -> OUTPUT
	PORTD &= ~IO_PD6;				// SERVO_OUT -> LOW
    
    vli=vlm=vla=vri=vrm=vra=100;
	
    sei();
	while(true) 
	{
		
		uint8_t key = getPressedKeyNumber(); 
		// This function returns a 0 if no key is pressed and
		// the key number from 1 to 5 otherwise.

		if(key) // If a key is pressed... (key != 0)
		{
			// ... we update the values
			// on the LCD display:
			
			switch(key)
			{
				case 1: 
						
				break;
				
				case 2: 
						
				break;
			}
		}
		//behaviourController();
		
		task_LCDHeartbeat();
		task_checkINT0();
	    task_I2CTWI();
		
		
		vri=120;delay(20);
		vli=80;delay(20);
		vra=120;delay(20);
		vla=80;delay(20);

		
	}
	return 0;
}


ISR (TIMER1_COMP_vect)
{
   static uint16_t count=1; // Servoansteuerung

   (count>vli)?vli_off:vli_on;
   (count>vlm)?vlm_off:vlm_on;
   (count>vla)?vla_off:vla_on;
   (count>vri)?vri_off:vri_on;
   (count>vrm)?vrm_off:vrm_on;
   (count>vra)?vra_off:vra_on;
   
   if(count<2000) count++; else count=0;
}

bei dem Code kriege ich volgenden Fehler:

Compiling: RP6Control_10_Move2.c
avr-gcc -c -mmcu=atmega32 -I. -gdwarf-2 -Os -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wall -Wstrict-prototypes -Wa,-adhlns=RP6Control_10_Move2.lst -I../../RP6lib -I../../RP6lib/RP6control -I../../RP6lib/RP6common -std=gnu99 -MD -MP -MF .dep/RP6Control_10_Move2.o.d RP6Control_10_Move2.c -o RP6Control_10_Move2.o
RP6Control_10_Move2.c:1010: warning: 'TIMER1_COMP_vect' appears to be a misspelled signal handler

und die Beine bewegen sich nicht.

DIV blenderkid

**radbruch** · 21.03.2008, 16:06

Ich kann anstatt der LEDs aber auch die IOs des M32 benutzen oder?

Na klar. Ich habe kein M32 und muss deshalb diese "Klimmzüge" machen weil die RP6-Base nur zwei wirklich freie Pins hat(ADC0/1). Um helfen zu können muss ich mich erstmal mit dem M32 vertraut machen...

Gruß

mic

**radbruch** · 21.03.2008, 17:54

RP6Control_10_Move2.c:1010: warning: 'TIMER1_COMP_vect' appears to be a misspelled signal handler

Der Timer1 beim Mega32 hat zwei Kanäle, die Interrupts heisen hier "TIMER1 COMPA" und "TIMER1 COMPB". Der Name der ISR für OCR1A wäre dann: TIMER1_COMPA_vect

**blenderkid** · 22.03.2008, 15:12

Code:

	 cli();
    TCCR1A =  (0 << WGM00) | (1 << WGM01);               // CTC-Mode
    TCCR1A |= (0 << COM00) | (0 << COM01);               // ohne OCR-Pin
    TCCR1A |=   (0 << CS02)  | (1 << CS01) | (0 << CS00);   // prescaler /8
    TIMSK =  (1 << OCIE0);                               // Interrupt ein
    OCR1A  = 9; // 100kHz?

sind diese Angaben für die M32 richtig, denn es klappt nicht.