So sind die Bewegungen auch etwas schöner:
computerkalle.de/rp6.php RP6 Servoarm Video

Code:
#include "RP6RobotBaseLib.h"

uint8_t  i, pause, servo_stellzeit, servo_delay;

void servo(uint8_t w0, uint8_t w1, uint8_t w2, uint8_t w3, uint8_t w4)
{
unsigned int count=0;
   do{
      PORTA |= E_INT1;      // E_INT1 (Pin8)
      sleep(w0);
      PORTA &= ~E_INT1;
      PORTC |= 1;            // SCL (Pin10)
      sleep(w1);
      PORTC &= ~1;
      PORTC |= 2;            // SDA (Pin12)
      sleep(w2);
      PORTC &= ~2;
      PORTB |= 1;
      sleep(w3);
      PORTB &= ~1;
      PORTB |=2;
      sleep(w4);
      PORTB &=~2;
      sleep(servo_delay-(w0+w1+w2+w3+w4));
      //sleep(127);
   } while (count++ < servo_stellzeit);
   mSleep(2000*pause);
}

int main(void) {

initRobotBase();
i=0;
servo_stellzeit=2;
DDRA |= E_INT1;            // E_INT1 als Ausgang
DDRC |= 3;                  // SCL und SDA als Ausgang
// 5 - 15 - 24             // Min - Mitte - Max
//servo(24,15,20,10,10); 
//servo(15,5,20,10,10);           // Grundstellung
servo(24,24,20,10,10);
servo(24,24,20,10,10);
servo(24,24,20,10,10);
while (1)
{//servo(greifer auf/zu,links/rechts,up/down,10,10);
servo(24,24,20,10,10); //runter
servo(24,24,19,10,10); 
servo(24,24,18,10,10); 
servo(24,24,17,10,10); 
servo(24,24,16,10,10); 
servo(24,24,15,10,10); 
servo(24,24,14,10,10); 
servo(24,24,13,10,10); 
servo(24,24,12,10,10); 
servo(24,24,11,10,10); 
servo(24,24,10,10,10); 
servo(24,24,9,10,10); 
servo(24,24,8,10,10); 
servo(24,24,7,10,10); 
servo(24,24,6,10,10); 
servo(24,24,5,10,10);//greifen 
servo(23,24,5,10,10); 
servo(22,24,5,10,10); 
servo(21,24,5,10,10);  
servo(20,24,5,10,10); 
servo(19,24,5,10,10); 
servo(18,24,5,10,10); 
servo(17,24,5,10,10); 
servo(16,24,5,10,10); 
servo(15,24,5,10,10); 
servo(14,24,5,10,10); 
servo(13,24,5,10,10); 
servo(12,24,5,10,10); 
servo(11,24,5,10,10); 
servo(10,24,5,10,10); //hoch
servo(10,24,6,10,10); 
servo(10,24,7,10,10); 
servo(10,24,8,10,10); 
servo(10,24,9,10,10); 
servo(10,24,10,10,10); 
servo(10,24,11,10,10); 
servo(10,24,12,10,10); 
servo(10,24,13,10,10); 
servo(10,24,14,10,10); 
servo(10,24,15,10,10); 
servo(10,24,16,10,10); 
servo(10,24,17,10,10); 
servo(10,24,18,10,10); 
servo(10,24,19,10,10); 
servo(10,24,20,10,10); 
servo(10,24,21,10,10); 
servo(10,24,22,10,10); 
servo(10,24,23,10,10); //Drehung, von recht nach links
servo(10,23,24,10,10);
servo(10,22,24,10,10);
servo(10,21,24,10,10);
servo(10,20,24,10,10);
servo(10,19,24,10,10);
servo(10,18,24,10,10);
servo(10,17,24,10,10);
servo(10,16,24,10,10);
servo(10,15,24,10,10);
servo(10,14,24,10,10);
servo(10,13,24,10,10);
servo(10,12,24,10,10);
servo(10,11,24,10,10);
servo(10,10,24,10,10);
servo(10,9,24,10,10);
servo(10,8,23,10,10); //runter
servo(10,8,22,10,10); 
servo(10,8,21,10,10); 
servo(10,8,20,10,10); 
servo(10,8,19,10,10); 
servo(10,8,18,10,10); 
servo(10,8,17,10,10); 
servo(10,8,16,10,10); 
servo(11,8,15,10,10);//greifer auf
servo(12,8,15,10,10); 
servo(13,8,15,10,10); 
servo(14,8,15,10,10); 
servo(15,8,15,10,10); 
servo(16,8,15,10,10); 
servo(17,8,15,10,10);
servo(18,8,15,10,10); 
//servo(19,8,14,10,10); //hoch
servo(18,8,16,10,10); 
servo(18,8,17,10,10); 
servo(18,8,18,10,10);  
servo(19,8,19,10,10); 
servo(20,8,20,10,10); 
servo(20,8,21,10,10); 
servo(20,8,22,10,10); 
servo(20,8,23,10,10); 
servo(20,8,24,10,10); 
servo(20,9,24,10,10); //Drehung, von links nach rechts
servo(20,10,24,10,10); 
servo(20,11,24,10,10); 
servo(20,12,24,10,10); 
servo(20,13,24,10,10); 
servo(20,14,24,10,10); 
servo(20,15,24,10,10); 
servo(20,16,24,10,10); 
servo(20,17,24,10,10); 
servo(20,18,24,10,10); 
servo(20,19,24,10,10); 
servo(20,20,24,10,10); 
servo(20,21,24,10,10); 
servo(21,22,24,10,10); 
servo(22,23,24,10,10); 
servo(23,24,24,10,10); 
}

return 0;
}