Just as a follow-up, since all my apps are built C++ and this is the first time I’ve seen someone substantially interested in doing the same.
Here’s the main method for a typical program; note how it’s using lots of classes and objects, but avoiding dynamic memory allocation entirely:
int main (void)
{
LineFollowConfig lfc;
lfc.kp = 3.0;
lfc.ki = 0;
lfc.kd = 100.0;
lfc.maxSpeed = 245;
lfc.minSpeed = 180;
lfc.maxTurn = 255;
lfc.lostTime = 500;
AdjustValueOperation<double> adjustKP("KP", &lfc.kp, 0.05);
AdjustValueOperation<double> adjustKI("KI", &lfc.ki, 0.05);
AdjustValueOperation<double> adjustKD("KD", &lfc.kd, 5.0);
AdjustValueOperation<int> adjustFast("Fast", &lfc.maxSpeed, 5);
AdjustValueOperation<int> adjustSlow("Slow", &lfc.minSpeed, 5);
AdjustValueOperation<int> adjustTurn("Turn", &lfc.maxTurn, 5);
AdjustValueOperation<int> adjustLost("Lost", &lfc.lostTime, 25);
FollowLineOperation followLine (lfc);
RawLineOperation rawLine;
SimpleSumoOperation simpleSumo;
Operation* ops[] = {
&followLine,
&rawLine,
&simpleSumo,
&adjustKP,
&adjustKI,
&adjustKD,
&adjustFast,
&adjustSlow,
&adjustTurn,
&adjustLost,
};
Menu menu (ops, sizeof(ops) / sizeof(ops[0]));
menu.run();
return 0;
}
The menu class manages a top-level ring menu (buttons 1 and 3 scroll through the menu, button 2 selects–just like the demo program):
#ifndef MENU_H
#define MENU_H
#include <string.h>
#include <common/lcd.h>
class Operation {
public:
virtual const char* getName() { return "(unnamed)"; }
virtual void run() { }
};
class Menu {
public:
Menu(Operation** ops, int count);
void run();
protected:
void show();
protected:
Operation** _operations;
int _mode;
int _modeCount;
};
Menu::Menu(Operation** ops, int count)
{
_operations = ops;
_mode = 0;
_modeCount = count;
}
void Menu::show()
{
const char* name = _operations[_mode]->getName();
LCD::clear();
LCD::center(0, name);
if (HARDWARE_LCD_WIDTH >= 16) {
LCD::center(1, "A:<< B:Go C:>>");
} else {
LCD::center(1, "<< Go >>");
}
}
void Menu::run()
{
for (;;) {
show();
unsigned char button = OrangutanPushbuttons::waitForButton(ANY_BUTTON);
if (button == BUTTON_A) {
_mode = (_mode == 0) ? (_modeCount-1) : (_mode-1);
}
if (button == BUTTON_C) {
_mode = (_mode+1 == _modeCount) ? 0 : (_mode+1);
}
if (button != BUTTON_B) {
continue;
}
_operations[_mode]->run();
while (OrangutanPushbuttons::isPressed(ANY_BUTTON)) {
OrangutanTime::delayMilliseconds(1);
}
}
}
#endif
The AdjustValueOperation class makes it easy to customize constants in your program without recompiling:
#ifndef ADJUST_VALUE_H
#define ADJUST_VALUE_H
#include <common/menu.h>
template<class T> class AdjustValueOperation : public Operation
{
public:
AdjustValueOperation(const char *name, T *val, T step)
{
_name = name;
_value = val;
_step = step;
}
const char* getName()
{
static char name[64];
#if HARDWARE_LCD_WIDTH >= 16
strcpy(name, "Set ");
strcat(name, _name);
strcat(name, " (");
strcat(name, valueToString());
strcat(name, ")");
#else
strcpy(name, _name);
strcat(name, " ");
strcat(name, valueToString());
name[HARDWARE_LCD_WIDTH]=0;
#endif
return name;
}
const char* valueToString()
{
static char name[64];
T value = *_value;
char* out = name;
if (value < 0) {
*out++ = '-';
value = 0 - value;
}
uint32_t mantissa = (uint32_t)value;
if (mantissa == 0) {
*out++ = '0';
} else {
int length = 0;
uint32_t dup = mantissa;
while (dup >= 1) {
dup /= 10;
length++;
}
for (int ii = 0; ii < length; ++ii) {
uint32_t digit = mantissa % 10;
mantissa /= 10;
out[length-ii-1] = digit + '0';
}
out += length;
}
value -= (T)((uint32_t)value);
if ((uint32_t)(value*100) != 0) {
*out++ = '.';
for (int prec = 0; prec < 2 && value != 0; ++prec) {
value *= 10;
uint32_t digit = ((uint32_t)value) % 10;
value -= digit;
*out++ = digit + '0';
}
}
*out = 0;
return name;
}
void run()
{
show();
while (!OrangutanPushbuttons::isPressed(MIDDLE_BUTTON)) {
if (OrangutanPushbuttons::isPressed(TOP_BUTTON)) {
*_value -= _step;
show();
while (OrangutanPushbuttons::isPressed(TOP_BUTTON)) {
OrangutanTime::delayMilliseconds(10);
}
}
if (OrangutanPushbuttons::isPressed(BOTTOM_BUTTON)) {
*_value += _step;
show();
while (OrangutanPushbuttons::isPressed(BOTTOM_BUTTON)) {
OrangutanTime::delayMilliseconds(10);
}
}
}
}
protected:
void show()
{
char text[64];
#if HARDWARE_LCD_WIDTH >= 16
strcpy (text, "Value for ");
strcat (text, _name);
#else
strcpy (text, _name);
#endif
LCD::clear();
LCD::center(0, text);
strcpy (text, valueToString());
LCD::center(1, text);
}
protected:
const char * _name;
T* _value;
T _step;
};
#endif
And here’s the follow line operation from that same thing. All an Operation method needs to implement is a getName() method for the menu to use and a run() method for the menu to invoke:
struct LineFollowConfig
{
double kp;
double ki;
double kd;
int maxSpeed;
int minSpeed;
int maxTurn;
int lostTime;
};
class FollowLineOperation : public Operation
{
public:
FollowLineOperation(LineFollowConfig &lfc) : _lfc(lfc)
{
}
const char* getName()
{
return "Follow";
}
void run()
{
MotorController motor (HARDWARE_MOTOR_ORDER);
motor.setSpeed(_lfc.maxSpeed);
LineSensor line (HARDWARE_LINESENSOR_PINS);
line.setEmitter (ROBOT_LINESENSOR_QTR);
Indicator indicator (HARDWARE_INDICATOR);
IndicatorConfig lights (&indicator);
lights.configure (INDICATOR_TARGET, INDICATOR_GREEN);
lights.configure (INDICATOR_SEEK, INDICATOR_BLUE);
lights.configure (INDICATOR_CONTROL, INDICATOR_RED);
Every display (15);
Every startup (1000);
Timer lost (_lfc.lostTime);
int startupCount = 4;
double integralError = 0;
while (!OrangutanPushbuttons::isPressed(MIDDLE_BUTTON) &&
!OrangutanPushbuttons::isPressed(TOP_BUTTON) &&
!OrangutanPushbuttons::isPressed(BOTTOM_BUTTON) &&
!indicator.isButtonPressed()) {
if (startupCount != 0) {
if (startup.ready()) {
LCD::clear();
if ((--startupCount) == 0) {
lights.mode(INDICATOR_TARGET);
} else {
LCD::center(0, "Start In");
OrangutanLCD::gotoXY((LCD::width()-4)/2, 1);
OrangutanLCD::print(startupCount);
OrangutanLCD::print("...");
}
}
OrangutanTime::delayMilliseconds(1);
continue;
}
int position = line.read();
if (position == LINESENSOR_BAR) {
lights.mode(INDICATOR_CONTROL);
integralError = 0;
motor.stop();
} else if (position != LINESENSOR_NO_LINE) {
if (line.confidence() == 0) {
lights.mode(INDICATOR_SEEK);
lost.restart();
motor.setSpeed(_lfc.minSpeed);
} else if (lost.ready()) {
lost.stop();
motor.setSpeed(_lfc.maxSpeed);
lights.mode(INDICATOR_TARGET);
}
// Error ranges from -50..50, with 0 meaning we're doing great,
// negative numbers mean we need to turn to the left and
// positive numbers mean we need to turn to the right.
int error = position - 50;
// Proportional is just KP * error
double proportional = _lfc.kp * error;
// Derivative is KD * (change in error)
double derivative = _lfc.kd * errorVelocity(error);
// Integral is KI (sum of errors), and we clamp it to prevent runaway.
double maxintegral = (_lfc.maxTurn * 0.5) / _lfc.ki;
integralError += ((double)error / 50);
integralError = max(integralError, 0-maxintegral);
integralError = min(integralError, maxintegral);
double integral = _lfc.ki * integralError;
// Now drive the motors, eh?
double adjust = proportional + derivative + integral;
if (adjust > _lfc.maxTurn) {
adjust = _lfc.maxTurn;
} else if (adjust < 0-_lfc.maxTurn) {
adjust = 0-_lfc.maxTurn;
}
motor.setTurn((int)adjust,255);
}
if (display.ready()) {
OrangutanLCD::clear();
if (position == LINESENSOR_NO_LINE) {
LCD::center(0, "no line");
} else if (position == LINESENSOR_BAR) {
LCD::center(0, "stop");
} else {
position = (position == 100) ? 99 : position;
OrangutanLCD::gotoXY(LCD::width() * position / 100,0);
OrangutanLCD::print("|");
}
}
motor.update();
OrangutanTime::delayMilliseconds(1);
}
}
protected:
double errorVelocity (double currentError)
{
static double lastError = 0;
static double currentVelocity = 0;
static double lastVelocity = 1;
static uint32_t elapsed = 1;
static uint32_t elapsedAtLastChange = 1;
elapsed++;
if (currentError == lastError) {
if ((elapsed = min(elapsed,30000)) > elapsedAtLastChange) {
currentVelocity = lastVelocity / elapsed;
}
} else {
currentVelocity = (currentError - lastError) / elapsed;
lastVelocity = currentVelocity;
elapsedAtLastChange = elapsed;
elapsed = 0;
}
lastError = currentError;
return currentVelocity;
}
protected:
LineFollowConfig &_lfc;
};