Saturday, February 9, 2013

Goof Ball Bot version 0.01 Arduino Project

I've been going on at length now for almost 3 months describing a startup Arduino project. So here is what I'm working on. It's a sphere with a gravity drive. I locomotion is generated by displacing a weight inside a sphere and letting gravity move to device. I know, it doesn't have much power, and I doubt it could climb any significant incline, but it's fun to watch.

I didn't even know if I could get it working until a few weeks ago, and now that it's working I'm pleased with the development process so far.

The arduino board and electronics are mounted on a robotics platform and I'm using a very high torque motor assembly that has its output shaft attached to the sides of a pet critter crawl. This weekend I plan to refactor the layout and add a Lipo rechargeable battery. This will keep me from fumbling with the rechargeable AA's that I'm using now.

My motion controller state machine is using accelerometer axis data to keep the ball rolling forward or backward. My main controller is using accelerometer "crash" events (tap and double tap). It switches direction if the ball taps into something, and it shuts off if it detects a double tap. I use the double tap to stop it so I can power it off without it starting up on me. Because of the double tap shutdown routine, if it happens to bump into something twice it stops; that's why there are breaks in the video, I get it restarted with a single tap.

So here is a 4 minute video of the Goof Ball Bot terrorizing my Maine Coon cats.

Enjoy -

Goof Ball Bot v0.01 by Doug McFarland

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.

Monday, January 28, 2013

Power with a Li-Ion (or LiPo) 3.7 volt cell

I'm running my project with 6 AA re-chargeable batteries. This turns out to be a bit of a pain because the batteries are not easily accessible, so I started looking for an alternative solution.

I found a lot of batteries like the Li-Ion or LiPo re-chargeablebatteries but I was not finding any way to convert that up to 5V. I look over several sites looking for an inverter, or regulator or something that would increase the voltage. Then I discovered the magic word was "boost". There are several types of voltage boosters, and boosters with chargers built in.

Sparkfun sells a Boost Converter that goes from 3.x to 5 volts and a combo board LiPo Charger/Booster. However, during my brief search, I only found one place that sells a LiPo charger/booster that also allows the UBS connection to be used to program your Arduino board, that was SeeeStudio. It called a LiPo Rider (400ma) and LiPo Rider Pro at 1000 ma output.

Although I plan to do most of my programming in wireless mode, it would be nice to occasionally program while charging the battery.

I'll admit I only looked at a few Robotics/Arduino/Microcontroller sites, so there may be other systems out there. Anyhow, this means I can attach my board to this charger/booster with a very short home made USB connector and power it and program it at the same time.

Sunday, January 27, 2013

CritterBot with a Little Sense of Balance

My goal to this point was to put a bot inside a critter crawl ball. I'm not sure why, but it just seemed like a fun thing to do.

So here we have it. I've attached the high torque gear motor axles to 3/4 inch wooden dowels and attached them to the sides of a 13 inch critter crawl that I bought at the pet store.

I specifically made the assembly heavy at one end so I could use gravity as a drive mechanism. Never mind steering at this point, in fact I'm not sure it can ever be steered, but I'll explore that later.

By adding the accelerometer, I'm able to detect the tilt of the assembly. So using this, I should be able to keep the motor from just flipping the assembly over randomly. In other words, I need some sense of balance as I want to keep the assembly horizontal to the floor to maximize the gravity imbalance, thus causing it to roll.

After mounting the accelerometer I created this graph. What I thought would be a difficult algorithm turned out to be quite simple. If I want to move forward, then I should keep the Z axis at zero, and I can simply use the sign of X to determine if I need to add or subtract power from the motor. So if the angle of the assembly is at 45 degrees in the Z axis, and the X axis is positive, then add more power. Simple. If I want to reverse direction, then just the opposite is true if I ignore the sign of the Z axis. The controller has subscribed to accelerometer events, so the forward code uses those events to adjust the speed:

case ADXL345_STATUS_SIG:
{
  int8_t x = Q_EVT_CAST(ADXL345EventArg)->x;
  int8_t z = Q_EVT_CAST(ADXL345EventArg)->z;

  if( moveState == MOVE_STATE_FORWARD )
  {
    if( z > 5 )
    {
      motorSpeed +=  x < 0 ? -10 : +10;

      if( motorSpeed < 0 )
        motorSpeed = 0;
      else if( motorSpeed > 100 )
        motorSpeed = 100;

      DCMotorControl_Set(DC_SPIN_MOTOR, motorSpeed, DCMD_MODE_FORWARD);
    }
  }
}

I've hard coded the values here but eventually I'll make them constants, or variables if the situation arises. So we add plus 10 or minus 10 percent to the motor speed if the z axis is not within 5 degrees of zero.

Backing up to the accelerometer process, it sends out an average angle update to all subscribers

case ADFTIMER_NOTICE:
{
  if( Q_EVT_CAST(ADFTimerEventArg)->processID == me->m_ProcessID )
   {
      ...
      ADXL345EventArg *ea  = Q_NEW(ADXL345EventArg, ADXL345_STATUS_SIG);
      ea->x = me->xAve;
      ea->y = me->yAve;
      ea->z= me->zAve;
      QF::PUBLISH(ea, ADXL345_STATUS_SIG);

at regular intervals. These intervals are based on the accelerometer's subscription to the timer process for an event every 200 milliseconds:

     ADFTimer_StartTimer(me->m_ProcessID, 0, 200);

So the accelerometer takes readings every 200 milliseconds, averages those readings, and sends out an accelerometer event every 5 events, or about one second. What surprised me was that with only a 1 second sample, the CritterBot appeard to behave fairly well. In the following video, you can see the bot climb to 90 degrees and stop because gravity didn't get the ball rolling. But as soon as it starts rolling the bot tries to stay at zero Z axis so you can see and hear it accelerate across the floor.


Cool.

I'll need to experiment more, which I will be able to do as soon as I can program it remotely. I've received my Adafruit parts and built the wireless XBee receiver part, but need my SeeeStudio order to get the transmitter part up and running.

Tuesday, January 22, 2013

Why are PWM ports not all created equal?

When I posted about my H-Bridge motor control, I indicated that it didn't work on ports 10 and 11, but when I switched to ports 5 and 6 everything started to work correctly. I also mentioned that some code examples have you connect to specific ports, not just any ports. What bothered me was that it wasn't obvious why you "must" connect to a specific port. "Are all PWM ports created equal?" kind of question on my part. Well, while I was trolling the Arduino forum I found out about equality when someone asked about "the best" PWM ports to use.

It so happens that the PWM ports are modulated by timers. Ok I knew that. What I didn't know is that ports are assigned to specific timers, not any available timer. Now that makes a difference. From this link we have the following port/timer assignments:

3    PD 3        PWM T2B, Ext Int 1

5    PD 5        PWM T0B
6    PD 6        PWM T0A

9    PB 1        PWM T1A
10   PB 2        PWM T1B

11   PB 3        PWM T2A, MOSI


So timer zero is assigned PWM ports 5 and 6; timer 1 is assigned to pins 9 and 10; and timer 2 is assigned to 3 and 11.

In my application, timer 2 is used for the Quantum Leap Framework, so I could try to use pin 11 for PWM but it will be taken over by my QF/AD framework. So this is a good reason that I couldn't drive the motor from pin 11 via. PWM.

What this chart tells me is that for my specific application, I can't use pins 3 and 11 for PWM, but they are fine as standard digital I/O.

I'm thinking of adding a PWM driven speaker to my project and now I know what ports I can use.

Monday, January 21, 2013

ADF Timer event

It didn't take long to realize that I didn't need all of my devices to be checking on status updates every 10 milliseconds, and I also needed to have some long timed events, like run the motor for 2 seconds. To this end I created a timer event.

For example, I wanted to read the accelerometer x y and z angles every 250 milliseconds.  The timer event class has an add timer method that allows you to specify a "ProcessID" and a "TimerID". The process ID is the class that is creating the timer. Each class gats it's unique process ID from the QF unique priority. These are all encapsulated in the class variable m_ProcessID. This means any class can get its own unique id by referencing me->m_ProcessID. Because each "process" can have multiple timers, you can specify them with timer IDs. The signature of the add method is:

ADFTimer_AddTimer(uint8_t processID, uint8_t timerID, ADFTIMER_TYPE type)

Creating a timer in the ADXL345 class:

ADFTimer_AddTimer(me->m_ProcessID, 0, ADFTIMER_REPEAT);

creates a repeating timer, as opposed to a ADFTIMER_ONSHOT timer. Starting the timer:

ADFTimer_StartTimer(uint8_t processID, uint8_t timerID,
                        uint16_t milliSeconds)

for this process using timer ID 0 for 250 ms is

ADFTimer_StartTimer(me->m_ProcessID, 0, 250);

All we have to do subscribe to the event during class initialization:

me->subscribe(ADFTIMER_NOTICE);

and we will receive notices from the timer. Once a notice comes in we need to ensure it is our timer:

if( Q_EVT_CAST(ADFTimerEventArg)->processID == me->m_ProcessID );

and then we can react to it. In the above case, the accelerometer is only using one timer so we don't need to further determine which timer it is.

So currently I have the following device startups:

    ADFTimer_Start          ( ++priority );
    DigitalODC_Start        ( ++priority );
    DigitalIDC_Start        ( ++priority );
    AnalogIDC_Start         ( ++priority );
    DCMotorControl_Start    ( ++priority );
    ADXL345Handler_Start    ( ++priority );

Plus my controller:

    MyController_Start      ( ++priority );

In about 17K, the ADF platform can:

  • Write to N number of digital output pins
  • Read N number of digital input pins
  • Write to N number of Analog pins
  • Control N number of H-Bridge DC motors
  • Read 1 accelerometer on the IIC bus.
  • Time N number of events
The last point I want to make it that the timer resolution is somewhat variable depending on the number of interrupts per second that are designated in the bsp.h file. For example, I'm currently running at 200 interrupts per second:

#define BSP_TICKS_PER_SEC  200U
#define TICK_DIVIDER ((F_CPU / BSP_TICKS_PER_SEC / 1024) - 1)
#define TICK_MILLIS  (1000/BSP_TICKS_PER_SEC)

This would provide a 5 millisecond time tick. However, if ticks per second is set to the minimum of 66, That is about 15ms per tick. If the time of the event is not exactly a modulo of the tick millis, then the time will be longer by tick millis. This means in my current scenario, my timer is either exactly correct or off by plus 5 ms.

Sunday, January 20, 2013

Simple Accelerometer Data Conversion to Degrees

What I though would be a very simple search turned into hours of research and reading. My task is simple. Read an accelerometer, in this case an ADXL345, and determine what its orientation is in degrees. Geeez, you would have thought I was the first person to do this, so now I'll be the first person to just give you the simple lines of code to do it.

   double xyz[3];
   double ax, ay, az;

   adxl.get_Gxyz(xyz);
   ax = xyz[0];
   ay = xyz[1];
   az = xyz[2];
   double xAngle = atan( ax / (sqrt(square(ay) + square(az))));
   double yAngle = atan( ay / (sqrt(square(ax) + square(az))));
   double zAngle = atan( sqrt(square(ax) + square(ay)) / az);

   xAngle *= 180.00;   yAngle *= 180.00;   zAngle *= 180.00;
   xAngle /= 3.141592; yAngle /= 3.141592; zAngle /= 3.141592;

Yea I know this could have been better written, but it works. There. Done! My 3 hours of research resulted in 11 or so lines of code. There are lots of blogs and DIY posts that go into the physics of accelerometers and go on at length about vector addition, angles, radians etc. But if you're like me all I want to know is if my toy robot fell down and can't get up.

But credits are due. I did find something I could read and understand without a masters in math at www.arduinoos.com. My code is a distillation of code found on page 5.

Note the get.Gxyz method returns (what I understand) are voltage levels from 0 to about 1.1 volts. My ADXL345 is running on IIC, but I can only assume if you are reading analog voltages through analog input pins you would get something similar.

Now, if you use this code you might notice some slight problems. You see the accelerometer should be calibrated, and/or the readings should be normalized. But this code will get you going if your task is rather simple. If you're building a fleet of quadcopters this might not be enough code.

I'm reading angles from 0 to 89.8 degrees in both the positive and negative direction. Now I don't think I'm finished with my angle calculations because I expect my toy to get bounced around a bit and I'll have to sort that out, but this is a simple first start.

I wish I could have found a post like this last night when I started my research, so hopefully you will find this of use, which is the purpose of this blog. Adruinos for Gumps.