Code Optimierung für Interrupt möglich?

**sternst** · 23.08.2011, 22:19

Zitat von Felix G

Wenn man eh schon Assembler einsetzt kann man das:

"PORTx |= 1 << nr"

auch gleich dadurch ersetzen:

"sbi PORTx, nr"

Unsinn. Das Problem ist ja, dass nr keine Konstante ist.

**Felix G** · 23.08.2011, 22:41

Zitat von sternst

Unsinn. Das Problem ist ja, dass nr keine Konstante ist.

ja, ist mir auch gerade aufgefallen...
ich war noch bei der ersten Variante (ohne Schleife), wo der Compiler natürlich auch von sich aus schon sbi und cbi verwenden sollte.

**radbruch** · 24.08.2011, 17:25

Hallo

Nun kann ich schon mal ein Zwischenergebniss abliefern. Zufällig habe ich die Steuerplatine des Caterpillar (Danke an den Sponsor), ein schnuckeliges Platinchen mit einem 16MHz-Mega16. Darauf läuft nun dieses Programm das theoretisch 18 Servos ansteuern kann. Theoretisch deshalb, weil ich grad keine 18 Servos rumliegen habe und vermutlich das Platinchen abrauchen würde, wenn ich das ohne seperate Spannungsversorgung für die Servos versuchen würde.

Wie schon in der ersten Vorversion läuft der Timer1 mit Prescaler /64, die Auflösung ist deshalb ca. 0,000004 Sekunde (1/(16MHz/64)) oder 4µs. Eine Millisekunde dauert deshalb 250 Zähltakte, das wären etwa 1600 Kontrollertakte zwischen den ISR-Aufrufen. In meinem Testprogramm zeige ich nebenher die Registerinhalte an und schiebe die Bits für die Statusled per Hardware-SPI raus ohne die Ansteuerung merklich zu stören.

Die errechneten Werte scheinen auch zu passen: Ein 20ms-Zyklus dauert 5000 Zählschritte des Timers (0,02s/0,000004s), der Drehbereich meines Testservos beträgt ca. von 90 bis 450 (muss ich noch genauer erforschen). Das wären bei neun Servos pro ISR ca. 4050 Zähltakte oder knapp 1000 Zähltakte für die Impulspause. Da wäre also sogar noch Platz für weitere Servos oder eine schnellere Wiederholfrequenz:

Code:

// 18 Servos ansteuern mit 16MHz-Mega16 (cat16) und 16-Bit Timer1              24.8.2011 mic

// https://www.roboternetz.de/community/threads/54583-Code-Optimierung-f%C3%BCr-Interrupt-m%C3%B6glich

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdlib.h> // für itoa() in writeInteger()

// Servoausgänge A 1-9
// S1 bis S8 und Speaker als Dummy (PB3)
#define servoainit {DDRB |= 0b1011; PORTB &= ~0b1011; DDRC |= 0b11111100; PORTC &= ~0b11111100;}
#define servoa1on  PORTB |=  (1<<PB0) // S1
#define servoa1off PORTB &= ~(1<<PB0)
#define servoa2on  PORTB |=  (1<<PB1) // S2
#define servoa2off PORTB &= ~(1<<PB1)

#define servoa3on  PORTC |=  (1<<PC2) // S3
#define servoa3off PORTC &= ~(1<<PC2)
#define servoa4on  PORTC |=  (1<<PC3) // S4
#define servoa4off PORTC &= ~(1<<PC3)
#define servoa5on  PORTC |=  (1<<PC4) // S5
#define servoa5off PORTC &= ~(1<<PC4)
#define servoa6on  PORTC |=  (1<<PC5) // S6
#define servoa6off PORTC &= ~(1<<PC5)
#define servoa7on  PORTC |=  (1<<PC6) // S7
#define servoa7off PORTC &= ~(1<<PC6)
#define servoa8on  PORTC |=  (1<<PC7) // S8
#define servoa8off PORTC &= ~(1<<PC7)

#define servoa9on  PORTB |=  (1<<PB3) // Dummy (Speaker)
#define servoa9off PORTB &= ~(1<<PB3)

// Servoausgänge B 1-9
// J1 PD2/RC5, J2 PA0/Roll, J5 PA6, J6 PA7, J7 PA7-PD7/OCR2, J8 PA4/HeadL, J9 PA2/Tail, J10 PA1/Rang
#define servobinit {DDRA |= 0b11011111; PORTA &= ~0b11011111; DDRD |= 0b10000100; PORTD &= ~0b10000100;}
#define servob1on  PORTD |=  (1<<PD2) // J1 PD2/RC5
#define servob1off PORTD &= ~(1<<PD2)
#define servob2on  PORTA |=  (1<<PA0) // J2 PA0/Roll
#define servob2off PORTA &= ~(1<<PA0)

#define servob3on  PORTA |=  (1<<PA6) // J5 PA6/ADC6
#define servob3off PORTA &= ~(1<<PA6)
#define servob4on  PORTA |=  (1<<PA7) // J6 PA7/ADC7
#define servob4off PORTA &= ~(1<<PA7)
#define servob5on  PORTD |=  (1<<PD7) // J7 PD7/OCR2 (Pin4, PA7 ist auf Pin3!)
#define servob5off PORTD &= ~(1<<PD7)
#define servob6on  PORTA |=  (1<<PA4) // J8 PA4/HeadL
#define servob6off PORTA &= ~(1<<PA4)
#define servob7on  PORTA |=  (1<<PA2) // J9 PA2/Tail
#define servob7off PORTA &= ~(1<<PA2)
#define servob8on  PORTA |=  (1<<PA1) // J10 PA1/Rang
#define servob8off PORTA &= ~(1<<PA1)

#define servob9on  PORTA |=  (1<<PA3) // J8 PA3 auf Pin4
#define servob9off PORTA &= ~(1<<PA3)

#define green	0x10
#define red		0x20
#define yellow 0x30

volatile uint8_t p; // 20ms-Timer
volatile uint16_t servo_pos_a[10]={5000, 250, 250, 250, 250, 250, 250, 250, 250, 250}; // Pos 0 ist Impulspause
volatile uint16_t servo_pos_b[10]={0, 250, 250, 250, 250, 250, 250, 250, 250, 250};

/************************* 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]);}
/******************************************************************************/

void sleep(uint8_t pause);                    // 1/50 Sekunde blockierende Pause
void leds(uint8_t mask);

int main(void)
{
	cli();

	/************************ UART-Setup für cat16 *******************************/
	#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);
	/***************************************************************************/

	//Init SPI für leds()
	SPCR = ( (0<<SPE)|(1<<MSTR) | (1<<SPR1) |(1<<SPR0)); // Disable SPI, Master, set clock rate fck/128

	DDRB = 0b10110000;                        // SCK: PB7, MoSi: PB5, SS: PB4 (!)?
	DDRD = 0b01000000;                        // PD6 ist STRB fürs 4094

	//Timer1 Initialisierung
	TCCR1A = 0;
	TCCR1B = (0<<CS12) | (1<<CS11) | (1<<CS10); // Prescaler /64
	//TCCR1B|= (1<<WGM12); // CTC-Mode  23.8.11 NormalMode!
	TCNT1=0; // Zählregister initialisieren (vorsichtshalber)
	OCR1A=5000; // 20ms bis zum ersten Interrupt
	OCR1B=5000;
	TIMSK |= (1 << OCIE1A);
	TIMSK |= (1 << OCIE1B);

	servoainit; // Datenrichtung der Servopins A einstellen
	servobinit; // Datenrichtung der Servopins B einstellen

	sei(); // ... und los!
	leds(red);
	writeString("Hallo\n");
	sleep(50);
	
	while(1)
	{
	   leds(yellow);
	   servo_pos_a[1]=90;
	   servo_pos_b[3]=90;
		sleep(50);
	   leds(red);
	   servo_pos_a[1]=250;
	   servo_pos_b[3]=210;
		sleep(50);
	   leds(green);
	   servo_pos_a[1]=450;
	   servo_pos_b[3]=330;
	   sleep(50);
	   leds(red);
	   servo_pos_a[1]=250;
	   servo_pos_b[3]=450;
		sleep(100);
	}

	return(0);
}

ISR (TIMER1_COMPA_vect)
{
	static uint8_t servo_nr=1;
	uint16_t temp=0;

	switch(servo_nr)
	{
		case 1: temp=servo_pos_a[1]; TCNT1=0; OCR1B=50; servoa1on; break; // ;)
  		case 2: temp=servo_pos_a[2]; servoa1off; servoa2on; break;
  		case 3: temp=servo_pos_a[3]; servoa2off; servoa3on; break;
  		case 4: temp=servo_pos_a[4]; servoa3off; servoa4on; break;
		case 5: temp=servo_pos_a[5]; servoa4off; servoa5on; break;
  		case 6: temp=servo_pos_a[6]; servoa5off; servoa6on; break;
  		case 7: temp=servo_pos_a[7]; servoa6off; servoa7on; break;
  		case 8: temp=servo_pos_a[8]; servoa7off; servoa8on; break;
  		case 9: temp=servo_pos_a[9]; servoa8off; servoa9on; break;
		case 10: servoa9off; servo_nr=0; OCR1A = servo_pos_a[0]; if(p) p--; break; // Impulspause
	}

	if(servo_nr) OCR1A = temp + TCNT1; // nächsten Interruptzeitpunkt berechnen
/*
	   else
		{
			writeInteger(TCNT1, 10);
			writeChar('A');
			writeInteger(OCR1A, 10);
			writeChar('\n');
		}
*/
	servo_nr++;
}

ISR (TIMER1_COMPB_vect)
{
	static uint8_t servo_nr=1;
	uint16_t temp=0;

	switch(servo_nr)
	{
		case 1: temp=servo_pos_b[1]; servob1on; break;
  		case 2: temp=servo_pos_b[2]; servob1off; servob2on; break;
  		case 3: temp=servo_pos_b[3]; servob2off; servob3on; break;
  		case 4: temp=servo_pos_b[4]; servob3off; servob4on; break;
		case 5: temp=servo_pos_b[5]; servob4off; servob5on; break;
  		case 6: temp=servo_pos_b[6]; servob5off; servob6on; break;
  		case 7: temp=servo_pos_b[7]; servob6off; servob7on; break;
  		case 8: temp=servo_pos_b[8]; servob7off; servob8on; break;
  		case 9: temp=servo_pos_b[9]; servob8off; servob9on; break;
		case 10: servob9off; servo_nr=0; break; // nach dem letzen Impuls warten auf A!
	}

	if(servo_nr) OCR1B = temp + TCNT1; // nächsten Interruptzeitpunkt berechnen
/*
	   else
		{
			writeInteger(TCNT1, 10);
			writeChar('B');
			writeInteger(OCR1B, 10);
			writeChar('\n');
		}
*/
	servo_nr++;
}

void sleep(uint8_t pause)                    // 1/50 Sekunde blockierende Pause
{
   p=pause+1;
   while(p);
}
void leds(uint8_t mask)
{
	SPCR = ( (1<<SPE)|(1<<MSTR) | (1<<SPR1) |(1<<SPR0));
	SPDR = mask;
	while(!(SPSR & (1<<SPIF))); // Wait for transmission complete!
	SPCR = ( (0<<SPE)|(1<<MSTR) | (1<<SPR1) |(1<<SPR0));
	PORTD |= (1<<6); 	// STRB high
	PORTD &= ~(1<<6);	// STRB low
}

Ob sich die zwei ISRs ungünstig beeinflussen kann ich noch nicht sagen. Mit der ausgekommentierten Ausgabefunktion in den ISRs erzeugte das Demo folgende Ausgabe:

Code:

Links jeweils der Wert des TCNT1 nach allen Impulsen, rechts der Wert des OCR1X-Registers. Führend ist die A-ISR, hier wird neben den Impulsen auch die Impulspause erzeugt. Nach Ende der Impulspause wird das Zählregister zurückgesetzt und das OCR1B-Register mit einem Startwert geladen der dafür sorgt, dass die B-ISR wieder startet. Der OCR1A-Wert für die Wiederholfrequenz ist zu Testzwecken und für Spielereinen im Wert des Dummy-Servo servo_pos_a[0] gespeichert.

Gruß

mic

**erik_wolfram** · 24.08.2011, 20:04

Hallo,
nun wage ich es auch mal mich hier wieder zu melden

Habe jetzt 2 unterschiedliche Methoden probiert:
1. ich habe ein gleiches/ähnliches Interrupt wie Radbruch programmiert - etwas einfacher, aber es funktioniert (bei 12 Servos kommt man ohne die Pause auch auf eine ungefaire Abarbeitungszeit für alle Impulse von 20ms)
2. ich habe den Prescaler erhöht - so haben die Servos eine Auflösung von 50 Einheiten für einen Ausschlag von 90° von Links nacht Recht.

Beides funktioniert erstmal ... dafür habe ich festgestellt, dass meine Platine störanfällig für die höhen Ströme ist - d.h. ich muss erstmal eine neue Paltine ätzen...

Ansonsten hätte ich gerne mal den Code von radbruch getestet - das ist so leider noch nicht möglich.

MFG Erik

**radbruch** · 13.09.2011, 22:21

Hallo

Leider hat mein Code einen grundsätzlichen Gedankenfehler, denn die Impulspause ist nur für das erste Servo wirklich konstant. Die Pausenlängen aller anderen Servos schwanken je nach Impulsdaueränderungen der anderen Servos im jeweiligen ISR-Thread. Solange das nicht stört, sollte man es wohl ignorieren.

Bei meiner Suche nach 18 funktionsfähigen Servos habe ich inzwischen 12 Stück gefunden bzw. repariert:

Das wie gewohnt etwas unscharfe Video (meine Cam ist im Urlaub) zeigt einen kleinen Ausschnitt aus einem Dauerlauftest mit 12 Servos und unbelasteter 90°-Drehung nach ca. 1,5 Stunden. Ich denke, ich bin auf dem besten Weg zu meinem ersten richtigen Spinnenbot. :)

Gruß

mic

P.S.: Fast vergessen, hier mein aktuelles Programm für das aufgebohrte cat16:

Code:

// 18.facher Servotester                                                          13.9.2011 mic

// 18 Servos ansteuern mit 16MHz-Mega16 (cat16) und 16-Bit Timer1

// https://www.roboternetz.de/community/threads/54583-Code-Optimierung-f%C3%BCr-Interrupt-m%C3%B6glich

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdlib.h> // für itoa() in writeInteger()

// Servoausgänge A 1-9
// S1 bis S8 und Speaker als Dummy (PB3)
#define servoainit {DDRB |= 0b1011; PORTB &= ~0b1011; DDRC |= 0b11111100; PORTC &= ~0b11111100; DDRD |=  (1<<PD3); PORTD &= ~(1<<PD3);}

#define servoa1on  PORTD |=  (1<<PD3) // INT1
#define servoa1off PORTD &= ~(1<<PD3)
#define servoa2on  PORTB |=  (1<<PB0) // S1
#define servoa2off PORTB &= ~(1<<PB0)
#define servoa3on  PORTB |=  (1<<PB1) // S2
#define servoa3off PORTB &= ~(1<<PB1)
#define servoa4on  PORTC |=  (1<<PC2) // S3
#define servoa4off PORTC &= ~(1<<PC2)
#define servoa5on  PORTC |=  (1<<PC3) // S4
#define servoa5off PORTC &= ~(1<<PC3)
#define servoa6on  PORTC |=  (1<<PC4) // S5
#define servoa6off PORTC &= ~(1<<PC4)
#define servoa7on  PORTC |=  (1<<PC5) // S6
#define servoa7off PORTC &= ~(1<<PC5)
#define servoa8on  PORTC |=  (1<<PC6) // S7
#define servoa8off PORTC &= ~(1<<PC6)
#define servoa9on  PORTC |=  (1<<PC7) // S8
#define servoa9off PORTC &= ~(1<<PC7)

// Servoausgänge B 1-9
// J1 PD2/RC5, J2 PA0/Roll, J5 PA6, J6 PA7, J7 PA7-PD7/OCR2, J8 PA4/HeadL, J9 PA2/Tail, J10 PA1/Rang
#define servobinit {DDRA |= 0b11011111; PORTA &= ~0b11011111; DDRD |= 0b10000100; PORTD &= ~0b10000100;}

#define servob1on  PORTD |=  (1<<PD2) // J1 PD2/RC5
#define servob1off PORTD &= ~(1<<PD2)
#define servob2on  PORTA |=  (1<<PA0) // J2 PA0/Roll
#define servob2off PORTA &= ~(1<<PA0)
#define servob3on  PORTA |=  (1<<PA6) // J5 PA6/ADC6
#define servob3off PORTA &= ~(1<<PA6)
#define servob4on  PORTA |=  (1<<PA7) // J6 PA7/ADC7
#define servob4off PORTA &= ~(1<<PA7)
#define servob5on  PORTD |=  (1<<PD7) // J7 PD7/OCR2 (Pin4, PA7 ist auf Pin3!)
#define servob5off PORTD &= ~(1<<PD7)
#define servob6on  PORTA |=  (1<<PA4) // J8 PA4/HeadL
#define servob6off PORTA &= ~(1<<PA4)
#define servob7on  PORTA |=  (1<<PA3) // J8 PA3/HeadR auf Pin4
#define servob7off PORTA &= ~(1<<PA3)
#define servob8on  PORTA |=  (1<<PA2) // J9 PA2/Tail
#define servob8off PORTA &= ~(1<<PA2)
#define servob9on  PORTA |=  (1<<PA1) // J10 PA1/Rang
#define servob9off PORTA &= ~(1<<PA1)


#define off		0
#define green	0x10
#define red		0x20
#define yellow 0x30

volatile uint8_t p; // 20ms-Timer
volatile uint16_t servo_pos_a[10]={5000, 0,0,0, 0,0,0, 0,0,0}; // Pos[0] ist Impulspause
volatile uint16_t servo_pos_b[10]={0,    0,0,0, 0,0,0, 0,0,0}; // Position=0 bedeutet kein Impuls

ISR (TIMER1_COMPA_vect)
{
	static uint8_t servo_nr=1;
	uint16_t temp=0;

	switch(servo_nr)
	{
		// Beim ersten Servo das Zählregister reseten und Interrupt für B-ISR initialisieren
		// Wenn das Ergebniss der Zuweisung ungleich 0 dann Impuls starten und switch() verlassen
		// sonst nächtes Servo, was bedeutet, das aktuelle Servo wird übersprungen (auch mehrfach!)
		// (( temp=servo_pos_a[] )) doppelte Klammerung wegen Kompilerwarnung!

		case 1: TCNT1=0; OCR1B=50; if((temp=servo_pos_a[1])) {servoa1on; break;} else servo_nr++; // ;)
  		case 2: servoa1off; if((temp=servo_pos_a[2])) {servoa2on; break;} else servo_nr++;
  		case 3: servoa2off; if((temp=servo_pos_a[3])) {servoa3on; break;} else servo_nr++;
  		case 4: servoa3off; if((temp=servo_pos_a[4])) {servoa4on; break;} else servo_nr++;
		case 5: servoa4off; if((temp=servo_pos_a[5])) {servoa5on; break;} else servo_nr++;
  		case 6: servoa5off; if((temp=servo_pos_a[6])) {servoa6on; break;} else servo_nr++;
  		case 7: servoa6off; if((temp=servo_pos_a[7])) {servoa7on; break;} else servo_nr++;
  		case 8: servoa7off; if((temp=servo_pos_a[8])) {servoa8on; break;} else servo_nr++;
  		case 9: servoa8off; if((temp=servo_pos_a[9])) {servoa9on; break;} else servo_nr++;
		case 10: servoa9off; servo_nr=0; OCR1A = servo_pos_a[0]; if(p) p--; break; // Impulspause
		//case 10: servoa9off; servo_nr=0; OCR1A = TCNT1+500; if(p) p--; break; // Impulspause (Test)
	}

	if(servo_nr) OCR1A = temp + TCNT1; // nächsten Interruptzeitpunkt berechnen
/*
	   else
		{
			writeInteger(TCNT1, 10);
			writeChar('A');
			writeInteger(OCR1A, 10);
			writeChar('\n');
		}
*/
	servo_nr++;
}

ISR (TIMER1_COMPB_vect)
{
	static uint8_t servo_nr=1;
	uint16_t temp=0;

	switch(servo_nr)
	{
		case 1: if((temp=servo_pos_b[1])) {servob1on; break;} else servo_nr++;
  		case 2: servob1off; if((temp=servo_pos_b[2])) {servob2on; break;} else servo_nr++;
  		case 3: servob2off; if((temp=servo_pos_b[3])) {servob3on; break;} else servo_nr++;
  		case 4: servob3off; if((temp=servo_pos_b[4])) {servob4on; break;} else servo_nr++;
		case 5: servob4off; if((temp=servo_pos_b[5])) {servob5on; break;} else servo_nr++;
  		case 6: servob5off; if((temp=servo_pos_b[6])) {servob6on; break;} else servo_nr++;
  		case 7: servob6off; if((temp=servo_pos_b[7])) {servob7on; break;} else servo_nr++;
  		case 8: servob7off; if((temp=servo_pos_b[8])) {servob8on; break;} else servo_nr++;
  		case 9: servob8off; if((temp=servo_pos_b[9])) {servob9on; break;} else servo_nr++;
		case 10: servob9off; servo_nr=0; break; // nach dem letzten Impuls warten auf A-ISR!
	}

	if(servo_nr) OCR1B = temp + TCNT1; // nächsten Interruptzeitpunkt berechnen
/*
	   else
		{
			writeInteger(TCNT1, 10);
			writeChar('B');
			writeInteger(OCR1B, 10);
			writeChar('\n');
		}
*/
	servo_nr++;
}

/************************* 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]);}
/******************************************************************************/

volatile uint8_t usart_puffer[20], usart_write=0, usart_read=0, eingabe=0;
uint8_t beinnr, servonr, temp8;
uint16_t posa, posb, posc, temp16;

SIGNAL (SIG_UART_RECV)
{
    usart_puffer[usart_write]=UDR;
    if(usart_puffer[usart_write++] == 13) eingabe=1; // CR empfangen
	 if(usart_write > 19) {usart_write=19; eingabe=2;} // Pufferüberlauf!
}

void sleep(uint8_t pause)                    // 1/50 Sekunde blockierende Pause
{
   p=pause;
   while(p);
}
void leds(uint8_t mask)
{
	SPCR = ( (1<<SPE)|(1<<MSTR) | (1<<SPR1) |(1<<SPR0));
	SPDR = mask;
	while(!(SPSR & (1<<SPIF))); // Wait for transmission complete!
	SPCR = ( (0<<SPE)|(1<<MSTR) | (1<<SPR1) |(1<<SPR0));
	PORTD |= (1<<6); 	// STRB high
	PORTD &= ~(1<<6);	// STRB low
}
void sleep(uint8_t pause);                    // 1/50 Sekunde blockierende Pause
void leds(uint8_t mask);                      // green, red, yellow or off

void c(uint8_t nr, uint16_t a, uint16_t b, uint16_t c, uint8_t d)
{
	if(a>=200 && a<=500 && b>=200 && b<=500 && c>=200 && c<=500) // Bereichsprüfung
	{
	   sleep(1); // Positionswerte in der Impulspause ändern, Synchronisationg nur mit A-ISR!
		if(nr<2)
	   {
	      servo_pos_a[nr*3+1]=a;
	      sleep(d); // gemeinsamen Anlauf verhindern!
	      servo_pos_a[nr*3+2]=b;
	      sleep(d);
	      servo_pos_a[nr*3+3]=c;
	   }
	   else if(nr<3)
	   {
	      servo_pos_b[3]=a; // ??? Testbelegung!
	      sleep(d);
	      servo_pos_b[4]=b;
	      sleep(d);
	      servo_pos_b[5]=c;
	   }
		else writeString("Fehler: Beinnummer!\n");
	}
	else writeString("Fehler: Drehbereich!\n");
}

int main(void)
{
	cli();
	
	//MCUCSR  |= 128; // JTAG ausschalten
	//MCUCSR  |= 128;

	/************************ UART-Setup für cat16 *******************************/
	#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); // Senden, Empfangen und Empfang-ISR einschalten
	/***************************************************************************/

	//Init SPI für leds()
	SPCR = ( (0<<SPE)|(1<<MSTR) | (1<<SPR1) |(1<<SPR0)); // Disable SPI, Master, set clock rate fck/128

	DDRB = 0b10110000;                        // SCK: PB7, MoSi: PB5, SS: PB4 (!)?
	DDRD = 0b01000000;                        // PD6 ist STRB fürs 4094

	//Timer1 Initialisierung
	TCCR1A = 0;
	TCCR1B = (0<<CS12) | (1<<CS11) | (1<<CS10); // Prescaler /64
	//TCCR1B|= (1<<WGM12); // CTC-Mode  23.8.11 NormalMode!
	TCNT1=0; // Zählregister initialisieren (vorsichtshalber)
	OCR1A=5000; // 20ms bis zum ersten Interrupt
	OCR1B=5000;
	TIMSK |= (1 << OCIE1A);
	TIMSK |= (1 << OCIE1B);

	servoainit; // Datenrichtung der Servopins A einstellen
	servobinit; // Datenrichtung der Servopins B einstellen

	sei(); // ... und los!
	leds(red);
	writeString("\nHallo\n");
	sleep(50);
	
	while(1)
	{
	   for(temp8=1; temp8<10;temp8++) servo_pos_a[temp8]=servo_pos_b[temp8]=250;
		sleep(50);
	   for(temp8=1; temp8<10;temp8++) servo_pos_a[temp8]=servo_pos_b[temp8]=450;
		sleep(50);
	}

	while(1)
	{
	   if(eingabe)
	   {
	      leds(green);
/*
	      for(temp8=usart_read; temp8<usart_write; temp8++)
	      {
	         writeChar(usart_puffer[temp8]);
			}
			writeChar('\n');
*/
	      if(usart_puffer[0] == 'a')
			{
			   servonr=(usart_puffer[1]-'0')*10+usart_puffer[2]-'0';
			   servo_pos_a[servonr]=(usart_puffer[3]-'0')*100+(usart_puffer[4]-'0')*10+usart_puffer[5]-'0';
			   writeString("Servo: A");
			   writeInteger(servonr, 10);
			   writeString(" = ");
			   writeInteger(servo_pos_a[servonr],10);
			   writeChar('\n');
			}
	      else if(usart_puffer[0] == 'b')
			{
			   servonr=(usart_puffer[1]-'0')*10+usart_puffer[2]-'0';
			   servo_pos_b[servonr]=(usart_puffer[3]-'0')*100+(usart_puffer[4]-'0')*10+usart_puffer[5]-'0';
			   writeString("Servo: B");
				writeInteger(servonr, 10);
			   writeString(" = ");
			   writeInteger(servo_pos_b[servonr],10);
			   writeChar('\n');
			}
	      else if(usart_puffer[0] == 'c')
			{
			   // CB,aaa,bbb,ccc
			   // 01234567890123
				if((usart_puffer[2]+usart_puffer[6]+usart_puffer[10]) == (3*',')) // alle Kommas richtig gesetzt?
				{
				beinnr=usart_puffer[1]-'0';
			   posa=(usart_puffer[3]-'0')*100+(usart_puffer[4]-'0')*10+usart_puffer[5]-'0';
			   posb=(usart_puffer[7]-'0')*100+(usart_puffer[8]-'0')*10+usart_puffer[9]-'0';
			   posc=(usart_puffer[11]-'0')*100+(usart_puffer[12]-'0')*10+usart_puffer[13]-'0';

			   c(beinnr, posa, posb, posc, 10);
			   writeString("Bein: ");
				writeInteger(beinnr, 10);
			   writeString(" = ");
			   writeInteger(posa,10);
			   writeChar(' ');
			   writeInteger(posb,10);
			   writeChar(' ');
			   writeInteger(posc,10);
			   writeChar('\n');
			   }
			   else writeString("Fehler: Kommas im Datensatz!\n");
			}
			else writeString("ERROR!\n");
			usart_read=0;
			usart_write=0;
			eingabe=0;
			leds(red);
	   }
	}
	return(0);
}