Mission code

Revision as of 16:13, 16 January 2020

Back to robobot.

Back to Robobot mission application.

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;
}