Thursday, February 7, 2013

Arduino Event Driven State Machine Motor Controller

If you have been following along, you will remember that I've based my asynchronous device framework (ADF) on Quantum Leap's event driven state machine. I'm using the basic underlying state machine as a "single threaded" (RTC - run to completion non-preemptive) virtual process machine with inter-process communications. The modifications turned out to be fairly simple, and I've updated my framework to hide some of the QF implementation details.

To start, I'll describe one of the virtual asynchronous processes, my DC Motor controller. This process, like all of the other processes, gets a slice of time to process any messages that are pending in it's message queue. Any other process can send it a message, which is queued up, and then they are processed and delivered by the QF machine to my "active" class method. When each message is processed, the active method exits and the next message is processed until all of the messages are consumed.

The process does not execute unless there is a message for it to work on, so no cpu cycles are expended unless necessary. Additionally, the process must subscribe to receive each particular message, and all subscribers can send as well as receive messages, and more than one process can subscribe to the same message.

So my motor controller does nothing but receive instructions and send physical information to GPIO pins on the Arduino board. Currently the controller supports a 2 pin H-Bridge DC motor controller and a simple servo. However, what makes this process unique is that it can support any number of H-Bridges and Servos. This was my primary reason for creating this class.

So lets look at what the motor controller subscribes to:

QState DCMotorControl::initial(DCMotorControl *me, QEvent const *e)
{
    me->m_ProcessID = Q_EVT_CAST(DDEventArg)->pinNumber;
    me->subscribe(DCMC_ADD_MOTOR_SIG);
    me->subscribe(DCMC_SET_STATE_SIG);
    return Q_TRAN(&DCMotorControl::active);
}

During Quantum Leaps setup, the ::initial class method is called and during that call it subscribes to two events (messages), DCMC_MOTOR_SIG and DCMC_SET_STATE_SIG. These are defined in a globals.h file and are unique enumerated values. The last line of the code above just transfers control of the events to the ::active method of this class, and the first line setting m_ProcessID is something that I added to uniquely identify this particular process. I use the priority because all of the, up to 64, processes have unique priorities.

To assist other classes in sending messaged events to this controller class, I've only exposed two methods:

void DCMotorControl_Add(uint8_t motorID, 
   DCMD_CONTROLTYPE motorControlType, uint8_t pin1, uint8_t pin2)
{
  DCMDInitEventArg *ea  = Q_NEW(DCMDInitEventArg, DCMC_ADD_MOTOR_SIG);
  ea->pin1              = pin1;
  ea->pin2              = pin2;
  ea->motorID           = motorID;
  ea->controlType       = motorControlType;
  QF::PUBLISH(ea, DCMC_ADD_MOTOR_SIG);
}


void DCMotorControl_Set(uint8_t motorID, uint8_t amount, DCMD_MODE motorMode)
{
  DCMDEventArg *ea = Q_NEW(DCMDEventArg, DCMC_SET_STATE_SIG);
  ea->motorID      = motorID;
  ea->amount       = amount; 
  ea->motorMode    = motorMode; 
  QF::PUBLISH(ea, DCMC_SET_STATE_SIG);
}


These two methods are expose for all of the other processes to use to add a motor and set a motor. Basically the code assembles a DCMDInitEventArg message and posts it to the message queue, since these are the messages that this class subscribes to, they arrive in the ::active method to be parsed.

When a motor is added, it has one or more pins to control, it should have a unique motor ID and control type. This then creates a virtual motor, and all set events use this virtual motor ID. For example, I have an H Bridge DC motor and a Servo motor defined as:


const uint8_t    DCMOTORin1          =    5;
const uint8_t    DCMOTORin2          =    6;
const uint8_t    MOTION_MOTOR        =    56; 

// id of the motor on pin 5&6, an arbitrary value but unique to this motor

const uint8_t    BALANCESERVOpin     =    8;
const uint8_t    BALANCE_SERVO       =    8;



The code I use in another class to create the DC motor and Servo are as follows:

DCMotorControl_Add( MOTION_MOTOR,  DCMD_TYPE_HBRIDGE,
            DCMOTORin1,       DCMOTORin2  );
DCMotorControl_Add( BALANCE_SERVO, DCMD_TYPE_SERVO,  
           BALANCESERVOpin,   0          );


So with two simple calls, I can create two virtual motor devices, a DC H Bridge motor and a Servo. Then to control the DC motor, all I need to do is make a single call:

DCMotorControl_Set(MOTION_MOTOR, 50, DCMD_MODE_FORWARD);

To make the motor run at 50% power in the forward direction, and to change the servo to 90 degrees I call:

DCMotorControl_Set(BALANCE_SERVO,  90, DCMD_MODE_NONE);


Also remember these calls are made in a separate process in it own "thread" of execution. So the call only posts a message to the motor controller, when the motor controller runs, it processes any messages waiting for it. The advantage is that the motors can be controlled by completely separate "threads" or processes. I quote the thread because there is only one thread for the entire framework, it is divided up among the processes that have a message to process.

So how does the motor controller handle the asynchronous control messages?

In the active method of the motor control class, we watch for a DCMC_ADD_MOTOR_SIG signal event message which looks like this:

QState DCMotorControl::active(DCMotorControl *me, QEvent const *e)
{
    switch (e->sig)
    {
       ...

        case DCMC_ADD_MOTOR_SIG:
        {
            VMODevice *bod = new VMODevice(
                Q_EVT_CAST(DCMDInitEventArg)->motorID,
                Q_EVT_CAST(DCMDInitEventArg)->controlType,
                Q_EVT_CAST(DCMDInitEventArg)->pin1,
                Q_EVT_CAST(DCMDInitEventArg)->pin2);
            bod->m_servo = NULL;

            if( Q_EVT_CAST(DCMDInitEventArg)->controlType 

                                == DCMD_TYPE_SERVO )
            {
                bod->m_servo = new Servo();
                bod->m_servo->attach(bod->m_Pin1);
            }
            bod->m_NextVMODevice = me->m_VMODeviceList;
            me->m_VMODeviceList = bod;
            break;
        }


The QState method is a Quantum Leaps requirement for the class, and the active method is called with two arguments, "me" a pointer to this instance of the class, and "e" a pointer to the event message. Also, the Q_EVT_CAST is a Quantum Leaps built in macro to help cast the event arguments into the correct structure type. The message itself contains the information sent in the post message, and in this case we saw that it was:

      QF::PUBLISH(ea, DCMC_ADD_MOTOR_SIG);

that was described in the Add motor method. So we have a case/switch that detects the Add motor signal. In this case the code creates a new virtual class (struct) called VMODevice and populates it with motor ID, pin numbers etc. Then if the type of motor is a servo it creates a servo class object for this servo and does the appropriate attach call for initialization. The last two lines before the break link this new virtual motor object into a linked list. In this way we can create any number of motors with the caveat that we do not delete motors. This is important because linked lists, in this amount of memory space are not useful if they are added and deleted because memory becomes fragmented and eventually the system memory would becomes unusable. But if we only create, then fragmentation is limited and does not grow.

So when the class gets a message to set a motor value, the case is:

  case DCMC_SET_STATE_SIG:
  {
     DCMCListWalker(me->m_VMODeviceList, Q_EVT_CAST(DCMDEventArg));
     break;
  }

In this case, we walk through the linked list of motors until we match the motor ID:

void DCMCListWalker( VMODevice * link, DCMDEventArg const * device )
{
    if(link != NULL)
    {
        if( link->m_MotorID == device->motorID )
        {
            if(link->m_controlType == DCMD_TYPE_HBRIDGE)
            {
                uint8_t amount = map(device->amount, 0, 100, 0, 255);
                if(device->motorMode == DCMD_MODE_COAST )
                {
                    analogWrite(link->m_Pin1, 0);
                    analogWrite(link->m_Pin2, 0);
                }
                else if(device->motorMode == DCMD_MODE_FORWARD )
                {
                    analogWrite(link->m_Pin1, amount);
                    analogWrite(link->m_Pin2, 0);
                }
                else if(device->motorMode == DCMD_MODE_REVERSE )
                {
                    analogWrite(link->m_Pin1, 0);
                    analogWrite(link->m_Pin2, amount);
                }
                else // DCMD_MODE_STOP
                {
                    analogWrite(link->m_Pin1, 255);
                    analogWrite(link->m_Pin2, 255);
                }
            }
            if(link->m_controlType == DCMD_TYPE_SERVO)
            {
                link->m_servo->write(device->amount);
            }
        }

        else if(link->m_NextVMODevice != NULL )  // not it, go deeper
            DCMCListWalker(link->m_NextVMODevice, device);
    }
}

the list walker uses recursion to go through the linked list until a match is found, then the motor type is determined, and the appropriate calls are made to the device.

Now for a top level look at my processes and various states.

I have a process called MyController. It subscribes to timer events, motion controller events and collision events from an accelerometer. In the accelerator event handler it sends an event to a separate motion controller process:

           if( ADXL345Handler_WasTappedOnce( e ) )
            {
                  ...
                  MotionController_Stop();

The motion controller process, in addition to creating the DC motor as described before, also subscribes to accelerometer axis events, timer events and its control events like the stop command above:

   case MOTION_CONTROL_SIG:
   {
     me->m_moveState = Q_EVT_CAST(DDEventArg)->state;
     break;
   }

 and when it gets a regular accelerometer event, it can handle the stop request:

      ...
      else if(me->m_moveState == MOTION_CONTROL_STOP)
      {
         //
         DCMotorControl_Set(MOTION_MOTOR, 0, DCMD_MODE_COAST);


Which turns the DC motor off. All during this time, as the accelerometer axis events come into the motion controller the y axis is captured, and every 200 milliseconds during a timer event, the servo is adjusted:

   else if (timerID == BALANCETIMER)
   {
      ...
      DCMotorControl_Set(BALANCE_SERVO,  me->m_Balance, DCMD_MODE_NONE);

Both the DC motor and the servo are controlled by different processes. You might ask why the DC motor isn't controlled by the main controller and that is because the motion controller monitors the accelerometer axis to determine what forward and reverse mean, as well as reports stalls and other motion business.

All of my code is currently running in about 17K and includes the following 5 processes:

    ADFTimer_Start          ( ++priority );

    DCMotorControl_Start    ( ++priority );
    ADXL345Handler_Start    ( ++priority );
    MotionController_Start  ( ++priority );

    MyController_Start      ( ++priority );


When this gets running well, I'll start to add LEDs, sound and other sensors, all running in separate process and independently.

An additional note is the ADF Timer process uses a linked list on timers, so any process can create and use as many timers as it needs. This makes each process capable of doing multiple functions at different times. As I mentioned, I use a timer to adjust the balance servo at times independent of the DC motor control.

If you are interest in the base code, please refer to my google code here.

No comments:

Post a Comment