Hier ist mein Programm:

Code:
#include "RP6RobotBaseLib.h"    // The RP6 Robot Base Library. 
// Defines: 
#define SERVO_OUT         SDA         // PINC1  XBUS Pin 12 

// Servo movement limits (depending on servo type): 
#define LEFT_TOUCH   1//550         // Left servo touch 
#define RIGHT_TOUCH   254         // Right servo touch [max. 255] 
#define PULSE_ADJUST    4 
#define PULSE_REPETITION   19         // Pulse repetition frequency 
//Variablen 
int richtung=0; //0=rechts; 1=links 
int speed=5; 
/*****************************************************************************/ 
// Functions: 

/** 
 * INIT SERVO 
 * 
 * Call this once before using the servo task. 
 * 
 */ 
void initSERVO(void) 
{ 
   DDRC |= SERVO_OUT;               // SERVO_OUT -> OUTPUT 
   PORTC &= ~SERVO_OUT;            // SERVO_OUT -> LO 
   startStopwatch1();               // Needed for 20ms pulse repetition 
} 
void pulseSERVO(uint8_t position) 
{ 
   cli(); 
   PORTC |= SERVO_OUT;               // SERVO_OUT -> HI (pulse start) 
   delayCycles(LEFT_TOUCH); 
   while (position--) { 
      delayCycles(PULSE_ADJUST); 
   } 
   PORTC &= ~SERVO_OUT;            // SERVO_OUT -> LO (pulse end) 
   sei(); 
} 
void task_SERVO(void) 
{static uint8_t pos; 
   if (getStopwatch1() > PULSE_REPETITION) { // Pulse every ~20ms 
         if(richtung==0) pos=pos+speed; 
         else pos=pos-speed; 
         if (pos > RIGHT_TOUCH) {richtung = 1;} //nach links drechen 
         if (pos < LEFT_TOUCH) {richtung = 0;} //nach rechts drehen 

       pulseSERVO(pos); 
      setStopwatch1(0); 
   } 
} 

/*****************************************************************************/ 
// Main function - The program starts here: 

int main(void) 
{ 
   initRobotBase(); 
   powerON();//anschalten 
   startStopwatch1(); 

   setLEDs(0b111111); // Turn all LEDs on 
   mSleep(500);       // delay 500ms 
   setLEDs(0b000000); // All LEDs off 

   initSERVO(); 
    
   while(true)  
   { 
      task_SERVO();
		rotate(60, RIGHT, 90, true);
		task_SERVO();
		move(70, FWD, DIST_MM(300), true);
		task_SERVO();
      task_RP6System(); 
   } 
   return 0; 
}
[/code]