Programmierungs (C) Bewegungsproblem
Hallo
Ich habe mir jetzt vor paar Tagen den RP6 geholt (hatte anfänglich auch ein defektes USB Interface...) und bin jetzt fleißig am programmieren.
Momentan schreibe ich ein Programm mit einer Statemachine, was den Roboter durch den Raum manövrieren soll, ohne irgendwo anzuecken.
Code:
#include "RP6RobotBaseLib.h"
#define STATE_NONE 0
#define STATE_MOVE 1
#define STATE_ROTATE 2
#define STATE_HALT 3
#define STATE_CHANGE_DIRECTION 4
#define STATE_CHECK_FOR_OBSTACLE 5
#define STATE_INITIALIZE 6
uint8_t state;
uint8_t velocity;
void state_machine(void)
{
// Statusvariablen
int8_t is_moving = false;
int8_t is_rotating = false;
switch(state)
{
// Initialisieren
case STATE_INITIALIZE:
{
// Microcontroller initialisieren
initRobotBase();
// Motorencoder initialisieren
powerON();
// Entfernungssensoren aktivierencv
setACSPwrMed();
state = STATE_CHECK_FOR_OBSTACLE;
break;
}
// Fahren
case STATE_MOVE:
{
//move(velocity, FWD, DIST_MM(100), true);
//state = STATE_CHECK_FOR_OBSTACLE;
// Roboter fährt nicht
if(!is_moving)
{
move(velocity, FWD, DIST_MM(50), false);
is_moving = true;
state = STATE_MOVE;
}
// Roboter fährt bereits
else
{
// prüfen, ob Fahrvorgang abgeschlossen ist
if(isMovementComplete())
{
is_moving = false;
state = STATE_CHECK_FOR_OBSTACLE;
}
}
break;
}
// Rotieren
case STATE_ROTATE:
{
//rotate(velocity, LEFT, 45, true);
//state = STATE_CHECK_FOR_OBSTACLE;
// Roboter rotiert nicht
if(!is_rotating)
{
rotate(velocity, LEFT, 45, false);
is_rotating = true;
state = STATE_ROTATE;
}
// Roboter rotiert bereits
else
{
// Rotation ist abgeschlossen?
if(isMovementComplete())
{
is_rotating = false;
state = STATE_CHECK_FOR_OBSTACLE;
}
}
break;
}
// Kollision ermittlen
case STATE_CHECK_FOR_OBSTACLE:
{
// Hindernis gefunden?
if(obstacle_left || obstacle_right)
{
// rotieren...
setLEDs(0b111111);
state = STATE_ROTATE;
}
else
{
// fahren...
setLEDs(0b000000);
state = STATE_MOVE;
}
break;
}
}
}
int main(void)
{
// Anfangsstate festlegen...
state = STATE_INITIALIZE;
// Geschwindigkeit setzten
velocity = 200;
// Hauptschleife
while(true)
{
// gesamtes System updaten...
task_RP6System();
// State Machine durchlaufen
state_machine();
}
return 0;
}
Folgendes Problem habe ich: der Roboter soll, nachdem die Bewegungsroutine abgeschlossen ist prüfen, ob Hindernisse vor ihm liegen - er macht es aber nicht...
Warum kommt es in
Code:
case STATE_MOVE:
{
//move(velocity, FWD, DIST_MM(100), true);
//state = STATE_CHECK_FOR_OBSTACLE;
// Roboter fährt nicht
if(!is_moving)
{
move(velocity, FWD, DIST_MM(50), false);
is_moving = true;
state = STATE_MOVE;
}
// Roboter fährt bereits
else
{
// prüfen, ob Fahrvorgang abgeschlossen ist
if(isMovementComplete())
{
is_moving = false;
state = STATE_CHECK_FOR_OBSTACLE;
}
}
break;
}
nie zu
Code:
if(isMovementComplete())
{
is_moving = false;
state = STATE_CHECK_FOR_OBSTACLE;
}
Nochmal kurz die aktuelle (fehlerhafte VErhaltensbeschreibung des Roboters)
Er fährt einfach nur gerade durch, ohne Rücksicht auf Verluste ;)
(Die Sensoren vernachlässigt er (diese Funktionieren aber einwandfrei!))
mfG TcH
