Hallo

Da die Servos blockierend angesteuert werden solltest du nur eine servotrim-Funktion verwenden. Dort erzeugst du dann nacheinander die jeweiligen Impulse und den gemeinsamen 20ms-Zyklus (mehrfach weil die Servos etwas Zeit benötigen um die neue Position anzufahren). Nur die Servoansteuerung getestet:

Code:
#include "RP6RobotBaseLib.h"

// #define servopins adc // Ausgabe der Impulse an ADC0/1

uint16_t servopos = 20;
uint16_t servopos2 = 20;

void servotrim(void)
{
	uint8_t i;

   for(i=0;i<5;i++) 		// Impuls mehrfach senden -> Servostellzeit
	{

#ifdef servopins 				// ADC0/1-Version

		DDRA |= (1<<ADC0) | (1<<ADC1);
   	PORTA |= 1;   			// Servo1 an ADC0
		sleep(servopos);
   	PORTA |= 2;   			// Servo2 an ADC1
   	sleep(servopos2);
   	PORTA &= ~3;			// Impulse fertig, start der Pause für 20ms-Zyklus
   	sleep(200-servopos-servopos2);

#else

   	setLEDs(0b001000);   // Servo1 an LED4
		sleep(servopos);
   	setLEDs(0b010000);   // Servo2 an LED5?
   	sleep(servopos2);
   	setLEDs(0);				// Impulse fertig, start der Pause für 20ms-Zyklus
   	sleep(200-servopos-servopos2);

#endif

   }
	//mSleep(50); 				// hier würde ich nicht noch zusätzlich warten
}
void servoposi(void)
{
   if(servopos==8)
   {
      while(servopos != 20)
      {
         servopos++;
         servotrim();
         mSleep(50);
      }
   }
   else if(servopos==20)
   {
      while(servopos != 8)
      {
         servopos--;
         servotrim();
         mSleep(50);
      }
   }
}


//***********************SERVO2*******************************************************+
/*
void servotrim2(void)
{
setLEDs(1);

   sleep(servopos2);
   setLEDs(0);
   sleep(200-servopos2);
   mSleep(50);
}
*/
void servoposi2(void)
{
   if(servopos==8)
   {
      while(servopos2 != 20)
      {
         servopos2++;
         servotrim();
         mSleep(50);
      }
   }
   else if(servopos2==20)
   {
      while(servopos2 != 8)
      {
         servopos2--;
         servotrim();
         mSleep(50);
      }
   }
}

int main (void)
{
   initRobotBase();
   powerON();

   while (true)
   {
      task_RP6System();
      servoposi();
     servoposi2();
   }
   return 0;
}
Gruß

mic