Servo steuern

[Christian3]

Hallo,
ich verwende vollgenen script für meinen servo:

#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 550//550  Left servo touch 
#define RIGHT_TOUCH 10 // Right servo touch [max. 255] 
#define PULSE_ADJUST 4 
#define PULSE_REPETITION 18 // Pulse repetition frequency 
//Variablen 
 
int richtung=1; //0=rechts; 1=links 
int speed=2; 
/*****************************************************************************/ 
// 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(); 
task_RP6System(); 

} 
return 0; 
}

so jetzt möchte ich das er sich nach rechts dreht und das 5 sekunden wartet das er sich zurück dreht was auch noch cool wär wenn er beim zurück drehen die selbe geschwindigkeit hätte wie beim hin drehen
könnt ihr mir helfen?
Danke schonmal^^
lg

[Christian3]

SRY habs schon hinbekommen die werte entsprachen nicht die meines Servos

#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 mitte_TOUCH 550//550  Left servo touch 
#define RIGHT_TOUCH 100 // Right servo touch [max. 255] 
#define PULSE_ADJUST 4 
#define PULSE_REPETITION 18 // Pulse repetition frequenc
//Variablen 
 
int richtung=0; //0=rechts; 1=links 
int speed=2; 

/*****************************************************************************/ 
// 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(mitte_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==1) pos=pos+speed; 
if(richtung==0) pos=pos-speed; 



if (pos <10) {richtung = 1;

mSleep(1500); // delay 500ms 

} //nach rechts drehen 

if (pos > 250) {richtung = 0;
mSleep(1500); // delay 500ms 

} //nach links drechen 

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(); 

task_RP6System(); 




} 
return 0; 
}