 | Nod Robot Example Last Modified: 2006-11-06 | | |
| Acroname Robotics | |||
| | Nod Robot Example Last Modified: 2006-11-06 | | |
| Acroname Robotics | |||
 Introduction In this example, a BrainStem controls the behaviors of Acroname's Nod Robot. Nod uses a CMUcam to track a bright orange ball. He will also track any object in a bright orange t-shirt! The latest revision to Nod is a Devantech SP03 speech module. This example demonstrates a way to implement an embedded software interface to the CMUcam board and speech board. The software also shows how to implement some tracking logic this particular system. Theory The CMUcam has a serial interface. The serial port on the BrainStem is used primarily for programming, but it can also be used as a general purpose serial port.
Nod's CMUcam uses v1.12 firmware and is configured for 9600 baud communciation. Since the Stem uses TTL voltage levels for serial communication, the Stem must be connected to the TTL serial lines on the CMUcam and the CMUcam's MAX232 level-shifter must be removed. The program configures the camera to use its raw IO mode. This cuts down the amount of data that the BrainStem must process. The BrainStem must be configured for internal heartbeat mode (which can be done with the cmdVAL_SET command). Otherwise, the BrainStem will try to send heartbeat packets up the serial link and that would interfere with the camera. The CMUcam has a green LED that may be turned on and off manually. It is useful as a debugging tool to check the operation of the serial link. Nod's program has a blink routine that turns the green LED on and off. It acts as a status indicator during the initialization routine. Part of the purpose of the blink routine is to act as a delay. When the camera is operating in ASCII mode, it responds to all commands with an ACK. The delay in the blink routine gives the CMUcam time to respond with its ACK before the Stem transmits another command. This keeps the Stem and CMUcam synchronized. One of the final steps in the initialization routine is to switch the green LED back to its automatic mode. In this mode, the green LED turns on when the camera sees an object that matches its current color-tracking parameters. This is also a very helpful debugging feature. The tracking routines use proportional control. The location of the centroid of the ball is compared to the desired X,Y location. The head moves an amount proportional to the Y error in an attempt to keep the ball centered vertically. A medium servo speed setting of 8 (which can be set with the cmdSRV_CFG command) helps to makes the neck motion smoother, otherwise it can be quite jerky. The body pans with a speed proportional to the X error to keep the ball centered horizontally. If the size of the ball is too small, that means the ball is too far away and Nod adds some forward velocity to approach it. If the size of the ball is too large, that means it is too close so Nod backs up. The thresholds for the size tracking had to be determined experimentally. Nod can perform about 7 tracking updates per second. Source Code - Configuring Nod Getting Nod's systems to work properly requires several steps in a specific order. The first task is to disconnect the camera and connect the serial port of the BrainStem to the host computer. The Nod TEA program must be compiled and loaded into the BrainStem in file slot 0. See the following sections depending on which firmware your particular BrainStem has. After loading the program, the following batch file "nod.bag" must be executed using the batch command. The batch file initializes the servos and system settings for proper operation. Nod must be turned off and the camera must be connected to the BrainStem's serial port. Nod uses a custom-built cable to connect the camera to the Stem. It is made from a BrainStem IIC connector and Crimp Pack. The final step is to attach a common power connection to the camera and BrainStem. Once the power is turned on, the BrainStem and CMUcam go through their initialization sequences. Turning on both devices at the same time ensures that the camera will receive the BrainStem's initialization commands. If all goes well, Nod "nods" 3 times, then the green LED on the CMUcam blinks 5 times, and Nod is ready to play! /* filename: nod.bag */
2 31 0 128 /* cmdSRV_CFG, channel=0, range=128 */
2 31 1 128 /* cmdSRV_CFG, channel=1, range=128 */
2 31 2 128 /* cmdSRV_CFG, channel=2, range=128 */
2 31 3 136 /* cmdSRV_CFG, channel=3, range=136 */
2 32 0 0x11 0x23 /* cmdSRV_LMT, channel=0, position=17, resolution=35 */
2 32 1 0x11 0x24 /* cmdSRV_LMT, channel=1, position=17, resolution=36 */
2 32 2 0x11 0x23 /* cmdSRV_LMT, channel=2, position=17, resolution=35 */
2 32 3 0x05 0x29 /* cmdSRV_LMT, channel=3, position=5, resolution=41 */
2 33 0 128 /* cmdSRV_ABS, channel=0, ABSPOS=128 */
2 33 1 128 /* cmdSRV_ABS, channel=1, ABSPOS=128 */
2 33 2 128 /* cmdSRV_ABS, channel=2, ABSPOS=128 */
2 33 3 146 /* cmdSRV_ABS, channel=3, ABSPOS=146 */
2 18 5 1 /* cmdVAL_SET, internal heartbeat */
2 18 15 0 /* cmdVAL_SET, bootstrap VM in slot 0 */
2 19 /* cmdVAL_SAV, save settings to EEPROM */
2 20 /* cmdSRV_SAV, save servo configuration settings */
Source Code - TEA Program (Build 8 and earlier) The earliest version of Nod's program is listed below.
/* filename: noddeprecated.tea */
/* CMUcam BrainStem Example */
/* This code is deprecated and should only be used with
* old BrainStem controllers with Firmware 8 and earlier.
*/
#include <aCore.tea>
#include <aDig.tea>
#include <aServo.tea>
#include <aPPRK.tea>
#include <aGP.tea>
/* global absolute addressable buffer
* for holding serial data from CMUcam
*
* the buffer starts at absolute stack
* address 0
*/
unsigned char b00=0;
unsigned char b01=0;
unsigned char b02=0;
unsigned char b03=0;
unsigned char b04=0;
unsigned char b05=0;
unsigned char b06=0;
unsigned char b07=0;
unsigned char b08=0;
unsigned char b09=0;
unsigned char b10=0;
/* outputs a text string followed by 0x0D byte */
void tx_command(string s)
{
asm {
pushsb 3
next:
brz end
pushsb 1
pushlb 4
addb
pushsbx
popbm aPortSerial
decb 1
goto next
end:
popb
pushlb 0x0D
popbm aPortSerial
}
}
/* blinks the CMUcam tracking light */
void blink()
{
aCore_Sleep(1600);
tx_command("L1 1");
aCore_Sleep(400);
tx_command("L1 0");
aCore_Sleep(400);
}
/*
* reset camera
* enable poll mode
* enable middle mass tracking mode
* set auto-gain and exposure of 40
* set color tracking parameters for orange ball
* set auto mode for tracking light
* set raw IO mode and ACK/NCK suppression
*/
void init_CMUcam()
{
tx_command("RS");
blink();
tx_command("PM 1");
blink();
tx_command("MM 1");
blink();
tx_command("CR 19 32 16 40");
blink();
tx_command("TC 146 240 30 50 15 19");
blink();
tx_command("L1 2");
aCore_Sleep(2000);
tx_command("RM 7");
aCore_Sleep(2000);
}
void track()
{
asm
{
/* send raw output TC command */
pushlb 'T'
popbm aPortSerial
pushlb 'C'
popbm aPortSerial
pushlb 0
popbm aPortSerial
/* wait for raw input reply */
/* 255,M,mx,my,x1,y1,x2,y2,pix,conf,: */
pushmb aPortSerial
pushmb aPortSerial
pushmb aPortSerial
pushmb aPortSerial
pushmb aPortSerial
pushmb aPortSerial
pushmb aPortSerial
pushmb aPortSerial
pushmb aPortSerial
pushmb aPortSerial
pushmb aPortSerial
/* save bytes into global buffer */
popssa 9
popssa 7
popssa 5
popssa 3
popssa 1
popbsa 0
}
}
void nod()
{
aServo_Relative(3,-30);
aCore_Sleep(2500);
aServo_Relative(3,30);
aCore_Sleep(2500);
}
void main()
{
int i=1;
int n=0;
int err;
char v2;
char v3;
unsigned char neck;
nod();
nod();
nod();
aCore_Sleep(15000);
init_CMUcam();
while (1)
{
track();
if (b08>0)
{
n=10;
/* track in y (tilt) */
err=(72-b03)/4;
aServo_Relative(3,(char)err);
/* track in x (pan) */
err=(b02-40);
/* track in z if ball is at or below eye height */
v2= 0;
v3= 0;
neck=aServo_GetAbsolute(3);
if (neck>(unsigned char)160)
{
if (b08<30)
{
/* forward if ball is small */
v2= 80;
v3= -80;
}
else if (b08>50)
{
/* backward if ball is large */
v2= -80;
v3= 80;
}
}
/* do turn and pursuit */
aPPRK_Go(0,v2,v3,(char)err);
}
else
{
/* keep doing last turn if ball is lost */
/* stop turning after a few iterations */
if (n>0)
{
n=n-1;
aPPRK_Go(0,0,0,(char)err);
}
else
{
aPPRK_Go(0,0,0,0);
}
}
}
}
Source Code - TEA Program (Current Versions) The following program is the latest code that uses the BrainStem libraries released with the Build 14 software download. It only works with BrainStem modules that have firmware Build 9 or later. This code takes advantage of the new Build 9 serial port functions which make the code more compact and efficient. The new port functions also make it easier to connect serial devices in a network of BrainStem modules. Because the command strings are sent using TEA opcodes in hard-coded asm statements, the new program looks a lot longer than the old one. When compiled, the new program is only 64 bytes longer than the old program yet it does considerably more. This code also makes use of the new SP03 library functions. Now Nod can talk! He says "Nod is ready" when he is turned on. He says "Where's my ball" when he loses sight of his orange ball. And if the ball is held up too high for him to chase it, he gets upset and says "Do not tease me." /* nodcurrent.tea */
/* CMUcam plus SP03 BrainStem Example */
/* GP 1.0 Build 9 */
#include <aCore.tea>
#include <aServo.tea>
#include <aPPRK.tea>
#include <aSP03.tea>
#define SAY_READY 1
#define SAY_WHEREBALL 7
#define SAY_NOTEASE 10
/* global absolute addressable buffer
* for holding serial data from CMUcam
*
* the buffer starts at absolute stack
* address 0
*/
unsigned char b00=0;
unsigned char b01=0;
unsigned char b02=0;
unsigned char b03=0;
unsigned char b04=0;
unsigned char b05=0;
unsigned char b06=0;
unsigned char b07=0;
unsigned char b08=0;
unsigned char b09=0;
unsigned char b10=0;
/* blinks the CMUcam tracking light */
void blink()
{
asm
{
pushls 1600
popsm aPortVMTimer
pushls 0x4C31 /* "L1 1\\n" */
pushls 0x2031
pushls 0x0D05
popbm aPortSerialNTX
pushls 400
popsm aPortVMTimer
pushls 0x4C31 /* "L1 0\\n" */
pushls 0x2030
pushls 0x0D05
popbm aPortSerialNTX
pushls 400
popsm aPortVMTimer
}
}
/*
* reset camera (twice just to be sure)
* enable poll mode
* enable middle mass tracking mode
* set auto-gain and exposure of 40
* set color tracking parameters for orange ball
* set auto mode for tracking light
* set raw IO mode and ACK/NCK suppression
*/
void init_CMUcam()
{
asm
{
pushls 0x5253 /* "RS\\n" */
pushls 0x0D03
popbm aPortSerialNTX
}
blink();
asm
{
pushls 0x5253 /* "RS\\n" */
pushls 0x0D03
popbm aPortSerialNTX
}
blink();
asm
{
pushls 0x504D /* "PM 1\\n" */
pushls 0x2031
pushls 0x0D05
popbm aPortSerialNTX
}
blink();
asm
{
pushls 0x4D4D /* "MM 1\\n" */
pushls 0x2031
pushls 0x0D05
popbm aPortSerialNTX
}
blink();
/* "CR 19 32 16 40\\n" */
asm
{
pushls 0x4352 /* "CR" */
pushls 0x2031 /* " 1" */
pushls 0x3920 /* "9 " */
pushls 0x3332 /* "32" */
pushls 0x2031 /* " 1" */
pushls 0x3620 /* "6 " */
pushls 0x3430 /* "40" */
pushls 0x0D0F /* 13, 15 */
popbm aPortSerialNTX
}
blink();
/* "TC 146 240 30 50 15 19\\n" */
asm
{
pushls 0x5443 /* "TC" */
pushls 0x2031 /* " 1" */
pushls 0x3436 /* "46" */
pushls 0x2032 /* " 2" */
pushls 0x3430 /* "40" */
pushls 0x2033 /* " 3" */
pushls 0x3020 /* "0 " */
pushls 0x3530 /* "50" */
pushls 0x2031 /* " 1" */
pushls 0x3520 /* "5 " */
pushls 0x3139 /* "19" */
pushls 0x0D17 /* 13, 23 */
popbm aPortSerialNTX
}
blink();
asm
{
pushls 0x4C31 /* "L1 2\\n" */
pushls 0x2032
pushls 0x0D05
popbm aPortSerialNTX
pushls 2000 /* sleep 2000 */
popsm aPortVMTimer
pushls 0x524D /* "RM 7\\n" */
pushls 0x2037
pushls 0x0D05
popbm aPortSerialNTX
pushls 3000 /* sleep 3000 */
popsm aPortVMTimer
}
}
void track()
{
asm
{
/* send raw output TC command */
pushls 0x5443 // push bytes "TC" 0 3
pushls 0x0003
popbm aPortSerialNTX
/* wait for 11-byte raw input reply */
/* 255,M,mx,my,x1,y1,x2,y2,pix,conf,: */
pushlb 11
popbm aPortSerialNRX
/* discard size of read */
popb
/* save bytes into global buffer */
popssa 9
popssa 7
popssa 5
popssa 3
popssa 1
popbsa 0
}
}
void nod()
{
aServo_Relative(3,-30);
aCore_Sleep(2500);
aServo_Relative(3,30);
aCore_Sleep(2500);
}
void main()
{
int i=1;
int n=0;
int errx,erry;
char v2;
char v3;
char chigh=0;
char cQuery=0;
unsigned char neck;
/* SP03 board says "Nod is ready" at power-up */
nod();
nod();
nod();
init_CMUcam();
aSP03_SpeakPhrase(SAY_WHEREBALL);
aSP03_WaitForCompletion();
while (1)
{
track();
if (b08>0)
{
/* ball has been found */
/* reset query flag and "last track" counter */
n=10;
cQuery=1;
/* get errors in y (tilt) and x (pan)*/
erry=(72-b03)/4;
errx=(b02-40);
/* move neck */
aServo_Relative(3,(char)erry);
/* track in z if ball is at or below eye height */
v2=0;
v3=0;
neck=aServo_GetAbsolute(3);
if (neck>(unsigned char)160)
{
chigh=0;
if (b08<30)
{
/* forward if ball is small */
v2= 80;
v3= -80;
}
else if (b08>50)
{
/* backward if ball is large */
v2= -80;
v3= 80;
}
}
else
{
/* ball is too high for Nod to chase it */
/* he considers this teasing!!! */
chigh=chigh+1;
if (chigh==15)
{
aSP03_SpeakPhrase(SAY_NOTEASE);
chigh=0;
}
}
/* do pan and pursuit */
aPPRK_Go(0,v2,v3,(char)errx);
}
else
{
/* keep doing last turn if ball is lost */
/* in an attempt to reacquire the ball */
/* after a few iterations consider ball lost */
/* then stop turn and ask for ball */
if (n>0)
{
n=n-1;
aPPRK_Go(0,0,0,(char)errx);
}
else
{
aPPRK_Go(0,0,0,0);
if (cQuery)
{
aSP03_SpeakPhrase(SAY_WHEREBALL);
aSP03_WaitForCompletion();
cQuery=0;
}
}
}
}
}
Revision History:
| ||||||
| voice: 720-564-0373, email: sales@acroname.com, address: 4822 Sterling Dr., Boulder CO, 80301-2350, privacy © Copyright 1994-2012 Acroname, Inc., Boulder, Colorado. All rights reserved. |
