Mission segment loop
Back to Robobot
Back to Robobot mission
Contents |
RunMission loop
This function in the 'UMission' class is called in its own thread, once the mission app is started.
The (example) code looks like this. Some of the code is explained below.
/**
* Thread for running the mission(s)
* All missions segments are called in turn based on mission number
* Mission number can be set at parameter when starting mission command line.
*
* The loop also handles manual override for the gamepad, and resumes
* when manual control is released.
* */
void UMission::runMission()
{ /// current mission number
mission = fromMission;
int missionOld = mission;
bool regbotStarted = false;
bool ended = false;
bool inManual = false;
int loop = 0;
missionState = 0;
int missionStateOld = missionState;
// fixed C-string buffer for code lines etc
const int MSL = 120;
char s[MSL];
/// initialize robot mission to do nothing (wait for mission lines)
missionInit();
/// start (the empty) mission, ready for mission snippets.
bridge->send("start\n"); // ask REGBOT to start controlled run (ready to execute)
bridge->send("oled 3 waiting for REGBOT\n");
///
for (int i = 0; i < 3; i++)
{
if (not bridge->info->isHeartbeatOK())
{ // heartbeat should come at least once a second
sleep(1);
}
}
if (not bridge->info->isHeartbeatOK())
{ // heartbeat should come at least once a second
system("espeak \"Oops, no usable connection with robot.\" -ven+f4 -s130 -a60 2>/dev/null &");
bridge->send("oled 3 Oops: Lost REGBOT!");
printf("# ---------- error ------------\n");
printf("# No heartbeat from robot. Bridge or REGBOT is stuck\n");
printf("# You could try restart ROBOBOT bridge ('b' from mission console) \n");
printf("# -----------------------------\n");
stop();
}
/// loop in sequenct every mission until they report ended
while (not finished and not th1stop)
{ // stay in this mission loop until finished
loop++;
// test for manuel override (joy is short for joystick or gamepad)
if (bridge->joy->manual)
{ // just wait, do not continue mission
usleep(20000);
if (not inManual)
system("espeak \"Mission paused.\" -ven+f4 -s130 -a40 2>/dev/null &");
inManual = true;
bridge->send("oled 3 GAMEPAD control\n");
}
else
{ // in auto mode
if (not regbotStarted)
{ // wait for start event is received from REGBOT
// - in response to 'bot->send("start\n")' earlier
if (bridge->event->isEventSet(33))
{ // start mission (button pressed)
regbotStarted = true;
}
}
else
{ // mission in auto mode
if (inManual)
{ // just entered auto mode, so tell.
inManual = false;
system("espeak \"Mission resuming.\" -ven+f4 -s130 -a40 2>/dev/null &");
bridge->send("oled 3 running AUTO\n");
}
switch(mission)
{
case 1: // running auto mission
ended = mission1(missionState);
break;
case 2:
ended = mission2(missionState);
break;
case 3:
ended = mission3(missionState);
break;
case 4:
ended = mission4(missionState);
break;
default:
// no more missions - end everything
finished = true;
break;
}
if (ended)
{ // start next mission part in state 0
mission++;
ended = false;
missionState = 0;
}
// show current state on robot display
if (mission != missionOld or missionState != missionStateOld)
{ // update small O-led display on robot - when there is a change
UTime t;
t.now();
snprintf(s, MSL, "oled 4 mission %d state %d\n", mission, missionState);
bridge->send(s);
if (logMission != NULL)
{
fprintf(logMission, "%ld.%03ld %d %d\n",
t.getSec(), t.getMilisec(),
missionOld, missionStateOld
);
fprintf(logMission, "%ld.%03ld %d %d\n",
t.getSec(), t.getMilisec(),
mission, missionState
);
}
missionOld = mission;
missionStateOld = missionState;
}
}
}
//
// check for general events in all modes
// gamepad buttons 0=green, 1=red, 2=blue, 3=yellow, 4=LB, 5=RB, 6=back, 7=start, 8=Logitech, 9=A1, 10 = A2
// gamepad axes 0=left-LR, 1=left-UD, 2=LT, 3=right-LR, 4=right-UD, 5=RT, 6=+LR, 7=+-UD
// see also "ujoy.h"
if (bridge->joy->button[BUTTON_RED])
{ // red button -> save image
if (not cam->saveImage)
{
printf("UMission::runMission:: button 1 (red) pressed -> save image\n");
cam->saveImage = true;
}
}
if (bridge->joy->button[BUTTON_YELLOW])
{ // yellow button -> make ArUco analysis
if (not cam->doArUcoAnalysis)
{
printf("UMission::runMission:: button 3 (yellow) pressed -> do ArUco\n");
cam->doArUcoAnalysis = true;
}
}
// are we finished - event 0 disables motors (e.g. green button)
if (bridge->event->isEventSet(0))
{ // robot say stop
finished = true;
printf("Mission:: insist we are finished\n");
}
else if (mission > toMission)
{ // stop robot
// make an event 0
bridge->send("stop\n");
// stop mission loop
finished = true;
}
// release CPU a bit (10ms)
usleep(10000);
}
bridge->send("stop\n");
snprintf(s, MSL, "espeak \"%s finished.\" -ven+f4 -s130 -a12 2>/dev/null &", bridge->info->robotname);
system(s);
printf("Mission:: all finished\n");
bridge->send("oled 3 finished\n");
}
Initialization
When the thread is started the following initialization code is executed:
/// initialize robot mission to do nothing (wait for mission lines)
missionInit();
/// start (the empty) mission, ready for mission snippets.
bridge->send("start\n"); // ask REGBOT to start controlled run (ready to execute)
This 'missionInit();' code is repeated here
/**
* Initializes the communication with the robobot_bridge and the REGBOT.
* It further initializes a (maximum) number of mission lines
* in the REGBOT microprocessor. */
void UMission::missionInit()
{ // stop any not-finished mission
bridge->send("robot stop\n");
// clear old mission
bridge->send("robot <clear\n");
//
// add new mission with 3 threads
// one (100) starting at event 30 and stopping at event 31
// one (101) starting at event 31 and stopping at event 30
// one ( 1) used for idle and initialisation of hardware
// the mission is started, but staying in place (velocity=0, so servo action)
//
bridge->send("robot <add thread=1\n");
// Irsensor should be activated a good time before use
// otherwise first samples will produce "false" positive (too short/negative).
bridge->send("robot <add irsensor=1,vel=0:dist<0.2\n");
//
// alternating threads (100 and 101, alternating on event 30 and 31 (last 2 events)
bridge->send("robot <add thread=100,event=30 : event=31\n");
for (int i = 0; i < missionLineMax; i++)
// send placeholder lines, that will never finish
// are to be replaced with real mission
// NB - hereafter no lines can be added to these threads, just modified
bridge->send("robot <add vel=0 : time=0.1\n");
//
bridge->send("robot <add thread=101,event=31 : event=30\n");
for (int i = 0; i < missionLineMax; i++)
// send placeholder lines, that will never finish
bridge->send("robot <add vel=0 : time=0.1\n");
usleep(10000);
//
//
// send subscribe to bridge
bridge->pose->subscribe();
bridge->edge->subscribe();
bridge->motor->subscribe();
bridge->event->subscribe();
bridge->joy->subscribe();
bridge->motor->subscribe();
bridge->info->subscribe();
bridge->irdist->subscribe();
bridge->imu->subscribe();
usleep(10000);
// there maybe leftover events from last mission
bridge->event->clearEvents();
}
The code makes sure that the REGBOT is not in a mission
bridge->send("robot stop\n");
// clear old mission
bridge->send("robot <clear\n");
Then it prepares 3 REGBOT threads (thread 1, 100 and 101) and fills them with a number of mission lines
Thread 1 has one line "robot <add irsensor=1,vel=0:dist<0.2\n" to make sure the IR sensor is active, sets the velocity to 0 and wait until the robot has moved 0.2m. This keeps the REGBOT busy until one of the other threads actually moves the robot. When all threads has reached the last line, then the REGBOT terminates (stops the mission).
Thread 100 and 101 are initiated with a number of lines that are placeholders for actual mission lines. Neither of the threads are activated.
The REGBOT does not support inserting mission lines, so the only way is to override existing lines.
The line "start" is sent to the REGBOT to start control loops and interpreting the mission lines just send.
Safe start
Once the REGBOT is initialized the small line-display (O-LED display)
bridge->send("oled 3 waiting for REGBOT\n");
Then a check to see if the REGBOT is at all responding, if not, then print a clear message on the console.
for (int i = 0; i < 3; i++)
{
if (not bridge->info->isHeartbeatOK())
{ // heartbeat should come at least once a second
sleep(1);
}
}
if (not bridge->info->isHeartbeatOK())
{ // heartbeat should come at least once a second
system("espeak \"Oops, no usable connection with robot.\" -ven+f4 -s130 -a60 2>/dev/null &");
bridge->send("oled 3 Oops: Lost REGBOT!");
printf("# ---------- error ------------\n");
printf("# No heartbeat from robot. Bridge or REGBOT is stuck\n");
printf("# You could try restart ROBOBOT bridge ('b' from mission console) \n");
printf("# -----------------------------\n");
stop();
}
Segment loop
The first part of the loop
while (not finished and not th1stop)
{ // stay in this mission loop until finished
loop++;
// test for manuel override (joy is short for joystick or gamepad)
if (bridge->joy->manual)
{ // just wait, do not continue mission
usleep(20000);
if (not inManual)
system("espeak \"Mission paused.\" -ven+f4 -s130 -a40 2>/dev/null &");
inManual = true;
bridge->send("oled 3 GAMEPAD control\n");
}
else
{ // in auto mode
if (not regbotStarted)
{ // wait for start event is received from REGBOT
// - in response to 'bot->send("start\n")' earlier
if (bridge->event->isEventSet(33))
{ // start mission (button pressed)
regbotStarted = true;
}
}
The loop runs until all mission segments has reported 'finished', or the mission is terminated with a 'quit' command from the console (setting the 'th1stop' flag).
If in manual override mode (gamepad control), then this is displayed on the line display (on line 3).
Once in automatic mode, then wait for the REGBOT to report that it has started interpreting the mission lines - by emitting an event 33.
Mission segment call
The mission segment state 'mission' controls which mission to call
switch(mission)
{
case 1: // running auto mission
ended = mission1(missionState);
break;
case 2:
ended = mission2(missionState);
break;
case 3:
ended = mission3(missionState);
break;
case 4:
ended = mission4(missionState);
break;
default:
// no more missions - end everything
finished = true;
break;
}
if (ended)
{ // start next mission part in state 0
mission++;
ended = false;
missionState = 0;
}
In this example, there are 4 mission functions (the last two is just empty).
A mission will return false all the time until it is finished.
When the actual mission segment returns true, the mission segment is finished. This will increase the mission counter and set the mission state to 0 for the next mission segment.
Global checks
The mission loop is also the place for global functions that should be active regardless of the mission segment.
In this example code, there is assigned gamepad buttons to activate a few 'test' functions.
if (bridge->joy->button[BUTTON_RED])
{ // red button -> save image
if (not cam->saveImage)
{
printf("UMission::runMission:: button 1 (red) pressed -> save image\n");
cam->saveImage = true;
}
}
if (bridge->joy->button[BUTTON_YELLOW])
{ // yellow button -> make ArUco analysis
if (not cam->doArUcoAnalysis)
{
printf("UMission::runMission:: button 3 (yellow) pressed -> do ArUco\n");
cam->doArUcoAnalysis = true;
}
}
The red gamepad button tells the camera thread that an image should be saved to the disk. The yellow button sets another flag to start ArUco code processing.
The flag in the camera class (cam->saveImage and cam->doArUcoAnalysis) will be reset, once the process is finished.
Note that both these processes take time and prohibit other camera functions while being processed .
