David is a developer for IBM. He can be contacted at [email protected].
Central to the LEGO Mindstorms Robotic Invention System is a programmable brick called the Robotic Command Explorer (RCX) and a visual-programming environment for programming it. The RCX is built around a 16-MHz Hitachi H8/3292 microcontroller and is packaged in a LEGO-enabled case that lets you attach LEGO bricks, beams, sensors, and the like, to it. The RCX, which is about the size of a handheld calculator, stores several programs that can be selected and executed via a small keypad and LCD.
The development environment's user interface consists of graphical elements -- each representing an RCX code instruction -- stacked together to form program logic. Development takes place in a host/target environment, where you create programs on a PC, then download them -- via an infrared transmitter attached to the PC serial port -- to the RCX. The simplicity of the Mindstorms visual interface is required because the package was originally designed for ages 12 and up. Consequently, the usefulness of the interface quickly wears thin for serious programmers.
Addressing this shortcoming, LEGO (http://www.legomindstorms.com/) responded with an SDK that includes the technical reference (in PDF format) for the Spirit OCX control that's installed with the Mindstorms software package. The document includes a brief description on using the Spirit commands and a few unsupported examples using Visual Basic. It also includes good descriptions of each Spirit command, along with snippets of Visual Basic code where possible. The same OCX control is used for the LEGO Technic CyberMaster, so the document identifies any differences in commands or parameters appropriately.
Robot programs are bytecodes downloaded from a PC to the RCX via the infrared link, then executed by firmware in the RCX. An RCX program can contain one or more tasks, which are like threads in a Windows program. Tasks run concurrently and can share common variables. Generally, a task is created to perform a specific function, such as monitoring a sensor input. Up to 10 tasks can be created in one program in the RCX. The RCX lets you group common code logic in subroutines that can be used by any task. Also, the RCX has 32 global variables available to all tasks and subroutines that are not accessible from the Mindstorms user interface.
Given the Spirit control command set, there are three possible ways to control a robot. In this article, I present a Visual C++ MFC program that combines RCX code logic with C++ code logic to achieve a desired robot behavior. The complete source code and related files are available electronically; see "Resource Center," page 5.
The test robot is the Torbot (Figure 1) described in the "Top Secret Plans" section at the end of the Robotics Invention System Constructopedia. The robot has two motors, each powering a caterpillar-like tread. This mechanism lets the robot move forward and backward when the motors run the treads in the same direction. Turning the robot is achieved by running the treads in opposite directions. On the Torbot's front are two touch sensors that activate when the attached "feelers" hit an object. The feelers are also designed to activate if the robot is on a table and a feeler falls off an edge. The program goal is simple: Make the robot move forward until it strikes an object (or reaches the edge of a table). If a sensor is activated, the robot should backup briefly, turn in the direction opposite of the sensor (right for the left sensor and vice versa), and continue forward. This behavior results in the robot continuously navigating a room by bumping and turning, bumping and turning.
The program flow requires cooperation between the PC code logic and RCX code logic. This presents an interesting challenge to prevent race conditions where both programs try to access the same resources at the same time. The program flow is as follows: First, the RCX code detects a sensor that has been activated, turns off both motors, and sets its variable 0 (var0) to a unique value for that sensor. This does two things. It causes an asynchronous event to be created by the Spirit control on the PC, notifying the PC code that var0 in the RCX has changed. It then prevents the RCX code logic from turning off the motors when it sees that the sensor is still set. Now it's up to the PC code logic to react to the hit. Upon receiving the event, the PC code knows that both motors are off and knows which sensor has been activated from the value in var0. Using immediate commands, it calls the appropriate methods with reasonable delays to backup and turn the robot away from the obstacle. Before finishing, it sets var0 back to 0, thereby allowing the RCX logic to detect another obstacle.
The reason the motors are turned off by the RCX code is because of the delay between the sensor activation, detection, and subsequent reaction by the PC code. I did not enjoy seeing the Torbot bind up against an immovable object for a half second or so while waiting for the PC code to react. Also, I found that if the Torbot got out of range of the Mindstorms infrared tower (which downloads code from the PC) and hit an object, it would continue to struggle against the obstacle until I came to its rescue. Having the RCX code turn off both motors upon striking an object seemed a logical and more humane design.
VC++ and SPIRIT.OCX
The program I present here was created using Microsoft's Visual C++ 6.0 and MFC. This means that VC++ is installed on the same PC as my LEGO Mindstorms software package (the Spirit.OCX). The first step is to create a project using the VC++ Project Wizard selecting "MFC AppWizard (exe)." On Step 1 of the Wizard, VC++ lets you select a single-document view without using the document view architecture. I didn't need a document in the program because it just contains robot code. The ActiveX Controls checkbox is checked on Step 3 to let the generated program use ActiveX controls. Selecting Finish on the Wizard generates 11 source files containing four classes.
Next, I added a wrapper class for the Spirit control to the project. VC++ automatically creates such a class from a registered ActiveX or OCX control. Under the Project menu, selecting Add to Project followed by Components and Controls shows the Components and Controls Gallery dialog. Double-clicking Registered ActiveX Controls shows a list of all the available controls on the system. The Spirit control is added to the project by selecting the Insert button. If the default selections are used, a CSpirit class is generated. The methods of the class match the commands documented in the Spirit Technical Reference.
To use the control in your program, I recommend you create a dialog. On my system, I have an RCX MFC application to which I add robot program logic. For each new robot challenge, I create a new dialog to encapsulate the unique features and logic that the robot requires. I then add a menu option to the main application to invoke that new dialog. This lets me have one common program to launch any of my robot program creations. Also, any common methods my robot programs might need can be easily used or copied.
For this project, I created a dialog called "Torbot" to hold the PC and RCX code. The dialog contains a Download button to download the RCX code, a Start button to activate the RCX from the PC, a Stop button to stop the RCX, and a Cancel button to end the dialog. Although I can start the RCX program by pressing its green Run button, I have found it slightly annoying to download the program and then turn around, bend over, and press the button. You'll find in debugging your programs that you will be continuously downloading, starting, and stopping the RCX. All my dialogs now have a Start button. The Stop button becomes convenient when bad program logic causes your robot to go awry (and it will) and you just can't seem to get your finger on any of the buttons fast enough.
Next, I added the Spirit control to the dialog. When the CSpirit class was generated and added to the project, the Spirit control also became available on the dialog control palette for this project. The control can be added like any other control by dragging it from the palette and placing it anywhere in the dialog. The LEGO logo appears as a placeholder for the control. The control has no visible functionality, so I usually make it invisible (uncheck the Visible setting in the control's Properties) but there is no reason not to advertise LEGO on your dialog. Finally, I usually include a text field on the dialog to show any error messages that may occur when an RCX program is downloaded or status messages when the program is running. Figure 2 is the completed dialog.
Once the dialog is finished, the ClassWizard generates an MFC class wrapper for the dialog and its controls. I called the class for my dialog "TorbotDlg." I added variables for each of the buttons and controls. I named the variable for the Spirit control m_spirit. Using this same variable name each time lets me easily copy code snippets from dialogs from other robot projects. Next, I used the ClassWizard to add message maps for each button except Cancel. The default dialog behavior for Cancel is fine. These methods are called OnDownload(), OnStart(), and OnStop(); see Figure 3.
I also created a message map for WM_INITDIALOG that creates an OnInitDialog() method. This initializes the Spirit control. Finally, I used the ClassWizard to register for the Spirit event methods for DownloadDone, AsyncronBrickError, and VariableChange. The ClassWizard created methods called:
OnDownloadDoneSpiritctrl1()
OnAsyncronBrickErrorSpiritctrl1()
OnVariableChangeSpiritctrl1()
The first two are used when downloading the RCX program, and the last event is used for monitoring the sensor value changes.
At this point, the program logic can be added. Before the Spirit control command can be used, the InitComm() method must be invoked. This sets up the communication parameters between the PC and the infrared tower. It only needs to be called once, but should be the first method invoked (there are a few exceptions to this rule; see the Spirit Technical Reference for details). If InitComm() fails, the Spirit control methods cannot be invoked. I added a call to this method in OnInitDialog() along with calls to the following two methods that check to see if the infrared tower is connected and working:
m_spirit.TowerAndCableConnected();
m_spirit.TowerAlive();
If any of these methods return False, the logic displays an error message and disables all buttons except Cancel.
RCX Code
The RCX program logic contains two parts. The first part is a downloaded RCX program that monitors the sensor inputs and communicates their state to the PC. The program consists of one task to monitor both sensors. The task runs in an infinite loop checking the sensors inputs. If a sensor is activated, it sets the RCX variable 0 (var0) to a unique value. The left sensor is assigned value 1 and the right sensor is assigned value 2. When neither sensor is active, var0 is set to 0.
The code to generate the RCX program is implemented in the OnDownload() method of the dialog. The sequence of Spirit methods is called to generate the RCX program in Listing One.
First, the RCX program selection is set to program 5 (1) where the generated program (bytecodes) will be downloaded and stored. Task 0 for this program is created by calling the BeginOfTask() method (2). All methods after this call and before the next EndOfTask() call will be interpreted by the Spirit control as RCX program logic and not immediate commands. Notice the use of the brackets. The commands could just as easily be coded on consecutive lines without the brackets or indentations, but I find this style easier to read and therefore easier to debug.
The first thing the RCX program does is set up var0 and the sensors. var0 is set to 0 (3) and the two sensor inputs are set to read switches as Boolean values (4).
Now the main program loop begins. An infinite loop in a task can be created by passing a length of 0 to the Loop() method (5). This only works when passing a constant (SRC_ CONSTANT). Inside the loop, the logic checks for switch 1 on sensor input 1 (6). If the switch is activated, the sensor value will be 1 and logic between the If() and its corresponding EndIf() is executed. Next, I added a check to see if var0 has been already set (7). This is required because the PC code and the RCX code are running on separate processors and a race condition may occur when setting var0 and accessing the outputs (motors). The PC code is responsible for setting var0 to 0 when it has finished processing the sensor value. If var0 is set (that is, not 0) the PC code is still processing the last switch setting. If var0 is 0, the logic inside the If()/EndIf() is executed by the RCX. Here, both motors are turned off (8) and var0 is set to 1 (9), indicating switch 1 has been activated. I also included a slight delay (10) in the logic here to give the Spirit control time to create its event for the PC code. The delay is not technically necessary but seemed the polite thing to do.
The EndIf()s close their respective If() code logic. Similar logic is created to handle the switch on sensor 3 (12). Here, though, var0 is set to 2 when switch 3 is activated. The end of the infinite loop is marked by the call to EndLoop() (13). The task is completed by invoking EndOfTask(), which automatically starts sending the generated bytecodes to the RCX. This is done on an asynchronous thread inside the control so the program is not likely to be finished downloading upon return of EndOfTask().
That's all there is to setting up the RCX code. When the Download button is pressed on the dialog, the code is generated and sent to the RCX. The DownloadDone event method is called after the program bytecode transfer to the RCX (triggered by the EndOfTask() method) is complete. The DownloadDone event method has an error code parameter. If an error occurs, the error code is not 0 and I have the number displayed in the dialog's text field. If no error occurs, I call the Spirit control's PlaySystemSound() method to beep, signifying a successful download: m_spirit.PlaySystemSound(SOUND_ SWEEP_UP);. I used the same system tone that the Mindstorms package uses on a download complete.
PC Code
The PC program logic starts in the OnStart() method of the dialog as in Listing Two. The code here sets the motors into a known state for power and direction (1), creates the Spirit ActiveX event to notify the PC code when var0 has changed (2), starts the RCX program (3), and turns the motors on (4). Because of the orientation of the motors (and their wire attachments), motor A and motor C are run in opposite directions to get the treads to run in the same direction. The coded orientation causes the Torbot to move forward when both motors are turned on at (4).
The Spirit event mechanism actually internally polls an RCX resource (only var0 works right now). The last parameter on SetEvent() tells the Spirit control how often to check var0. I picked 10 milliseconds, which is the lowest possible value (polls most often) to achieve the fastest possible reaction time.
The main part of the PC program logic that turns the robot on and reacts to the sensor values by making the robot backup and turn is coded in the VariableChanged event. This method is called whenever var0 is changed in the RCX. Listing Three shows the VariableChange event code.
The logic is broken up into two parts (using a switch statement), each handling var0 values 1 or 2 appropriately. Both parts backup the Torbot and turn it away from the obstacle before setting var0 back to 0. Each case reverses both motors, turns on both motors, makes the RCX beep, reverses one motor (causing a turn), reverses the second motor (causing it to go forward again), and finally clears var0.
The switch statement is preceded by a call to ClearEvent(), which stops the Spirit control from polling var0. The switch statement is followed by a call to SetEvent(), which restarts the event processing for var0 changes. Without the ClearEvent()/SetEvent(), the VariableChange method is not called if the last value is the same as the previous. This means if I hold down one of the switches (activating it), the RCX program stops the motors and sets var0, causing an event. After the PC code turns the Torbot and sets var0 to 0, it is immediately reset in the RCX code that is monitoring the switch value in a tight loop. This is usually not enough time for the Spirit control to notice the change so an event is not created. The result is that the Torbot stops and will not continue. Using ClearEvent()/SetEvent() in VariableChange forces the event to occur.
Conclusion
The robot's reaction time is slow because it is dependent on the communication and reaction time of the PC and Spirit control. Since the VariableChange event uses a polling technique, the reaction time could never be faster than 10 milliseconds. Therefore, this particular Torbot program could probably be more effectively implemented in RCX code only. However, the program I present shows many facets of programming robot logic cooperatively with PC code and RCX code to achieve a desired behavior.
Others doing RCX programming work outside of the Mindstorms user interface include David Baum, who has developed a C-like language called NotQuiteC (NQC) specifically for creating RCX code (http:// www.enteract.com/~dbaum/lego/nqc/). Finally, two must web sites for Mindstorms developers are Kekoa Proudfoot's RCX Internals (http://graphics.stanford.edu/ ~kekoa/rcx/) and Russell Nelson's Mindstorms Internals (http://www.crynwr.com/ lego-robotics/).
DDJ
Listing One
void TorbotDlg::OnDownload()
{
m_spirit.SelectPrgm( PROGRAM_5 ); // (1) program 5
m_spirit.BeginOfTask( TASK_0 ); // (2) start task generation
{
m_spirit.SetVar( VAR_0, SRC_CONSTANT,0 ); // (3) set var0 to 0
// (4) set sensor 1 to switch
m_spirit.SetSensorType( SENSOR_1, SENSOR_TYPE_SWITCH );
m_spirit.SetSensorMode( SENSOR_1, SENSOR_MODE_BOOLEAN, 0 );
// set sensor 3 to switch
m_spirit.SetSensorType( SENSOR_3, SENSOR_TYPE_SWITCH );
m_spirit.SetSensorMode( SENSOR_3, SENSOR_MODE_BOOLEAN, 0 );
m_spirit.Loop( SRC_CONSTANT,INFINITE_LOOP ); // (5) loop forever
{
// (6) is switch on 1 ?
m_spirit.If( SRC_SENSOR_VALUE, SENSOR_1, OP_EQUAL,
SRC_CONSTANT, 1 );
{
// (7) are we still processing the variable ?
m_spirit.If( SRC_VARIABLE, VAR_0, OP_EQUAL,
SRC_CONSTANT, 0 );
{
// (8) turn off motors
m_spirit.Off( MOTOR_AC );
// (9) set var0 to 1
m_spirit.SetVar( VAR_0, SRC_CONSTANT, 1 );
// (10) slight delay to let event fire
m_spirit.Wait( SRC_CONSTANT, 20 );
}
m_spirit.EndIf(); // (11)
}
m_spirit.EndIf();
// (12) is switch on 3 ?
m_spirit.If( SRC_SENSOR_VALUE, SENSOR_3, OP_EQUAL,
SRC_CONSTANT, 1 );
{
// are we still processing the variable ?
m_spirit.If( SRC_VARIABLE, VAR_0, OP_EQUAL,
SRC_CONSTANT, 0 );
{
// turn off motors
m_spirit.Off( MOTOR_AC );
// set var0 to 2
m_spirit.SetVar( VAR_0, SRC_CONSTANT, 2 );
// slight delay to let event fire
m_spirit.Wait( SRC_CONSTANT, 20 );
}
m_spirit.EndIf();
}
m_spirit.EndIf();
}
m_spirit.EndLoop(); // (13)
}
m_spirit.EndOfTask(); // (14) starts the download
}
Listing Two
void TorbotDlg::OnStart()
{
// (1) start motors in a known state:
// max power, forward direction
m_spirit.Off( MOTOR_AC );
m_spirit.SetPower( MOTOR_AC, SRC_CONSTANT, POWER_FULL );
m_spirit.SetFwd( MOTOR_A ); // these directions make
m_spirit.SetRwd( MOTOR_C ); // both treads run forward
// (2) setup to receive var0 changes every 10ms
m_spirit.SetEvent( SRC_VARIABLE, VAR_0, 10 );
m_spirit.StartTask( TASK_0 ); // (3) turn on task 0
m_spirit.On( MOTOR_AC ); // (4) startup the motors
}
Listing Three
void TorbotDlg::OnVariableChangeSpiritctrl1(short Number, short Value)
{
m_spirit.ClearEvent( SRC_VARIABLE, VAR_0 );
switch( Value )
{
case 1:
m_spirit.AlterDir( MOTOR_AC ); // backup
m_spirit.On( MOTOR_AC );
m_spirit.PlaySystemSound( SOUND_CLICK ); // lil' beep
::Sleep( 300 ); // .3 second delay
m_spirit.AlterDir( MOTOR_A ); // turn right
::Sleep( 300 ); // .3 second delay
m_spirit.AlterDir( MOTOR_C ); // go straight
m_spirit.SetVar( VAR_0, SRC_CONSTANT, 0 ); // reset var0
break;
case 2:
m_spirit.AlterDir( MOTOR_AC ); // backup
m_spirit.On( MOTOR_AC );
m_spirit.PlaySystemSound( SOUND_CLICK ); // lil' beep
::Sleep( 300 ); // .3 second delay
m_spirit.AlterDir( MOTOR_C ); // turn left
::Sleep( 300 ); // .3 second delay
m_spirit.AlterDir( MOTOR_A ); // go straight
m_spirit.SetVar( VAR_0, SRC_CONSTANT, 0 ); // reset var0
break;
}
m_spirit.SetEvent( SRC_VARIABLE, VAR_0, 10 ); // re-enable event
}
Copyright © 1999, Dr. Dobb's Journal
