Würfel sammler
noisia.sytes.net/text/Cube-collector.avi

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(20*pause);
}

int main(void) {

initRobotBase();
i=0;
servo_stellzeit=20;
DDRA |= E_INT1;            // E_INT1 als Ausgang
DDRC |= 3;                  // SCL und SDA als Ausgang
// 5 - 15 - 25             // Min - Mitte - Max
servo(25,15,20,10,10); 
servo(15,5,20,10,10);           // Grundstellung
servo(10,5,15,10,10);
while (1)
{
servo(25,24,20,10,10); 
servo(25,24,10,10,10); 
servo(25,24,5,10,10); 
servo(20,24,5,10,10); 
servo(10,24,5,10,10); 
servo(10,24,7,10,10); 
servo(10,24,9,10,10); 
servo(10,24,10,10,10); 
servo(10,24,12,10,10); 
servo(10,24,13,10,10); 
servo(10,24,15,10,10); 
servo(10,24,18,10,10); 
servo(10,24,21,10,10); 
servo(10,24,23,10,10); 
servo(10,24,25,10,10); 
servo(10,8,25,10,10); 
servo(10,7,22,10,10); 
servo(10,8,18,10,10); 
servo(20,8,18,10,10); 
servo(25,8,16,10,10); 

/*
servo(15,8,20,10,10);  //runter&greifer auf
servo(16,8,19,10,10);    
 servo(17,8,17,10,10);
 servo(18,8,15,10,10);  
servo(17,8,14,10,10);  
servo(16,8,14,10,10);  
servo(15,8,14,10,10);//greifer zu
servo(14,8,14,10,10);
servo(13,8,14,10,10);
servo(12,8,14,10,10);
//=(greifer,baseturn,beuger1
servo(12,8,14,10,10);//hoch
servo(12,8,14,10,10);
servo(12,8,16,10,10);  //links drehung beginn    
servo(12,10,19,10,10);
servo(12,13,21,10,10);  
servo(12,15,23,10,10);
servo(12,18,21,10,10);  
servo(12,21,19,10,10);    
servo(12,22,17,10,10);  
servo(12,23,16,10,10);  
servo(12,24,13,10,10); //links drehung ende
servo(12,24,12,10,10);//runter greifer auf
servo(13,24,10,10,10);  
servo(19,24,10,10,10); 
servo(19,24,15,10,10); */
}

return 0;
}
gruss