Gubr

Gubr
Information
Owner	BradDunagan
Version	1.0
Status	In Progress
Started On	2011-02-11

Overview

Lately there has been quite a bit of interest expressed in microcontrollers by Midsouth Makers. So, maybe the code that runs on my little robot, Gubr, will be useful to some. Gubr does not use an Arduino board but the microcontroller is the same as that of some Arduinos. And according to Pololu, the library they provide is useful (to some degree, I suppose) on Arduinos.

The controller board is the "Orangutan SVP-1284 Robot Controller".

I had not done any embedded work for quite a while so I was somewhat apprehensive about Gubr's software side of the project. But, I was pleasantly surprised by how things went. The hard stuff is taken care of by the library. I just looked at the examples and went from there.

If you look at the listing below you see a big loop. One of the things I was interested in was how much latency there might be in responses to certain things (like, the detection of an oncoming wall). So I keep track of the loop time and display it periodically. Its about 1 to 2 milliseconds. Nearly all of that time is taken by the silly way I am reading the range sensors (see the for ( iA = 0; ... ) loop). Otherwise the main loop time would probably be in the low micosecond range. To be applicable I should display the longest time of any one loop, and possibly the average as I am doing now.

I want the robot to move around, detect obstacles and provide data back to the PC. The PC maintains records and tells the robot what to do.

If there are any questions I will gladly elaborate and maybe add to this article. My email is brad.dunagan@gmail.com.

Here is the program that runs on Gubr -

Source Code

/*
         1         2         3         4         5         6         7         8
12345678901234567890123456789012345678901234567890123456789012345678901234567890
*/
#include    <pololu/orangutan.h>

#include    <stdio.h>

#include    <stdlib.h>

#include    <string.h>

#define IRSNS   5


const char  Bar_130 [] PROGMEM =
{
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11111
};

const char  Bar_110 [] PROGMEM =
{
    0b00000,
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11111
};

const char  Bar_090 [] PROGMEM =
{
    0b00000,
    0b00000,
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11111
};

const char  Bar_070 [] PROGMEM =
{
    0b00000,
    0b00000,
    0b00000,
    0b11111,
    0b11111,
    0b11111,
    0b11111,
    0b11111
};

const char  Bar_050 [] PROGMEM =
{
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b11111,
    0b11111,
    0b11111,
    0b11111
};

const char  Bar_030 [] PROGMEM =
{
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b11111,
    0b11111,
    0b11111
};

const char  Bar_010 [] PROGMEM =
{
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b11111,
    0b11111
};

const char  Bar_000 [] PROGMEM =
{
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b00000,
    0b11111
};

void    PrintRangeBar ( int X, int Y, unsigned int Rng )
{
    lcd_goto_xy ( X, Y );

    if ( Rng > 130 )  print_character ( 7 );
    else
    if ( Rng > 110 )  print_character ( 6 );
    else
    if ( Rng >  90 )  print_character ( 5 );
    else
    if ( Rng >  70 )  print_character ( 4 );
    else
    if ( Rng >  50 )  print_character ( 3 );
    else
    if ( Rng >  30 )  print_character ( 2 );
    else
    if ( Rng >  10 )  print_character ( 1 );
    else
    if ( Rng >   0 )  print_character ( 0 );
}

int main()
{
    unsigned int    AvgRng[6], iA, Sum, nSum;

    unsigned long   TmPrv, TmNow, TmDelta, TmSum, TmCnt, TmBatt, TmPCChk;

    char    Cmd [32], nCmdChars, bGotCmd;

    char    Rsp [32], nRspChars, bSntRsp;

    int     MtrSpdL, MtrSpdR;               //  Motors' speed.  -511 to +512
    int     MtrEncL, MtrEncR;               //  Motors' encoder count.
    int     MtrMovL, MtrMovR;               //  Motors' move-to encoder count.
    int     bMtrRng;                        //  While running better be 
                                            //  connected to the PC.
    int     Batt;                           //  Battery millivolts.

    lcd_init_printf();

    lcd_load_custom_character ( Bar_130, 7 );
    lcd_load_custom_character ( Bar_110, 6 );
    lcd_load_custom_character ( Bar_090, 5 );
    lcd_load_custom_character ( Bar_070, 4 );
    lcd_load_custom_character ( Bar_050, 3 );
    lcd_load_custom_character ( Bar_030, 2 );
    lcd_load_custom_character ( Bar_010, 1 );
    lcd_load_custom_character ( Bar_000, 0 );

    clear();

    set_analog_mode ( MODE_8_BIT );

    time_reset();

    TmPrv = get_ms();       TmSum = TmCnt = TmBatt = TmPCChk = 0;


    serial_set_baud_rate ( UART0, 9600 );

    serial_receive ( UART0, Cmd, 32 );

    bGotCmd = bSntRsp = nCmdChars = 0;

    svp_set_mode ( SVP_MODE_ENCODERS );

    MtrSpdL = MtrSpdR = 0;
    MtrEncL = MtrEncR = 0;      
    MtrMovL = MtrMovR = 0;      bMtrRng = 0;

    while ( 1 )
    {
        for ( iA = 0; iA < 6; iA++ )
        {
            start_analog_conversion ( iA );

            Sum = 0;    nSum = 0;

            while ( 1 )
            {
                while ( analog_is_converting() )
                {
                    delay_us (  200 );
                }

                Sum += analog_conversion_result();

                if ( ++nSum == 10 )  break;
            }

            AvgRng[iA] = Sum / nSum;
        }

        if ( bSntRsp )
        {
            if ( serial_send_buffer_empty ( UART0 ) )
            {
                bSntRsp = 0;

                serial_receive ( UART0, Cmd, 32 );

                bGotCmd = 0;
            }
        }
        else
        if ( (nCmdChars = serial_get_received_bytes ( UART0 )) != 0 )
        {
            bGotCmd = (Cmd[nCmdChars - 1] == '\0');
        }

        PrintRangeBar ( 0, 0, AvgRng[0] );
        PrintRangeBar ( 1, 0, AvgRng[1] );
        PrintRangeBar ( 2, 0, AvgRng[2] );
        PrintRangeBar ( 3, 0, AvgRng[3] );
        PrintRangeBar ( 4, 0, AvgRng[4] );
        PrintRangeBar ( 5, 0, AvgRng[5] );

        if ( bGotCmd && ! bSntRsp )
        {
            TmPCChk = get_ms();

            lcd_goto_xy ( 14, 0 );      printf ( "%c ", Cmd[0] );

            if ( Cmd[0] == 'C' )                //  Comm check.
            {
                strcpy ( Rsp, "Ok" );       nRspChars = 2;
            }
            else
            if ( Cmd[0] == 'R' )    //  IR sensor ranges.
            {
                Rsp[0] = 'R';

                Rsp[1] = (unsigned char)AvgRng[0];
                Rsp[2] = (unsigned char)AvgRng[1];
                Rsp[3] = (unsigned char)AvgRng[2];
                Rsp[4] = (unsigned char)AvgRng[3];
                Rsp[5] = (unsigned char)AvgRng[4];
                Rsp[6] = (unsigned char)AvgRng[5];
                    
                nRspChars = 7;
            }
            else
            if ( Cmd[0] == 'M' )    //  Set motors' speed.
            {
                MtrMovL = MtrMovR = 0;

                char Spd [4];       Spd[3] = 0;

                strncpy ( Spd, &Cmd[1], 3 );        MtrSpdL = atoi ( Spd ) * 8;
                strncpy ( Spd, &Cmd[4], 3 );        MtrSpdR = atoi ( Spd ) * 8;

                if (    (MtrSpdL < -511) || (MtrSpdL > 512)
                     || (MtrSpdR < -511) || (MtrSpdR > 512) )
                {
                    strcpy ( Rsp, "E" );        //  Error
                    
                    nRspChars = 1;

                    MtrSpdL = MtrSpdR = 0;
                }
                else
                {
                    set_motors ( MtrSpdL, MtrSpdR );

                    bMtrRng = MtrSpdL | MtrSpdR;

                    sprintf ( Rsp, "M" );
                    
                    nRspChars = 1;
                }
            }
            else
            if ( Cmd[0] == 'E' )    //  Encoder counts.
            {
                sprintf ( Rsp, "E%05d%05d", MtrEncL, MtrEncR );
                
                nRspChars = 11;
            }
            else
            if ( Cmd[0] == 'V' )    //  Move.
            {
                MtrMovR = atoi ( &Cmd[14] );    Cmd[14] = 0;
                MtrSpdR = atoi ( &Cmd[11] );    Cmd[11] = 0;
                MtrMovL = atoi ( &Cmd[ 4] );    Cmd[ 4] = 0;
                MtrSpdL = atoi ( &Cmd[ 1] );

                MtrSpdL *= 8;
                MtrSpdR *= 8;

                if (    (MtrSpdL < -511) || (MtrSpdL > 512)
                     || (MtrSpdR < -511) || (MtrSpdR > 512) )
                {
                    Rsp[0] = 'E';       //  Error.

                    MtrSpdL = MtrSpdR = 0;
                    MtrMovL = MtrMovR = 0;
                }
                else
                if (    ((MtrSpdL > 0) && (MtrMovL < 0))
                     || ((MtrSpdL < 0) && (MtrMovL > 0))
                     || ((MtrSpdR > 0) && (MtrMovR < 0))
                     || ((MtrSpdR < 0) && (MtrMovR > 0)) )
                {
                    Rsp[0] = 'E';       //  Error.

                    MtrSpdL = MtrSpdR = 0;
                    MtrMovL = MtrMovR = 0;
                }
                else
                {
                    if ( MtrMovL && MtrMovR )
                    {
                        svp_get_counts_and_reset_ab();
                        svp_get_counts_and_reset_cd();

                        set_motors ( MtrSpdL, MtrSpdR );

                        bMtrRng = MtrSpdL | MtrSpdR;
                    }

                    Rsp[0] = 'V';
                }

                nRspChars = 1;
            }
            else
            if ( Cmd[0] == 'S' )    //  Status.
            {
                Rsp[0] = 'S';
                Rsp[1] = (char)(MtrSpdL / 8);   *((short *)&Rsp[2]) = MtrEncL;
                Rsp[4] = (char)(MtrSpdR / 8);   *((short *)&Rsp[5]) = MtrEncR;
                
                nRspChars = 7;
            }
            else
            if ( Cmd[0] == '0' )    //  Zero (reset) the encoder counts.
            {
                svp_get_counts_and_reset_ab();
                svp_get_counts_and_reset_cd();

                strcpy ( Rsp, "0" );
                
                nRspChars = 1;
            }
            else
                memcpy ( Rsp, Cmd, nRspChars = nCmdChars );

            lcd_goto_xy ( 15, 0 );      printf ( "%c", Rsp[0] );

            serial_send ( UART0, Rsp, nRspChars );

            bSntRsp = 1;
        }

        MtrEncL = svp_get_counts_ab();
        MtrEncR = svp_get_counts_cd();

        TmNow   = get_ms();

        if ( bMtrRng && (   (AvgRng[0] > 120)
                         || (AvgRng[1] > 120)
                         || (AvgRng[2] > 120)
                         || (AvgRng[3] > 120)
                         || (AvgRng[4] > 120)
                         || (AvgRng[5] > 120)) )
        {
            set_motors ( 0, 0 );    MtrSpdL = MtrSpdR = 0;
                                    MtrMovL = MtrMovR = 0;
            bMtrRng = 0;
        }


        if ( bMtrRng && (TmNow - TmPCChk > 2000) )
        {
            set_motors ( 0, 0 );    MtrSpdL = MtrSpdR = 0;
                                    MtrMovL = MtrMovR = 0;
            bMtrRng = 0;
        }

        if ( bMtrRng && MtrMovL )
        {
            if (    ((MtrMovL < 0) && (MtrEncL <= MtrMovL))
                 || ((MtrMovL > 0) && (MtrEncL >= MtrMovL)) )
            {
                set_motors ( 0, 0 );    MtrSpdL = MtrSpdR = 0;
                                        MtrMovL = MtrMovR = 0;
                bMtrRng = 0;
            }           
        }

        if ( bMtrRng && MtrMovR )
        {
            if (    ((MtrMovR < 0) && (MtrEncR <= MtrMovR))
                 || ((MtrMovR > 0) && (MtrEncR >= MtrMovR)) )
            {
                set_motors ( 0, 0 );    MtrSpdL = MtrSpdR = 0;
                                        MtrMovL = MtrMovR = 0;
                bMtrRng = 0;
            }           
        }


        TmDelta = TmNow - TmPrv;    
        TmPrv   = TmNow;

        TmSum += TmDelta;       TmCnt++;

        //  Every 30 loops ...
        //
        if ( TmCnt == 30 )
        {
            //  Show the average loop time.
            //
            TmDelta = TmSum / TmCnt;

            TmSum = TmCnt = 0;

            lcd_goto_xy (  8, 0 );      print_unsigned_long ( TmDelta );

            //  And show the encoder counts.
            //
            lcd_goto_xy (  0, 1 );      printf ( "%5d", MtrEncL );
            lcd_goto_xy (  5, 1 );      printf ( "%5d", MtrEncR );
        }

        //  Show battery voltage every 5 seconds.
        //
        if ( (TmBatt == 0) || (TmNow - TmBatt > 5000) )
        {
            Batt = read_battery_millivolts_svp();

            lcd_goto_xy ( 10, 0 );      printf ( "%2d", Batt / 100 );

            TmBatt = TmNow;
        }
    } 
}