RP6 ausweichen problem

[xonic128]

Hi habe mir vor ein paar tagen den RP6v2 geholt.
Jetzt häng ich am ausweichen von Hindernissen :(

Folgendes will ich erreichen:

1. Der RP6 fährt erstmal geradeaus.
2. wenn ein Hindernis erkannt wird (z.B. obstacle_left ) dann soll er um 20 Grad nach rechts drehen.
3. Nachdem er gedreht hat soll er überprüfen ob das Hindernis immer noch Links ist - wenn ja dann nochmal 20 Grad drehen - so lange wiederholen bis alles frei ist.
4. Wenn dann wieder alles frei ist soll er weiterfahren bis zum nächsten Hindernis.

Mein Problem ist das es nicht ganz flüssig aussieht was mein RP6 da macht. er dreht mal 20 Grad (alle ok) mal sieht es aus als würde er nur 10 Grad drehen und es bewegt sich nur eine kette beim drehen.
Mir kommt es so vor als würde er das drehen abbrechen.

Hier mal mein Code:


#include "RP6RobotBaseLib.h"

int main(void)
{
initRobotBase();
powerON();
setACSPwrMed();

while(true)
{
task_RP6System();

changeDirection(FWD);

moveAtSpeed(80,80);

if((getBumperRight()||obstacle_right) && (getBumperLeft()||obstacle_left))
{
rotate(80, RIGHT, 45, true);
}
else if(getBumperRight()||obstacle_right)
{
rotate(80, LEFT, 20, true);
}
else if(getBumperLeft()||obstacle_left)
{
rotate(80, RIGHT, 20, true);
}
}
return 0;
}



Wie könnte ich das ausweichen von Hindernissen weicher machen? Ich will nicht das er gleich 90 Grad dreht, ich will das er sich langsam herantastet.

Danke!