Mission code
Back to robobot.
Back to Robobot mission application.
Contents |
Mission 1 code
This mission segment function is called very about 10ms until it returns true (finished). When the mission is in manual mode, then code is not called.
/**
* Run mission
* \param state is kept by caller, but is changed here
* therefore defined as reference with the '&'.
* State will be 0 at first call.
* \returns true, when finished. */
bool UMission::mission1(int & state)
{
bool finished = false;
// First commands to send to robobot in given mission
// (robot sends event 1 after driving 1 meter)):
switch (state)
{
case 0:
// tell the operatior what to do
printf("# press green to start.\n");
system("espeak \"press green to start\" -ven+f4 -s130 -a5 2>/dev/null &");
bridge->send("oled 5 press green to start");
state++;
break;
case 1:
if (bridge->joy->button[BUTTON_GREEN])
state = 10;
break;
case 10: // first PART - wait for IR2 then go fwd and turn
snprintf(lines[0], MAX_LEN, "vel=0 : ir2 < 0.3");
// drive straight 0.6m - keep an acceleration limit of 1m/s2 (until changed)
snprintf(lines[1], MAX_LEN, "vel=0.2,acc=1:dist=0.6");
// stop and create an event when arrived at this line
snprintf(lines[2], MAX_LEN, "event=1, vel=0");
// add a line, so that the robot is occupied until next snippet has arrived
snprintf(lines[3], MAX_LEN, ": dist=1");
// send the 4 lines to the REGBOT
sendAndActivateSnippet(lines, 4);
// make sure event 1 is cleared
bridge->event->isEventSet(1);
// tell the operator
printf("# case=%d sent mission snippet 1\n", state);
system("espeak \"code snippet 1.\" -ven+f4 -s130 -a5 2>/dev/null &");
bridge->send("oled 5 code snippet 1");
//
// play as we go
play.setFile("../The_thing_goes_Bassim.mp3");
play.setVolume(5); // % (0..100)
play.start();
// go to wait for finished
state = 11;
featureCnt = 0;
break;
case 11:
// wait for event 1 (send when finished driving first part)
if (bridge->event->isEventSet(1))
{ // finished first drive
state = 999;
play.stopPlaying();
}
break;
case 999:
default:
printf("mission 1 ended \n");
bridge->send("oled 5 \"mission 1 ended.\"");
finished = true;
break;
}
return finished;
}
On the first call, the 'state' variable is 0. The state is used in a case statement.
Green button pressed
On the first state, state 0, the operator is told what to do:
switch (state)
{
case 0:
// tell the operatior what to do
printf("# press green to start.\n");
system("espeak \"press green to start\" -ven+f4 -s130 -a5 2>/dev/null &");
bridge->send("oled 5 press green to start");
state++;
break;
case 1:
if (bridge->joy->button[BUTTON_GREEN])
state = 10;
break;
The 'printf(...)' prints to the console.
Synthetic speach
The line ' system("espeak \"press green to start\" -ven+f4 -s130 -a5 2>/dev/null &"); ' will announce, using the speaker, what to do. Note that the text is in quotes '"' that are escaped by a backslash. The parameter '-ven+f4' is to use a female f4 voice. The '-s130' makes the speak a bit faster (130%). The '-a5' sets the volume to 5%. and the '2>/dev/null' pipes any error messages to the bit bucked (null device).
Google espeak for more details.
O-LED information
Further the line 'bridge->send("oled 5 press...");' will display a line (on line 5) on the small o-led display.
Next state
Once these messages are announced, then state is changed to 1.
On the next call to the function, there is a check to see if the green button is pressed, if it is, then state is changed to 10.
Drive snippet
When the state is t10 the following code is executed:
case 10: // first PART - wait for IR2 then go fwd and turn
snprintf(lines[0], MAX_LEN, "vel=0 : ir2 < 0.3");
// drive straight 0.6m - keep an acceleration limit of 1m/s2 (until changed)
snprintf(lines[1], MAX_LEN, "vel=0.2,acc=1:dist=0.6");
// stop and create an event when arrived at this line
snprintf(lines[2], MAX_LEN, "event=1, vel=0");
// add a line, so that the robot is occupied until next snippet has arrived
snprintf(lines[3], MAX_LEN, ": dist=1");
// send the 4 lines to the REGBOT
sendAndActivateSnippet(lines, 4);
// make sure event 1 is cleared
bridge->event->isEventSet(1);
...
There is an array of C-strings that is used to build the mission snippet.
The line 'snprintf(lines[0], MAX_LEN, "vel=0 : ir2 < 0.3");' prepares the command 'vel=0 : ir2 < 0.3', velocity zero and then wait until the IR2 sensor has a detection that is closer than 0.3m.
The next line is 'vel=0.2,acc=1:dist=0.6' sets velocity to 0.2m/s with a safe acceleration of 1m/s^2. This line then holds until 0.6m is driven.
The third line is 'event=1, vel=0' it will set the speed to 0 and generate an event, event 1. There is no ':' and thus the REGBOT will continue to the next line right away.
The fourth line is ': dist=1' this line has a ':' and will wait until the robot has moved 1m. This will not happen, as the velocity is zero, so the REGBOT will wait for further instructions.
Now 4 lines are prepared, ready to send to the REGBOT and activated.
sendAndActivateSnippet(lines, 4);
It will take some time before the drive is finished and event 1 generated. To be on the safe side the event 1 flag is cleared with the 'isEventSet(1);' (could have been set by a previous mission).
Music gimmick
Now that the robot has speakers, it could be a gimmick to play some music.
There is a couple of lines to play:
case 10: // first PART - wait for IR2 then go fwd and turn
....
// play as we go
play.setFile("../The_thing_goes_Bassim.mp3");
play.setVolume(5); // % (0..100)
play.start();
// go to wait for finished
state = 11;
featureCnt = 0;
break;
The class has a variable called 'play' that has the code to start and stop music files. I have found a royalty-free clip called 'The_thing_goes_Bassim.mp3', so this filename is set to the play object, then the volume is set to just 5% - not to be too loud. And then finally started.
The state is then changed to 11.
Wait for mission snippet event
The 4 lines of REGBOT mission snippet is sent and activated, now we should wait until it has finished. This is what state 11 is doing.
case 11:
// wait for event 1 (send when finished driving first part)
if (bridge->event->isEventSet(1))
{ // finished first drive
state = 999;
play.stopPlaying();
}
break;
case 999:
default:
printf("mission 1 ended \n");
bridge->send("oled 5 \"mission 1 ended.\"");
finished = true;
break;
When the event 1 is detected, the state is set to 999. State 999 is also the default state, and will just set the 'finished' flag to true. This will tell the mission loop that this segment is finished, and will then reset the state to 0 and call the next mission segment instead.
Mission2 code
This part will look for an ArUco code marker in a camera image. If found then move to the marker, if not then turn a bit.
bool UMission::mission2(int & state)
{
bool finished = false;
switch (state)
{
case 0:
system("espeak \"looking for ArUco\" -ven+f4 -s130 -a5 2>/dev/null &");
bridge->send("oled 5 looking 4 ArUco");
state=11;
break;
case 11:
// wait for finished driving first part)
if (fabsf(bridge->motor->getVelocity()) < 0.001 and bridge->imu->turnrate() < (2*180/M_PI))
{ // finished first drive and turnrate is zero'ish
state = 12;
// wait further 30ms - about one camera frame at 30 FPS
usleep(35000);
// start aruco analysis
printf("# started new ArUco analysis\n");
cam->arUcos->setNewFlagToFalse();
cam->doArUcoAnalysis = true;
}
break;
case 12:
if (not cam->doArUcoAnalysis)
{ // aruco processing finished
if (cam->arUcos->getMarkerCount(true) > 0)
{ // found a marker - go to marker (any marker)
state = 30;
// tell the operator
printf("# case=%d found marker\n", state);
system("espeak \"found marker.\" -ven+f4 -s130 -a5 2>/dev/null &");
bridge->send("oled 5 found marker");
}
else
{ // turn a bit (more)
state = 20;
}
}
break;
case 20:
{ // turn a bit and then look for a marker again
int line = 0;
snprintf(lines[line++], MAX_LEN, "vel=0.25, tr=0.15: turn=10,time=10");
snprintf(lines[line++], MAX_LEN, "vel=0,event=2:dist=1");
sendAndActivateSnippet(lines, line);
// make sure event 2 is cleared
bridge->event->isEventSet(2);
// tell the operator
printf("# case=%d sent mission turn a bit\n", state);
system("espeak \"turn.\" -ven+f4 -s130 -a5 2>/dev/null &");
bridge->send("oled 5 code turn a bit");
state = 21;
break;
}
case 21: // wait until manoeuvre has finished
if (bridge->event->isEventSet(2))
{// repeat looking (until all 360 degrees are tested)
if (featureCnt < 36)
state = 11;
else
state = 999;
featureCnt++;
}
break;
case 30:
{ // found marker
// if stop marker, then exit
ArUcoVal * v = cam->arUcos->getID(6);
if (v != NULL and v->isNew)
{ // sign to stop
state = 999;
break;
}
// use the first (assumed only one)
v = cam->arUcos->getFirstNew();
// marker position in robot coordinates
float xm = v->markerPosition.at<float>(0,0);
float ym = v->markerPosition.at<float>(0,1);
float hm = v->markerAngle;
// stop some distance in front of marker
float dx = 0.3; // distance to stop in front of marker
float dy = 0.0; // distance to the left of marker
xm += - dx*cos(hm) + dy*sin(hm);
ym += - dx*sin(hm) - dy*cos(hm);
// limits
float acc = 1.0; // max allowed acceleration - linear and turn
float vel = 0.3; // desired velocity
// set parameters
// end at 0 m/s velocity
UPose2pose pp4(xm, ym, hm, 0.0);
printf("\n");
// calculate turn-straight-turn (Angle-Line-Angle) manoeuvre
bool isOK = pp4.calculateALA(vel, acc);
// use only if distance to destination is more than 3cm
if (isOK and (pp4.movementDistance() > 0.03))
{ // a solution is found - and more that 3cm away.
// debug print manoeuvre details
pp4.printMan();
printf("\n");
// debug end
int line = 0;
if (pp4.initialBreak > 0.01)
{ // there is a starting straight part
snprintf(lines[line++], MAX_LEN, "vel=%.3f,acc=%.1f :dist=%.3f",
pp4.straightVel, acc, pp4.straightVel);
}
snprintf(lines[line++], MAX_LEN, "vel=%.3f,tr=%.3f :turn=%.1f",
pp4.straightVel, pp4.radius1, pp4.turnArc1 * 180 / M_PI);
snprintf(lines[line++], MAX_LEN, ":dist=%.3f", pp4.straightDist);
snprintf(lines[line++], MAX_LEN, "tr=%.3f :turn=%.1f",
pp4.radius2, pp4.turnArc2 * 180 / M_PI);
if (pp4.finalBreak > 0.01)
{ // there is a straight break distance
snprintf(lines[line++], MAX_LEN, "vel=0 : time=%.2f",
sqrt(2*pp4.finalBreak));
}
snprintf(lines[line++], MAX_LEN, "vel=0, event=2: dist=1");
sendAndActivateSnippet(lines, line);
// make sure event 2 is cleared
bridge->event->isEventSet(2);
//
// debug
for (int i = 0; i < line; i++)
{ // print sent lines
printf("# line %d: %s\n", i, lines[i]);
}
// debug end
// tell the operator
printf("# Sent mission snippet to marker (%d lines)\n", line);
//system("espeak \"code snippet to marker.\" -ven+f4 -s130 -a20 2>/dev/null &");
bridge->send("oled 5 code to marker");
}
else
{
printf("# No need to move, just %.2fm, frame %d\n",
pp4.movementDistance(), v->frameNumber);
// look again for marker
state = 11;
}
// wait for movement to finish
state = 31;
}
break;
case 31:
// wait for event 2 (send when finished driving)
if (bridge->event->isEventSet(2))
{ // look for next marker
state = 11;
// no, stop
state = 999;
}
break;
case 999:
default:
printf("mission 1 ended \n");
bridge->send("oled 5 \"mission 1 ended.\"");
finished = true;
play.stopPlaying();
break;
}
// printf("# mission1 return (state=%d, finished=%d, )\n", state, finished);
return finished;
}
