Code Optimierung für Interrupt möglich?

[erik_wolfram]

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]

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:

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