(Yet another) balancing robot

Hi, first post here.

I've released the source code for my balancing robot on my Github account, under a GPLv3 license. (Link will follow, as I can't post one on my first message.)

The robot is quite simple, using the same complementary filter + PID control loop you'll find almost everywhere else, but the implementation has some nice details that might be reused by other projects, such as a (very simple) RTOS-like scheduler and a Playstation2 driver (based on Studio Gyokimae's, but with smaller memory requirements).

(By the way: the comments in the code are all in portuguese, but I intend to translate them to english ASAP. The code is pretty easy to follow, though, so they might not even be needed.)

Have fun -- and should you have any questions, just ask :slight_smile:

Here's a photo of it externally (it's built on a standard Patola project box):

And here's a internal shot:

A video that shows it working is also available here:

The link to my repository is: GitHub - lpereira/Balancing-Robot

the comments are easy to translate with

That is ASAP :wink:

Boa Leandro :slight_smile:

Very good one. I am somewhat curious about the RTOS-like scheduler. Can you provide the relevant code so I can take a look ?

Best,

Álvaro

I am somewhat curious about the RTOS-like scheduler. Can you provide the relevant code so I can take a look?

Sure. It's not actually that powerful -- but it was sufficient for this robot. There's not even preemption.

Anyway, the relevant code follows. First, the Task class:

typedef void (*TaskCallback)(TaskManager &);

class Task {
private:
  long m_countdown, m_period;
  TaskCallback m_callback;

  friend class TaskManager;

  long decrementCountdownBy(long by) {
    return m_countdown -= by;
  }
 
  void inline resetCountdown(void) {
    m_countdown = m_period;
  }

  void inline run(TaskManager &tm) {
    m_callback(tm);
  }
 
public:
  Task(TaskCallback callback, long period) : 
  m_callback(callback), m_countdown(period), m_period(period) {
 
  }
};

Each task will be an instance of this class. Like so, for a blinking LED example with 1000ms interval:

void task1(TaskManager &tm)
{
    static ledStatus = false;
    digitalWrite(13, ledStatus^=1);
}

Task tasks[] = { Task(task1, 1000); };

You'll also need the scheduler itself; relevant parts of the code follows:

class TaskManager {
private:
  Task *m_tasks;
  unsigned int m_nTasks;
  unsigned long m_lastTime;
 
  void sleep(void) {
    set_sleep_mode(SLEEP_MODE_IDLE);
    sleep_enable();
    sleep_mode();
    sleep_disable();
  }
public:
  TaskManager(Task *tasks, int n_tasks) : 
  m_tasks(tasks), m_nTasks(n_tasks), m_lastTime(0L) {
 
  }

  void inline loop() {
    Task *task;
    unsigned int n_task;
    unsigned long elapsedTime = millis() - m_lastTime;
 
    for (n_task = m_nTasks; n_task > 0; n_task--) {
      task = &m_tasks[n_task];
 
      if (task->decrementCountdownBy(elapsedTime) <= 0) {
        task->run(*this);
        task->resetCountdown();
      }
    }
 
    m_lastTime = millis();
    sleep();
  }
};

So, if you call the loop() method of the task manager, all tasks will be executed roughly at the frequency you told them to execute:

TaskManger tm(tasks, 1);

void loop()
{
  tm.loop(); 
}

As there is no preemption, you should be aware that a task won't be stopped in middle-air. Also, there is no guarantee that the task execution periods will be respected. It is crude, but sufficient for most things people use Arduinos for :slight_smile:

Very cool! Nice work, keep it up!

That task-manager RTOS thingie is pretty slick; thanks for posting that (this should go in the playground).

:slight_smile:

So this is basically a event-driven dispatcher, using timers, right ?

Any plans on a real scheduler+ctx switcher ?

I've been thinking about that for a while now, and I find that register+stack saving/restoring would be quite slow. And would need some sort of signalling too.

Álvaro

Any plans on a real scheduler+ctx switcher ?

I've thought about that, but since there are already RTOSes compatible with Atmega168/328, I don't think it's a good idea to reinvent the wheel :slight_smile:

Two systems that might be interesting to look and try to use on an Arduino: http://www.femtoos.org/ and http://funkos.sourceforge.net/

Thanks for the links. Will try them when I have some spare time.

Álvaro

Nice job leandro :wink:

what type of motors are you using ??

It seems very stable, good job! What type of sensors is it using? and what brands/models?