Hallo

Die blockierende Ansteuerung der Servos mit Sleep() ist natürlich eine Sackgasse. Ich verwende nur noch diese zwei Ansätze:

Klassisch, libverträglich mit geringer Auflösung:
Code:
 // Servoansteuerung mit Timer1                                         31.1.2010 mic

// Einfach und elegant, warum finde ich das erst jetzt? Timer1 (RP6-Motoransteuerung)
// läuft ja sowieso im Hintergrund. Deshalb kann man die "klassische" Servoansteuerung
// in die Overflow-ISR einbauen und mit ca. 19kHz aufrufen lassen. Timersetup der Lib:
// Mode 10, Phase Correct mit ICR1 als Top ergibt bei ICR1=210 ca. 8MHz/420=19047,6Hz ;)

// Drehbereich meiner RS-2-Servos ist von ca. 14 bis ca. 38

#include "RP6RobotBaseLib.h"

volatile uint8_t servo1, servo2, servo3, p;    // Servopositionen und Impulszähler
uint8_t c;                                     // ein Char zur freien Verfügung

int main(void)
{
   initRobotBase();
   servo1=servo2=servo3=26;   // Servomitte?
   TIMSK |= (1 << TOIE1);       // Die Timer1 Overflow-ISR zur Servoansteuerung
   DDRA |= (E_INT1);          // Servopins auf Ausgang setzen
   DDRC |= (SCL | SDA);
   setLEDs(1);                // und los!
   startStopwatch1();
   startStopwatch2();
   startStopwatch3();
   while(1)
   {
      for(c=0;c<6;c++)            // 6 mal ein einfaches Demo....
      {
         setLEDs(1<<c);
         servo1=servo2=servo3=26; // mitte
         p=50; while(p);          // warten bis 50 Impulse gesendet (ca. 1 Sek.)
         servo1=servo2=servo3=14; // links
         p=50; while(p);
         servo1=servo2=servo3=38; // rechts
         p=50; while(p);
      }
      setStopwatch1(0);
      setStopwatch2(0);
      setStopwatch3(0);
      while(c)                    // und 6 mal ein etwas anspruchsvolleres Demo
      {
         setLEDs(1<<c);
         if(getStopwatch1()>1000)
         {
            setStopwatch1(0);
            switch(servo1)
            {
               case 38: servo1=30; break;
               case 30: servo1=20; break;
               case 20: servo1=14; break;
               case 14: servo1=21; break;
               case 21: servo1=31; break;
               case 31: servo1=38; break;
            }
         }
         if(getStopwatch2()>100)
         {
            setStopwatch2(0);
            servo2++;
            if (servo2 > 38) { servo2=14; c--; }
         }
         if(getStopwatch3()>300)
         {
            setStopwatch3(0);
            if (servo3 == 10) servo3=50; else servo3=10;
         }
         task_RP6System(); // Verträglichkeitstest ;)
      }
   }
   return 0;
}
ISR (TIMER1_OVF_vect)
{
   static uint16_t servocount=1;
   if(servocount > servo1) PORTA &= ~E_INT1; else PORTA |= E_INT1;       // PA4 XBUS 8
   if(servocount > servo2) PORTC &= ~SCL; else PORTC |= SCL;             // PC0 XBUS 10
   if(servocount > servo3) PORTC &= ~SDA; else PORTC |= SDA;             // PC1 XBUS 12
   if(servocount < 400) servocount++; else {servocount=1; if(p) p--;}    // p = 1/50 Sek
}
(Aus https://www.roboternetz.de/phpBB2/ze...=483757#483757)

Etwas komplizierter, aber resourcenschonender:
Code:
 //Servos mit Timer2 Overflow-ISR                                 27.2.2008 mic

#include "RP6RobotBaseLib.h"

uint8_t pos[3]={255,170,85};

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]=85;
      mSleep(1000);
      pos[0]=pos[1]=pos[2]=255;
      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=2000-256;}
   }
}
(Aus https://www.roboternetz.de/phpBB2/ze...ag.php?t=46624)

Ich erspare mir jetzt die Beschreibung der verwendeten Verfahren. Bei Interesse einfach nachfragen ;)

Mal wieder Servo (greifarm), sorry
Ich bastle auch an einem Greifarm für meinen RP6;)

Gruß

mic

[Edit]
Ach, es geht ja um das M32. Hab' ich überlesen, sorry. Die Methoden sollten aber auch mit dem M32 funktionieren, wenn man die höhere Taktfrequenz des M32 berücksichtigt.