Code Optimierung für Interrupt möglich?

Hallo,
ich arbeite nach wie vor an einer Kinematik für meinen Quadruped. Ich habe das erste Bein soweit zum laufen bekommen, dass er auf dem Boden Kreise "malt". Nun wollte ich mir das nächste Bein vornehmen und musste feststellen, dass nach dem Hinzufügen des 2. Beins die Servos nichtmehr richtig positionierten (leichte Abweichung). Eine genauere Debugging-Untersuchung ergab, dass das Interrupt welches alle 10µs ausgelöst wird selber 17µs braucht (für beide Beine).
Ich habe natürlich die Möglichkeit das Interrupt in längeren Abständen auszulösen, das kostet mich dann aber die genaue Auflösung der Servo-Ansprechzeiten...

Anderseits kommen mir 17µs sehr lange für die wenigen Befehle vor - vielleicht sieht hier ein erfahrenen Programmierer eine Optimierungs-Chance?!

Code:

#define F_CPU 16000000 ... // Servo 1 #define PAD_SERVO_0 5 #define PORT_SERVO_0 PORTC #define DDR_SERVO_0 DDRC ... volatile unsigned char servo_pos[12] = {90, 90, 100, 200, 160, 160, 100, 100, 100, 200, 200, 200}; ... void servo_init() { TIMSK |= (1 << OCIE2); TCCR2 |= (1 << WGM21) | (1 << CS20); OCR2 = F_CPU / 100000; // <------------ INTERRUPT alle 10µS // Servo-Ports auf Output DDR_SERVO_0 |= (1 << PAD_SERVO_0); ... // Interrupt global aktivieren TIMSK |= (1<<TOIE1); sei(); } ISR (TIMER2_COMP_vect) { // --------------------------------------- ab hier (0 µs) static int count; // Bein 0 if (count > servo_pos[0]) PORT_SERVO_0 &= ~(1 << PAD_SERVO_0); else PORT_SERVO_0 |= (1 << PAD_SERVO_0); if (count > servo_pos[1]) PORT_SERVO_1 &= ~(1 << PAD_SERVO_1); else PORT_SERVO_1 |= (1 << PAD_SERVO_1); if (count > servo_pos[2]) PORT_SERVO_2 &= ~(1 << PAD_SERVO_2); else PORT_SERVO_2 |= (1 << PAD_SERVO_2); // Bein 1 if (count > servo_pos[3]) PORT_SERVO_3 &= ~(1 << PAD_SERVO_3); else PORT_SERVO_3 |= (1 << PAD_SERVO_3); if (count > servo_pos[4]) PORT_SERVO_4 &= ~(1 << PAD_SERVO_4); else PORT_SERVO_4 |= (1 << PAD_SERVO_4); if (count > servo_pos[5]) PORT_SERVO_5 &= ~(1 << PAD_SERVO_5); else PORT_SERVO_5 |= (1 << PAD_SERVO_5); if (count < 2000) count ++; else count = 0; // --------------------------------------- bis hier 17µS } int main() { servo_init(); ...

Der Roboter hat 4 mal 3 Servos pro Bein!

Wenn der Debugger mir eine Teit von 17µS bei 4Mhz anzeigt - kann ich die Zeit dann bei 16Mhz vierteln?!

Mit freundlichen Grüßen Erik

Da gibt es 2 Ansatzpunkte zur Optimierung:
1. für Count könnte man im wesentlichen eine 8 Bit Variable nutzen, un die lange Verzögerung nach den Servo Pulsen seperat machen, z.B. indem man die Interrupts seltener macht. Der Zugriff auf das Feld könnte ggf. auch noch etwas schneller gehen, wenn man einen Pointer benutzt. Ob das viel bringt weiss ich aber nicht. Wenn es unbedingt sein muss gäbe es noch Assembler - gerade für Software PWM ist da ein recht großer Geschwindigkeitsvorteil drin, vor allem wenn die IO Ports günstig liegen.

2. Man macht die Servo Pulse nicht alle gleichzeitig, sondern versetzt nacheinander. Die Verzögerung kann man dann über das Timerregister einstellen, nicht über das Zählen von Interrupts. Allerdings ist man damit auf etwa 10 Servos beschränkt. Die 2 verbleibenden Servos könnte man per Hardware PWM (16 Bit) ansteuern. Als etwa komplizierte Alternative kann man auch immer je 2 Pulse parallel bearbeiten, um rund 1 ms versetzt. Das ist aber nicht so ganz einfach.

Was an Optimierungspotential vorhanden ist, lässt sich leider schlecht beurteilen da die Quelle wegen der zig ... nicht übersetzbar ist.

Danke, das war mir eine große Hilfe:

count habe ich zumindest als short (16bit) deklartiert und die ausgänge sozusagen kaskadiert (per if Abfrage) - so, dass die Beine separtiert nacheinander abgearbeitet werden - jetzt bin ich unterhalb von 2µS während der Abarbeitung.

Übrigens, nach langem suchen hab ich die Einstellung für den Debugger gefunden: bei Umstellung von 4Mhz auf 16Mhz viertelt sich die Zeit!

[EDIT:]

bei den "..." sind nur wierholungen für die deklarierung aller Servo-Ports sowie nicht verwendete Funktionen.

Hallo

Kauf dir ein Servoboard:
http://www.shop.robotikhardware.de/s...x.php?cPath=87

Im Ernst, mehr als 8 Servos wird immer kniffelig, weil dann die 20ms nicht mehr eingehalten werden können. Ich bastele ja schon länger an Servoansteuerungen rum und knall dir einfach mal hin, was ich so erbrütet habe.

Erste ganz wichtige Optimierung: Die ISR wird nicht mehr mit 100kHz aufgerufen sondern nur noch, wenn ein Impuls beendet wird und der nächste startet. Beispiel mit drei Servos an einem 8Mhz-Mega32:

Code:

//Servos mit Timer2 Overflow-ISR 27.2.2008 mic #include "RP6RobotBaseLib.h" uint8_t pos[3]={255,170,85}; // Positionen der drei Servos an SDA, SCL und E_INT int main(void) { initRobotBase(); DDRA |= 16; PORTA &= ~16; DDRC |= 3; PORTC &= ~3; //Timer2 Initialisierung TCCR2 = (0 << WGM21) | (0 << COM20) | (1 << CS22); // Normal Mode, prescaler /64 TIMSK |= (1 << TOIE2); // Timer2 Overflow-Interrupt erlauben while(1) { pos[0]=pos[1]=pos[2]=170; mSleep(1000); pos[0]=pos[1]=pos[2]=220; mSleep(1000); pos[0]=pos[1]=pos[2]=170; mSleep(1000); pos[0]=pos[1]=pos[2]=120; mSleep(1000); } return(0); } ISR (TIMER2_OVF_vect) { static uint8_t servo_nr=0; static uint16_t impulspause; if(servo_nr) { if(servo_nr==1) {TCNT2=-pos[0]; PORTC |= 1; impulspause-=pos[0];} if(servo_nr==2) {TCNT2=-pos[1]; PORTC &= ~1; PORTC |= 2; impulspause-=pos[1];} if(servo_nr==3) {TCNT2=-pos[2]; PORTC &= ~2; PORTA |= 16; impulspause-=pos[2];} if(servo_nr==4) {PORTA &= ~16; servo_nr=0;} if(servo_nr) servo_nr++; } else { if(impulspause>256) impulspause-=256; else {TCNT2=-impulspause; servo_nr++; impulspause=1500;} } }

In dieser Variante wird zuerst der immer gleiche Grundimpuls (quasi die kürzeste Impulsdauer) erzeugt und anschliesend die Impulslänge für die Position angehängt. Wertebereich für die Positionen ist ca. 0-255! Beispiel mit acht Servos:

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 Impulspause 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 } }

Trotz allen Tricks scheitert man irgendwann an den 20ms der Impulswiederholung. Ein Ansatz sind zwei parallele Threads mit dem Timer1 und der Impulslängenerzeugung mit MatchCompare A und B:

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++; }

Leider hat das nie richtig funktioniert. Da ich bisher nur acht Servos gleichzeitig ansteuern mußte, habe ich das nicht weiterentwickelt. Vielleicht sollte ich das mit meinem jetztigen Kentnissstand nochmals angreifen.

Hier noch klassisch acht Servos am asuro (Ausführungszeit der ISR habe ich nicht gemessen):
https://www.roboternetz.de/community...-walking-asuro

Und zum Abschluß mein aktueller Code für acht Servos am 16MHz-Mega16 meines Deltawalkerprojekts:

Code:

// Servoansteuerung mit 16Bit-Timer1 (mit arexx cat16) mic 18.1.2011 // Die 8 Servos werden an Port C und D angeschlossen: // Servo0 - PC0 // Servo1 - PC1 // Servo2 - PD2 // Servo3 - PD3 // Servo4 - PD4 // Servo5 - PD5 // Servo6 - PD6 // Servo7 - PD7 #include <avr/io.h> #include <avr/interrupt.h> #include <inttypes.h> #define green 0x10 #define red 0x20 #define yellow 0x30 volatile uint8_t count_20ms; volatile uint16_t servo[9] ={1700, 1700, 1700, 1700, 1700, 1700, 1111, 1111, 40000}; // Servopositionen uint16_t temp; void sleep(uint8_t pause); // 1/50 Sekunde blockierende Pause void leds(uint8_t mask); int main(void) { cli(); // für 16MHz-ATMega16 TCCR1A = (0<<WGM11)|(0<<WGM10); // CTC (Mode 4) TCCR1B = (0<<WGM13)|(1<<WGM12); TCCR1B |= (0<<CS12)|(1<<CS11)|(0<<CS10); // prescaler /8 TIMSK = (1<<OCIE1A); // MatchCompare-Interrupt erlauben //InitSPI SPCR = ( (0<<SPE)|(1<<MSTR) | (1<<SPR1) |(1<<SPR0)); // Disable SPI, Master, set clock rate fck/128 DDRB = 0b10110011; // SCK, MoSi, SS (!) DDRC = 0b11111100; // und Servoausgänge setzen DDRD = 0b01000000; // PD6 ist STRB fürs 4094 sei(); leds(green); sleep(100); while(1) // Demo { leds(red); cli(); servo[0]=2500; servo[1]=servo[2]=1600; sei(); sleep(50); leds(yellow); cli(); servo[0]=servo[1]=servo[2]=1700; sei(); sleep(50); } return(0); } ISR(TIMER1_COMPA_vect) { static uint8_t nr=8; // Nummer des aktuellen Servos PORTB &= ~0b00000011; // alle Servoimpulse low PORTC &= ~0b11111100; if(nr < 8) // Impuls für Servo oder Pause? (8 Servos) { if(nr<2) PORTB |= (1<<nr); // Impulsleitung des aktuellen Servos else PORTC |= (1<<nr); // auf High setzen und OCR1A = servo[nr]; // Impulslänge in OCR1A laden servo[8] -= servo[nr]; // Impulslänge von der Pause abziehen nr++; // nächstes Servo } else { OCR1A = servo[8]; // servo[8] ist die Impulspause servo[8] = 40000; // Startwert 20ms laden nr = 0; // beim nächsten ISR-Aufruf Impuls // für Servo 0 erzeugen if(count_20ms) count_20ms--; // blockierende Pause aktiv? } } void sleep(uint8_t pause) // 1/50 Sekunde blockierende Pause { count_20ms=pause+1; while(count_20ms); } 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 }

http://www.youtube.com/watch?v=sBgtgO6mCnY

Such dir was aus. :)

Das Thema ist so gewaltig, das kann man nicht einfach beantworten.

Gruß

mic

Die Servo Pulse nacheinander zu erzeugen hat vor allem den Vorteil, das man gar keinen Interrupt in kurzem Abstand braucht, sonder einfach den Timer so einstellt, das der nächste Interrupts kommt, wenn der aktuelle Puls zu Ende ist, und mit dem nächsten Puls begonnen werden kann. Die Auflösung wird dann nur noch vom Timer und der Interrupts Latenz bestimmt. Man kommt so auch auf unter 1 µs Auflösung, wenn man es will.

Zitat:

Zitat von radbruch

[...viel Code...]

*Bei der letzten Version fällt Code wie 1 << nr in der ISR unangenehm auf, da dieser Ausdruck in einer Schleife zur Laufzeit berechnet werden muss. Weil C vorschreibt, daß die Operation auf int-Ebene auszuführen ist, wird's besonders übel.

*Leider ist dem nicht so einfach beizukommen; in avr-gcc 4.7 gibt's eine Optimierung die das Problem abmildert, aber eine Schleife bleibt's immer noch.

Falls Ausführungsbeschwindigkeit oberste Priorität hat — was in einer ISR nicht unwahrscheinlich ist — und man keine Angst davor hat, den Code mit inline-Assembler zu verunstalten, geht 1 << nr auch ohne Schleife und in 7 Ticks:

Code:

// Berechnet 1 << nr static inline unsigned char pow2_n (unsigned char nr) { ** *unsigned char val; ** * ** *asm ("ldi *%0, 1" *"\n\t" ** * * * "sbrc %1, 1" *"\n\t" ** * * * "ldi *%0, 4" *"\n\t" ** * * * "sbrc %1, 0" *"\n\t" ** * * * "lsl *%0" * * "\n\t" ** * * * "sbrc %1, 2" *"\n\t" ** * * * "swap %0" ** * * * : "=&d" (val) ** * * * : "r" (nr)); ** *return val; }

Was soll denn dieser Schrott mir den Sternchen???
Die Foren-Software ist ja voll malle...

Also nochmal das ganze:

Code:

// Berechnet 1 << nr static inline unsigned char pow2_n (unsigned char nr) { ** *unsigned char val; ** * ** *asm ("ldi *%0, 1" *"\n\t" ** * * * "sbrc %1, 1" *"\n\t" ** * * * "ldi *%0, 4" *"\n\t" ** * * * "sbrc %1, 0" *"\n\t" ** * * * "lsl *%0" * * "\n\t" ** * * * "sbrc %1, 2" *"\n\t" ** * * * "swap %0" ** * * * : "=&d" (val) ** * * * : "r" (nr)); ** *return val; }

Hier also ohne die blöden code-Tags:
// Berechnet 1 << nr
static inline unsigned char
pow2_n (unsigned char nr)
{
unsigned char val;

asm ("ldi %0, 1" "\n\t"
"sbrc %1, 1" "\n\t"
"ldi %0, 4" "\n\t"
"sbrc %1, 0" "\n\t"
"lsl %0" "\n\t"
"sbrc %1, 2" "\n\t"
"swap %0"
: "=&d" (val)
: "r" (nr));

return val;
}

Wenn man eh schon Assembler einsetzt kann man das:

"PORTx |= 1 << nr"

auch gleich dadurch ersetzen:

"sbi PORTx, nr"

edit:
ah sorry das geht so nicht, ich war in Gedanken noch bei der ersten Variante (da sind sbi bzw. cbi genau das was der Compiler erzeugen sollte). Wenn nr nicht konstant ist, kann man diesen Befehl natürlich nicht einsetzen.

Zitat:

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.

Hallo

Bei mir funktionieren die Code-Tags normal (siehe oben).

Die ISR wird nur alle ca. 0.5ms bis 1,5ms aufgerufen und nach der If-Abfrage, die ich wegen der ungünstig belegten Ansteuerpins benötige, wird nur einmal geschoben:

if(nr<2) PORTB |= (1<<nr); // Impulsleitung des aktuellen Servos
else PORTC |= (1<<nr); // auf High setzen

Ich schätze mal, das braucht man nicht mehr optimieren.

Da das Thema nun angeschnitten ist habe ich mal nach dem Fehler in der 18er-Variante gesucht. Damals hatte ich von Timern nur den CTC-Modus kapiert und dieser ist hier völlig falsch. Da beim Match das TCNT1-Register zurückgesetzt wird, wird das Timeing in beiden Threads total gestört. Richtig wäre wohl der normale Mode bei dem der Timer endlos mit 16Bit hochzählt. Beim MatchCompare wird dann der Wert des nächsten Impulses zum TCNT1 dazugezählt (plus ein paar Takte fürs Laden ) und das Ergebniss in das OCR1X geschrieben. Die Impulspause darf dann nur in einem Thread ausgeführt werden, diese Funktion setzt nach Ende der Impulspause zusätzlich das Zählregister wieder auf 0 und der Zyklus startet von Vorne. Spannend. Da mein RP6 und die Servos eingemottet sind kann ich das im Moment nicht testen.

Gruß

mic

Zitat:

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.

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:

Hallo 2268A5000 2805B2267 2267A5000 2804B2316 2268A5000 2805B2315 2268A5000

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

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

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:

http://www.youtube.com/watch?v=F4jKB8vZ54I

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