Stopwatch wird ignoriert/ falsch interpretiert
Hallo, ich bin wieder auf ein neues Problem gestoßen.
Der Roboter soll bei der ACS links+rechts Erkennung eine kurze Pause einlegen (ca 5000ms) und danach weiterfahren, laut Programm. Ich habe hier den nur den Base als Master.
Mein Code-Ausschnitt (aus Examples Base Move 5):
Code:
#define AVOID_SPEED_L_ARC_LEFT 30
#define AVOID_SPEED_L_ARC_RIGHT 40 // 90
#define AVOID_SPEED_R_ARC_LEFT 40 // 90
#define AVOID_SPEED_R_ARC_RIGHT 30
#define AVOID_SPEED_ROTATE 30 // 60
#define AVOID_SPEED_WAIT 0 //Motoren halt
// States for the Avoid FSM:
#define AVOID_OBSTACLE_RIGHT 1
#define AVOID_OBSTACLE_LEFT 2
#define AVOID_OBSTACLE_MIDDLE 3
#define AVOID_OBSTACLE_MIDDLE_WAIT 4
#define AVOID_END 5
behaviour_command_t avoid = {0, 0, FWD, false, false, 0, IDLE};
void behaviour_avoid(void)
{
static uint8_t last_obstacle = LEFT;
static uint8_t obstacle_counter = 0;
switch(avoid.state)
{
case IDLE:
if(obstacle_right && obstacle_left) // left AND right sensor have detected something...
avoid.state = AVOID_OBSTACLE_MIDDLE;
else if(obstacle_left) // Left "sensor" has detected something
avoid.state = AVOID_OBSTACLE_LEFT;
else if(obstacle_right) // Right "sensor" has detected something
avoid.state = AVOID_OBSTACLE_RIGHT;
break;
case AVOID_OBSTACLE_MIDDLE:
avoid.dir = last_obstacle;
avoid.speed_left = AVOID_SPEED_WAIT;
avoid.speed_right = AVOID_SPEED_WAIT;
setStopwatch4(0);
startStopwatch4();
while(getStopwatch4() >= 4990){ // Die while Schleife verursacht nur Probleme....
if (getStopwatch4() >=5000)
{
setStopwatch4(0);
}}
avoid.speed_left = AVOID_SPEED_ROTATE;
avoid.speed_right = AVOID_SPEED_ROTATE;
//else
if(!(obstacle_left || obstacle_right))
{
if(obstacle_counter > 3)
{
obstacle_counter = 0;
setStopwatch4(0);
}
else
setStopwatch4(400);
startStopwatch4();
avoid.state = AVOID_END;
}
break;
case AVOID_OBSTACLE_RIGHT:
avoid.dir = FWD;
avoid.speed_left = AVOID_SPEED_L_ARC_LEFT;
avoid.speed_right = AVOID_SPEED_L_ARC_RIGHT;
if(obstacle_right && obstacle_left)
avoid.state = AVOID_OBSTACLE_MIDDLE;
if(!obstacle_right)
{
setStopwatch4(500);
startStopwatch4();
avoid.state = AVOID_END;
}
last_obstacle = RIGHT;
obstacle_counter++;
break;
case AVOID_OBSTACLE_LEFT:
avoid.dir = FWD;
avoid.speed_left = AVOID_SPEED_R_ARC_LEFT;
avoid.speed_right = AVOID_SPEED_R_ARC_RIGHT;
if(obstacle_right && obstacle_left)
avoid.state = AVOID_OBSTACLE_MIDDLE;
if(!obstacle_left)
{
setStopwatch4(500);
startStopwatch4();
avoid.state = AVOID_END;
}
last_obstacle = LEFT;
obstacle_counter++;
break;
case AVOID_END:
if(getStopwatch4() > 1000) // We used timing based movement here!
{ stopStopwatch4();
setStopwatch4(0);
avoid.state = IDLE;
}
break;
}
}
Die Wartezeit soll nur einmal aufgerufen werden pro Ausweichmanöver.
Ich habe da verschieden Schleifenvariationen ausprobiert und der RP6 ignoriert die Erkennung oder bleibt stehen. Ohne Schleife wird immer ein Teil übersprungen.
Wäre es sinnvoller den Timer in einem weiteren case z.B. AVOID_OBSTACLE_MIDDLE_WAIT einzurichten und mit einem "avoid.state = AVOID_OBSTACLE_MIDDLE;" überzuleiten? Oder läuft es auf das selbe Ergebnis heraus?
Wer sieht meinen Fehler?
