State machines: A supervisor controller

In this section we construct a more complicated state machine, called Supervisor and shown in the figure below. The basic job of the Supervisor machine is to activate and deactivate lower level controllers, which may be state machines themselves, based on certain events. This particular Supervisor machine pays attention to remote control events reported by the RemoteControl module. The controllers it activates make the machine calibrate, stand, sit and do pushups. The first three behaviors are governed by the standard RHexLib modules CalibMachine, StandMachine and SitMachine. The last behavior is governed by the PushupController constructed in a previous tutorial.

Note that this custom implementation of the supervisor is different from the facilities provided by the ModeSupervisor, which imposes a very particular structure on the transitions between different behaviors and the nature of user commands through a network channel. Finally, as many of the components used by this supervisor are RHex specific, we are not able to provide a complete implementation in the examples directory. Most of the modules that are used in the implementation are not part of the core distribution.

A state machine, called Supervisor, which is implemented in this section. This machine ``supervises'' the activity of other controllers, including other state machines. The events are either command events (which check the remote control state) or time events. Based on the remote control activity, the machine calibrates, stands, sits or does ``pushups''.

The transition table that corresponds to Figure above, and which should be saved in a file called Supervisor.dsc is as follows:

  Transition uncalibrated calCommand calibrating
  Transition calibrating calFail uncalibrated
  Transition calibrating calSuccess idle
  Transition idle standCommand runningStand
  Transition runningStand standDone standing
  Transition standing sitCommand runningSit
  Transition runningSit sitDone idle
  Transition standing pushupCommand doingPushups
  Transition doingPushups stopCommand runningStand
  Initial uncalibrated

There are seven states. uncalibrated is the initial state, corresponding to the situation that the robot has just been turned on. calibrating is the state the robot will be in while running a calibration state machine for each leg. In the idle state, the robot will be sitting with its motor drives deactivated to save power. In the runningStand state, the robot will be running the standMachine state machine which brings the robot to a standing position. In standing state, the robot will have completed the standMachine and will be waiting to either sit or start doing pushups. In the state doingPushups, the robot will be running the pushupController from the previous tutorials. Finally, in runningSit, the robot will be running the SitMachine controller.

There are five ``command'' events which will correspond to remote control events. First, the calCommand::check method, which signals the user's intention to calibrate the robot's legs, will be true when the left joystick is pushed forward. The standCommand::check method, which signals the user's intention to stand the robot up from idle mode, will be true when the left joystick is pushed backward. The pushupCommand::check method, which signals the user's intention to start the pushup controller from standing mode, will be true when the left joystick is pushed forward. The stopCommand::check method, which signals the user's intention to stop the pushup controller, will be true when the left joystick is pushed backward. The sitCommand::check method, which signals the user's intention to make the robot sit when in standing mode, will be true when the left joystick is pushed to the left.

The other events, calibSuccess, calibFail, standDone and sitDone, correspond to the various controllers coming to completion. For example, when the StandMachine has finished, its isDone flag returns true. Thus, standDone::check calls this method. As the figure shows, while each of the controllers is running, the Supervisor will be in a mode that is waiting for a signal that the controller has completed, before moving on to a state that waits for the next user input. The exception is the doingPushups state, which is terminated upon a remote control action from the user.

Coding the Supervisor

We will see that there are very few things to be done to code up this seemingly complex state machine. First, we use sm-setup.pl to create skeleton header and code files from the Supervisor.dsc file. This creates 90% of the code for the Supervisor machine.

The header, Supervisor.hh, is practially finished. We just need to include certain header files from RHexLib which define the other machines to be used and add variables to the Supervisor class to point to the modules it will need. We also suppose that PushupController.hh has been created and placed in the same directory as the supervisor code. The resulting header file is as follows:

#ifndef _SUPERVISOR_HH
#define _SUPERVISOR_HH

#include "ModuleManager.hh"
#include "StateMachine.hh"

// The next six includes have been added to the basic
// file produced by sm-setup.pl
#include "StdModules.hh"
#include "StandMachine.hh"
#include "SitMachine.hh"
#include "CalibMachine.hh"
#include "RemoteControl.hh"
#include "PushupController.hh"

class Supervisor : public StateMachine {

  public:

    Supervisor ( void );
    ~Supervisor ( void );
    void init ( void );
    void activate ( void );
    void deactivate ( void );

  private:

    // events
    EventObject ( CalCommand ) * calCommand;
    EventObject ( CalFail ) * calFail;
    EventObject ( CalSuccess ) * calSuccess;
    EventObject ( StandCommand ) * standCommand;
    EventObject ( StandDone ) * standDone;
    EventObject ( SitCommand ) * sitCommand;
    EventObject ( SitDone ) * sitDone;
    EventObject ( PushupCommand ) * pushupCommand;
    EventObject ( StopCommand ) * stopCommand;

    // states
    StateObject ( Uncalibrated ) * uncalibrated;
    StateObject ( Calibrating ) * calibrating;
    StateObject ( Idle ) * idle;
    StateObject ( RunningStand ) * runningStand;
    StateObject ( Standing ) * standing;
    StateObject ( RunningSit ) * runningSit;
    StateObject ( DoingPushups ) * doingPushups;

    // These six pointers have been added to the basic file produced
    // by sm-setup.pl. They will point to other modules used by the Supervisor
    PushupController * puControl;
    StandMachine     * standMach;
    SitMachine       * sitMach;
    CalibMachine     * cm[6];
    RemoteControl    * rc;

};

#endif

The next step is top modify the code file, Supervisor.cc, produced by sm-setup.pl. We will do this in several steps:

• Modify Supervisor::activate() to search for the required modules (for standing, sitting, etc.) and set up the module pointers that we added to Supervisor.hh.
• Modify Supervisor::activate() and Supervisor::deactivate() to grab and release the remote control module.
• Modify the event check methods to check for remote control events and controller isDone events.
• Modify the state entry, during and exit methods (not all of them) so that they activate and deactivate the various modules responsible for calibrating, standing, sitting and doing pushups.

First we modify the init method to search for the required modules:

void Supervisor::init ( void ) {

  int i;

  // this is where to search for the needed modules and set up pointers to them

  if ( ( standMach = ( StandMachine * ) 
         MMFindModule( STANDMACHINE_NAME, 0 )) == NULL)
     MMFatalError ( "Supervisor::init", "Cannot find Stand Machine" );
  
  if ( ( sitMach = ( SitMachine * ) 
         MMFindModule( SITMACHINE_NAME, 0 )) == NULL)
     MMFatalError ( "Supervisor::init", "Cannot find Sit Machine" );
  
  if ( ( rc = ( RemoteControl * ) 
         MMFindModule( REMOTECONTROL_NAME, 0 )) == NULL)
     MMFatalError ( "Supervisor::init", "Cannot find Remote Control" );

  if ( ( puControl = ( PushupController * ) 
         MMFindModule( "pushupcontroller", 0 )) == NULL)
     MMFatalError ( "Supervisor::init", "Cannot find Push Up Controller" );

  for ( i = 0; i < 6; i++ )
    if ( ( cm[ i ] = ( CalibMachine * ) 
           MMFindModule( CALIBMACHINE_NAME, i )) == NULL)
      MMFatalError ( "Supervisor::init", "Cannot find Calibration Machine" );

  StateMachine::init();

}

Next, we modify the activate and deactivate methods so that they grab, configure and release the remote control module. The arguments to rc->configure are the delays of the right and left joystick inputs. A deley of 0.005$s$, for example, means that the value has to be stable for 0.005 seconds before it is reported. The rc->setThreshold method sets the value above which the joystick is considered to be pushed in a particular direction. In the deactivate method, we also check to see if any of the modules that the Supervisor uses are active, and if they are, we deactivate them.

void Supervisor::activate ( void ) {

  // Configure the RC sticks
  rc->configure ( 0.005, 0.3 );
  rc->setThreshold ( 0.5 );

  // Grab and activate the remote control interface
  MMGrabModule( rc, this );

}

void Supervisor::deactivate ( void ) {

  // Release the previously grabbed modules
  MMReleaseModule( rc, this );

  if ( standMach->getState() == MODULE_ACTIVE ) MMReleaseModule ( standMach, this );
  if ( sitMach->getState() == MODULE_ACTIVE ) MMReleaseModule ( sitMach, this );
  if ( puControl->getState() == MODULE_ACTIVE ) MMReleaseModule ( puControl, this );

  for ( i=0; i<6; i++ )
    if ( cm[i]->getState() == MODULE_ACTIVE ) MMReleaseModule ( cm[i], this );

  StateMachine::deactivate();

}

The Event check Methods

Next, we add meaning to the event check methods. We start with calibCommand. This amounts to checking the value returned by the rc->leftSick and rc->rightStick methods. The values returned by these methods correspond to the cardinal map directions (north, west, northwest, etc.) Thus, the calibCommand::check method below returns true if the left joystick is pushed directly forward.

bool Supervisor::CalCommand::check ( void ) { 

  // Left RC stick pushed forward
 return bool ( OWNER->rc->leftStick() == RemoteControl::NORTH );

}

The next two events check for either the sucessful completion of all the calibration machines (one for each leg) or the unsucessful completion of at least one of them. CalibMachine::getStatus returns CalibMachine::FAILURE in the case that calibration failed, CalibMachine::CALIBRATING if calibration is still underway and CalibMachine::SUCCESS if calibration suceeded.

bool Supervisor::CalFail::check ( void ) {

  int i;

  // return true if any motor reports failure
  for ( i = 0; i < 6; i++ )
    if ( OWNER->cm[i]->getStatus() == CalibMachine::CALIBRATING ) return false;
  for ( i = 0; i < 6; i++ )
    if ( OWNER->cm[i]->getStatus() == CalibMachine::FAILURE ) return true;
  
  return false;

}

bool Supervisor::CalSuccess::check ( void ) {

  // return true if all motors report success
  int i;

  for ( i = 0; i < 6; i++ )
    if ( OWNER->cm[i]->getStatus() != CalibMachine::SUCCESS ) return false;
  
  return true;

}

The stand command is issued by pushing the left joystick directly backward.

bool Supervisor::StandCommand::check ( void ) { 

  // Left RC stick pushed back
  return bool ( OWNER->rc->leftStick() == RemoteControl::SOUTH );

}

Via the public interface to StandMachine, other modules can perceive two states. Either the machine is in the midst of standing, in which case StandMachine::isDone returns false or the machine is completed its task and is holding steady, and isDone returns true.

bool Supervisor::StandDone::check ( void ) { 

  return OWNER->standMach->isDone();

}

To sit, push the left joystick directly to the left.

bool Supervisor::SitCommand::check ( void ) {

  // Left RC stick pushed left
  return bool ( OWNER->rc->leftStick() == RemoteControl::WEST );

}

The SitMachine interface is similar to the StandMachine interface.

bool Supervisor::SitDone::check ( void ) {

  return OWNER->sitMach->isDone();

}

To start doing pushups, push the joystick directly forward while in standing mode.

bool Supervisor::PushupCommand::check ( void ) {

  // Left RC stick pushed forward
  return bool ( OWNER->rc->leftStick() == RemoteControl::NORTH );

}

To stop doing pushups, push the left joystick directly backward.

bool Supervisor::StopCommand::check ( void ) {

  // Left RC stick pushed back
  return bool ( OWNER->rc->leftStick() == RemoteControl::SOUTH );

}

The State Methods

The next and final step is to modify the state methods. Note that we do not modify them all as they are not all needed. For example, none of the during methods are changed at all as will often be the case in supervisor state machines. To see how the during methods are used, see the code of, for example, StandMachine. Here, only some of the entry and exit methods are actually required. Mostly what they do is activate and deactivate other modules. Activation of a module is done with the MMGrabModule function. Deactivate is done with the MMReleaseModule function.

There are six caibration machines to be activated upon entry into the calibrating state as follows. This method also sets the calibration method of each machine to GROUND which makes the robot's legs spin until the meet with some resistance (persumably the ground).

void Supervisor::Calibrating::entry ( void ) {

  int i;

  for ( i = 0; i < 6; i++ ) {

    OWNER->cm[i]->setMode( CalibMachine::GROUND );
    MMGrabModule ( OWNER->cm[i], owner );

  }
}

Upon completion of the calibrating state, when either calibSucess or calibFail become active, we release the calibration machines, thereby deactivating them.

void Supervisor::Calibrating::exit ( void ) {

  int i;

  for ( i = 0; i < 6; i++ )
    MMReleaseModule ( OWNER->cm[i], owner );
}

Successful calibration leaves the robot calibrated with the motor drives off. Thus, to start standing, we need to enable the motor drives and then grab the stand machine.

void Supervisor::RunningStand::entry ( void ) {

  int i;

  for ( i = 0; i < 6; i++ )  // enable motor drives
    DriveHW::instance()->driveEnable( i, true );
  
  MMGrabModule ( OWNER->standMach, owner );
}

We leave the standing machine running during the standing state though it is in the standing position. This is because the StandMachine holds the robot in the upright position after it is done. We release (and deactivate) the standing machine only when leaving the standing state on our way either to sitting or doing pushups.

void Supervisor::Standing::exit ( void ) {

  MMReleaseModule ( OWNER->standMach, owner );

}

To start sitting, we grab the sit machine (don't let anyone just anyone grab your sit machine!).

void Supervisor::RunningSit::entry ( void ) {  

  MMGrabModule ( OWNER->sitMach, owner );

}

When the SitMachine is done, we release it and also disable the motor drives to save power.

void Supervisor::RunningSit::exit ( void ) {

  int i;

  for ( i = 0; i < 6; i++ )  // disable motor drives
    DriveHW::instance()->driveEnable( i, false );

  MMReleaseModule ( OWNER->sitMach, owner );
}

Finally, it is easy to incoorporate the pushup controller, we just grab and release it as needed:

void Supervisor::DoingPushups::entry ( void ) {

  MMGrabModule ( OWNER->puControl, owner );

}

void Supervisor::DoingPushups::exit ( void ) {

  MMReleaseModule ( OWNER->puControl, owner );

}

A main() File

One more piece is required to get the Supervisor runnning. We need a main file. The following will do.

#include <stdio.h>
#include "sysutil.hh"
#include "ModuleManager.hh"
#include "StdModules.hh"
#include "StateMachine.hh"
#include "Supervisor.hh"
#include "RemoteControl.hh"
#include "PushupController.hh"

class UserModule : public Module {

  public:

    UserModule ( void ) : Module( "userinput", 0, false, false ) { };

    void  init ( void ) {}
    void  uninit ( void ) {}
    void  activate ( void ) {}
    void  deactivate ( void ) {}
    void  update ( void ) {

      int i;

      if ( kbhit() ) {

        // Diable all motor drives
        for ( i = 0 ; i < 6 ; i++) 
          DriveHW::instance()->driveEnable ( i, false );

        MMShutdown();
        MMPowerOff( );

      }
    }
} umod;

int main ( void ) {

  initHardware();

  // Read robot specific configuration file
  MMReadConfigFile( "rhex_michigan.rc" );

  // Read the application dependent configuration file
  MMReadConfigFile( "sup.rc" );

  Supervisor supervisor;
  PushupController puc;

  // Create and add all the standard modules
  RHexAddStdModules();

  // Add the user module, pushup controller and supervisor
  MMAddModule ( &umod, 1, 0, USER_CONTROLERS );
  MMAddModule ( &puc, 1, 0, USER_CONTROLLERS );
  MMAddModule ( &supervisor, 1, 0, USER_CONTROLLERS );

  // Activate the supervisor modules
  MMActivateModule ( &supervisor );

  MMMainLoop();

  cleanupHardware();

  MMShutdown();

  return 0;
}

The machines for calibration, standing and sitting as well as the remote control module are all added to the Module Manager by the command RHexAddStdModules. Only the Supervisor and the PushupController are added in main(). Also, only the Supervisor module is activated in main(). It takes care of activating everything else it needs. We have also added a simple module, called UserModule, which allows the user to stop the program by pressing any key.

Note, we have chosen to configure this program to use the rhex_michigan.rc file.

---

RHexLib Reference Documentation