Robobot software building blocks
Back to Robobot.
Back to Robobot software description
Contents |
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 'bplan20' is shown below; first the class definition in bplan20.h (most comments removed):
1 #pragma once
2 using namespace std;
3 class BPlan20
4 {
5 public:
6 ~BPlan20();
7 void setup();
8 void run();
9 void terminate();
10 private:
11 void toLog(const char * message);
12 int state, oldstate;
13 bool toConsole = true;
14 FILE * logfile = nullptr;
15 bool setupDone = false;
16 };
17 extern BPlan20 plan20;
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 12-15 are other private variables.
Line 17 creates a reference to an instance of this class (the instance is created in the implementation file bplan20.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 are 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);
state = 21;
}
else if (t.getTimePassed() > 10)
lost = true;
break;
case 21:
if (pose.turned >= M_PI)
{
mixer.setDesiredHeading(M_PI);
mixer.setVelocity(0.3);
// reset driven distance
pose.dist = 0;
state = 31;
}
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 state time for new state
t.now();
}
// wait a bit to offload CPU
usleep(2000);
}
if (lost)
{ // there may be better 'lost' 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.
