Robobot software building blocks: Difference between revisions

From Rsewiki
Line 64: Line 64:
=== Class implementation file ===
=== Class implementation file ===


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


  #include <string>
  #include <string>
  #include <string.h>
  #include <string.h>
  #include <math.h>
  #include <math.h>
#include <unistd.h>
#include "mpose.h"
#include "steensy.h"
  #include "uservice.h"
  #include "uservice.h"
  #include "mpose.h"
  #include "sencoder.h"
  #include "utime.h"
  #include "utime.h"
#include "cmotor.h"
#include "cservo.h"
#include "medge.h"
#include "cedge.h"
  #include "cmixer.h"
  #include "cmixer.h"
#include "bplan1.h"
   
   
  // create class instance
#include "bplan20.h"
  BPlan1 plan1;
  // create class object
  BPlan20 plan20;
   
   
  void BPlan1::setup()
  void BPlan20::setup()
  { // ensure there is default values in ini-file
  { // ensure there is default values in ini-file
   if (not ini.has("plan1"))
   if (not ini.has("plan20"))
   { // no data yet, so generate some default values
   { // no data yet, so generate some default values
     ini["plan1"]["log"] = "true";
     ini["plan20"]["log"] = "true";
     ini["plan1"]["run"] = "false";
     ini["plan20"]["run"] = "false";
     ini["plan1"]["print"] = "true";
     ini["plan20"]["print"] = "true";
   }
   }
   // get values from ini-file
   // get values from ini-file
   toConsole = ini["plan20"]["print"] == "true";
   toConsole = ini["plan20"]["print"] == "true";
   //
   //
   if (ini["plan1"]["log"] == "true")
   if (ini["plan20"]["log"] == "true")
   { // open logfile
   { // open logfile
     std::string fn = service.logPath + "log_plan1.txt";
     std::string fn = service.logPath + "log_plan20.txt";
     logfile = fopen(fn.c_str(), "w");
     logfile = fopen(fn.c_str(), "w");
     std::fprintf(logfile, "%% Mission plan1 logfile\n");
     fprintf(logfile, "%% Mission plan20 logfile\n");
     fprintf(logfile, "%% 1 \tTime (sec)\n");
     fprintf(logfile, "%% 1 \tTime (sec)\n");
     fprintf(logfile, "%% 2 \tInteger or float (for Matlab to plot\n");
     fprintf(logfile, "%% 2 \tMission state\n");
     fprintf(logfile, "%% 3 \t%% Mission status (mostly for debug)\n");
     fprintf(logfile, "%% 3 \t%% Mission status (mostly for debug)\n");
   }
   }
   setupDone = true;
   setupDone = true;
  }
  }
  BPlan1::~BPlan1()
 
  BPlan20::~BPlan20()
  {
  {
   terminate();
   terminate();
  }
  }
   
   
  void BPlan1::run()
  void BPlan20::run()
  {
  {
   if (not setupDone) setup();
   if (not setupDone) setup();
   toLog("Plan1 started");
  //
   toLog("Forward 0.1 m/s start");
  UTime t;
  mixer.setVelocity(0.1);
  bool finished = false;
  sleep(3);
  bool lost = false;
  toLog("reverse 0.1m/s and turning +1 rad/s (CCV)");
  state = 10;
    mixer.setTurnrate(3.14);
  oldstate = state;
    usleep(500000);
  //
    //
   toLog("Plan20 started");
    toLog("Servo 1 to -600 (500 units/sec) and vel=0");
   //
    servo.setServo(1, 1, -600, 500);
  while (not finished and not lost and not service.stop)
    mixer.setVelocity(0.5);
  {
    mixer.setTurnrate(0.0);// but continue turning
    switch (state)
     sleep(1);
    { // make a shift in heading-mission
     //
      case 10:
        toLog("Reset pose");
        '''pose.resetPose();'''
        toLog("forward at 0.3m/s");
        mixer.setVelocity(0.3);
        t.now();
        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);
   }
   }
    toLog("Servo 1 disable and stop");
  else
     servo.setServo(1, 0);
     toLog("Plan20 finished");
    mixer.setVelocity(0.0);
    sleep(1);
  toLog("Plan1 finished");
  }
  }
   
   
  void BPlan1::terminate()
  void BPlan20::terminate()
  {  
  { //
   fclose(logfile);
   if (logfile != nullptr)
    fclose(logfile);
  logfile = nullptr;
  }
  }
   
   
  void BPlan1::toLog(const char* message)
  void BPlan20::toLog(const char* message)
  {
  {
   UTime t("now");
   UTime t("now");
Line 142: Line 215:
   {
   {
     fprintf(logfile, "%lu.%04ld %d %% %s\n", t.getSec(), t.getMicrosec()/100,
     fprintf(logfile, "%lu.%04ld %d %% %s\n", t.getSec(), t.getMicrosec()/100,
             logCnt,
             oldstate,
             message);
             message);
   }
   }
   if (toConsole)
   if (toConsole)
     ...
  {
   logCnt++;
     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 ''#include'' files are library definitions if in <math.h> brackets or local definitions if in "usupport.h" quotes.
Line 161: Line 238:
Each function definition starts with the class type name followed by '::'.
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.:
The setup reads from the configuration structure. The structure itself is provided by the ''uservice'' module, e.g.:
   if (ini["plan1"]["log"] == "true")
   if (ini["plan20"]["log"] == "true")
where the section is [plan1] and the item ''log''. The (hertil)
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 ''::run()'' has the needed behaviour code.
 
forklar lidt
 
 
The function is actually called from the ''main.cpp'' code.

Revision as of 16:35, 29 December 2023

Back to 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.

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:

#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;
 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);
       t.now();
       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 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 ::run() has the needed behaviour code.

forklar lidt


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