Servo steuern

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

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 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

SRY habs schon hinbekommen die werte entsprachen nicht die meines servos

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 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;
}

---