View source for Robobot software building blocks

Back to [[Robobot_B | Robobot]].

Back to [[Robobot software description]]

== C++ code ==

The ''Raubase'' software is built in modules with mostly only one function, structured from the 'NASREM' architecture.

[[File:Robobot_level_3.png | 800px]]

Figure. The NASREM model is shown on the top right. This figure is for level 3 and primarily shows the behaviour and vision-related blocks.

=== File names ===

Each module has a header-file with the definition (e.g. bplan.h) of a class and a file with implementation (e.g. bplan.cpp).

The first letter in the filename is related to the NASREM model as:

* sxxxxx.h/.cpp are sensor retrival classes.
* mxxxxx.h/.cpp are modelling and feature extraction classes.
* cxxxxx.h/.cpp are control classes.
* bxxxxx.h/.cpp are behaviour classes.
* uxxxxx.h/.cpp are utility functions.

== C++ class structure ==

All classes have the same base structure.
As an example (of a relatively simple class), the behaviour example called 'bplan1' is shown below; first the class definition in ''bplan1.h'' (all comments removed):

 1   #pragma once
 2   using namespace std;
 3   class BPlan1
 4   {
 5   public:
 6     ~BPlan1();
 7     void setup();
 8     void run();
 9     void terminate(); 
 10  private:
 11    void toLog(const char * message);
 12    int logCnt = 0;
 13    bool toConsole = true;
 14    FILE * logfile = nullptr;
 15    bool setupDone = false;
 16  };
 17  extern BPlan1 plan1;

Line 1 is just to ensure that this definition is read once only.

Line 2 is to simplify some notations

Line 3 Is the start of the class definition, the class names start with Capital letters, following the normal convention for type definitions.

Line 6 Is the destructor that will ensure log files are properly closed.

Line 7-9 define the general functions to start (setup), run and terminate the class. Setup reads the configuration file (robot.ini), and prepares logfiles and other initialization tasks.

Line 11 defines a function to make it easier to save data to the logfile, in this case, the log is text messages. The function adds a timestamp for each call so that the timing of the message can be compared with other logfiles.

Line 13-15 are other private variables.

Line 17 creates a reference to an instance of this class (the instance is created in the implementation file bplan1.cpp).

=== Class implementation file ===

All the function definitions need to be implemented; this is in the '*.cpp' file.
A reduced example for the 'bplan20.cpp' function is shown below:

Some of the more important lines are highlighted in bold.

 #include <string>
 #include <string.h>
 #include <math.h>
 #include <unistd.h>
 #include "mpose.h"
 #include "steensy.h"
 #include "uservice.h"
 #include "sencoder.h"
 #include "utime.h"
 #include "cmotor.h"
 #include "cservo.h"
 #include "medge.h"
 #include "cedge.h"
 #include "cmixer.h"
 
 '''#include "bplan20.h"'''
 
 // create class object
 '''BPlan20 plan20;'''
 
 void BPlan20::setup()
 { // ensure there is default values in ini-file
  if (not ini.has("plan20"))
  { // no data yet, so generate some default values
    ini["plan20"]["log"] = "true";
    ini["plan20"]["run"] = "false";
    ini["plan20"]["print"] = "true";
  }
  // get values from ini-file
  toConsole = ini["plan20"]["print"] == "true";
  //
  '''if (ini["plan20"]["log"] == "true")'''
  { // open logfile
    std::string fn = service.logPath + "log_plan20.txt";
    logfile = fopen(fn.c_str(), "w");
    fprintf(logfile, "%% Mission plan20 logfile\n");
    fprintf(logfile, "%% 1 \tTime (sec)\n");
    fprintf(logfile, "%% 2 \tMission state\n");
    fprintf(logfile, "%% 3 \t%% Mission status (mostly for debug)\n");
  }
  setupDone = true;
 }
 
 BPlan20::~BPlan20()
 {
  terminate();
 }
 
 void '''BPlan20::run()'''
 {
  if (not setupDone) setup();
  //
  UTime t("now");
  bool finished = false;
  bool lost = false;
  state = 10;
  oldstate = state;
  //
  toLog("Plan20 started");
  //
  while ('''not finished''' and '''not lost''' and not '''service.stop''')
  {
    switch (state)
    { // make a shift in heading-mission
      case 10:
        toLog("Reset pose");
        '''pose.resetPose();'''
        toLog("forward at 0.3m/s");
        '''mixer.setVelocity(0.3);'''
        '''state = 11;'''
        break;
      case 11: // wait for distance
        '''if (pose.dist >= 0.3)'''
        { // done, and then
          toLog("now turn at 0.5 rad/s and 0.15m/s");
          // reset turned angle
          pose.turned = 0.0;
          '''mixer.setVelocity(0.0);
          mixer.setTurnrate(0.5);'''
          t.now();
          state = 21;
        }
        else if (t.getTimePassed() > 10)
          lost = true;
        break;
      case 21:
        if (pose.turned >= M_PI)
        {
          '''mixer.setDesiredHeading(M_PI);'''
          toLog("now go back");
          mixer.setVelocity(0.3);
          mixer.setTurnrate(0.0);
          // reset driven distance
          pose.dist = 0;
          state = 31;
          t.now();
        }
        else if (t.getTimePassed() > 12)
          lost = true;
        break;
      case 31: // wait for distance
        if (pose.dist >= 0.3)
        { // the end
          mixer.setVelocity(0.0);
          finished = true;
        }
        else if (t.getTimePassed() > 10)
          lost = true;
        break;
      default:
        toLog("Unknown state");
        lost = true;
        break;
    }
    if (state != oldstate)
    {
      oldstate = state;
      toLog("state start");
      // reset time in new state
      t.now();
    }
    // wait a bit to offload CPU
    usleep(2000);
  }
  '''if (lost)'''
  { // there may be better options, but for now - stop
    toLog("Plan20 got lost");
    mixer.setVelocity(0);
    mixer.setTurnrate(0);
  }
  else
    toLog("Plan20 finished");
 }
 
 void BPlan20::terminate()
 { //
  if (logfile != nullptr)
    fclose(logfile);
  logfile = nullptr;
 }
 
 void BPlan20::toLog(const char* message)
 {
  UTime t("now");
  if (logfile != nullptr)
  {
    fprintf(logfile, "%lu.%04ld %d %% %s\n", t.getSec(), t.getMicrosec()/100,
            oldstate,
            message);
  }
  if (toConsole)
  {
    printf("%lu.%04ld %d %% %s\n", t.getSec(), t.getMicrosec()/100,
           oldstate,
           message);
  }
 }


the ''#include'' files are library definitions if in <math.h> brackets or local definitions if in "usupport.h" quotes.

The next line
 // create a class instance
 BPlan1 plan1;
is the actual creation of the class instance, with the name ''plan1''.

The remaining parts are the definition code for the functions defined in the header-file ''bplan1.h''

Each function definition starts with the class type name followed by '::'.

The setup reads from the configuration structure. The structure itself is provided by the ''uservice'' module, e.g.:
  if (ini["plan20"]["log"] == "true")
where the section is [plan20] and the item ''log''. The parameters in the robot.ini file are all strings; therefore, a string compare is used in the if statement.

=== behaviour code ===

The ''plan20::run()'' has the needed behaviour code. 




The function is actually called from the ''main.cpp'' code.

It implements a state machine with the while - loop and the following case statement:

 state = 10;
 while (not finished and not lost and not service.stop)
 {
   switch (state)
   { // make a shift in heading-mission
     case 10:
         ...

The while loop stops if the ''service.stop'' flag is set, this flag is set if e.g. ctrl-C or the '''stop''' button is pressed.

==== case 10 ====

The first drive command is
      case 10:
        toLog("Reset pose");
        pose.resetPose();
        toLog("forward at 0.3m/s");
        mixer.setVelocity(0.3);
        state = 11;
        break;
It calls a function in the ''mixer'' module and sets the forward velocity to 0.3 m/sec. Then, set the next state to 11.

==== case 11 ====

Here we stay in case 11, waiting for a number of possibilities:

      case 11: // wait for distance
        if (pose.dist >= 0.3)
        { // done, and then
          toLog("now turn at 0.5 rad/s and 0m/s");
          // reset turned angle
          pose.turned = 0.0;
          mixer.setVelocity(0.0);
          mixer.setTurnrate(0.5);
          state = 21;
        }
        else if (t.getTimePassed() > 10)
          lost = true;
        break;

The driven distance is checked (maintained in the ''pose'' module) and checks the state time using a utility function from ''utime.h''.

When the distance is completed, the next function is set:
          pose.turned = 0.0;
          mixer.setVelocity(0.0);
          mixer.setTurnrate(0.5);
The turned angle is reset, the forward velocity is set to zero, and a turn rate of 0.5 radians per second (counterclockwise, as the turn rate is positive).

==== state 21 ====

This state checks the turned angle:
        if (pose.turned >= M_PI)
        {
          mixer.setDesiredHeading(M_PI);
          mixer.setVelocity(0.3);
          pose.dist = 0;
          ...
When the robot has turned 180 degrees (PI radians), the drive mode is set for a specific heading (pi).
          mixer.setDesiredHeading(M_PI);
In this case, it would almost be the same as setting the turn rate to zero, but using the angle may be more accurate.

There are (pt.) 3 heading modes:
* ''mixer.setTurnrate(<rad/sec signed>)'' for a specific turnrate.
* ''mixer.setDesiredHeading(<angle in radians>)'' to keep a specific heading (since last pose reset).
* ''mixer.setEdgeMode(<left or right (bool)>, <edge offset (m)>)'' to follow a line edge.
The call sets the heading mode.

==== state 31 ====

This state finishes the plan20 mission.

The control is returned to the ''main'' code.