Gubr
Revision as of 22:46, 11 February 2011 by BradDunagan (talk | contribs)
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 -
/*
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;
}
}
}