The Basics - Robot Software
 |
Overview
One of the least discussed topics in hobby level robotics is software. You can almost always find a hardware description, schematic, drawing, and tons of help on the electro-mechanical side of your project. However, the software, which admittedly is one of the hard parts, eludes many people. In this article, I would like to demonstrate how to structure your software so your robot will function properly.
I would also like to point out a great technique for getting started. Many software engineers make heavy use of something called 'Pseudo Code'. This is a technique where you write your program in half programming language and half human language (English, Spanish, Driud, etc). Pseudo code is very useful for getting the layout of your program ready, to get basic algorithms worked out, and to be able to read your 'code' quickly. I will demonstrate this technique a little later.
As for programming languages, it doesn't matter which language you choose to use. I personally use the 'C' programming language the most. BASIC, Assembler, or FORTH are all fine choices. It turns out that the language is a small part of the structure of your program. The software has a job to do, and the key is to get it done! I am going to write this article in SBASIC, but the techniques are the same no matter which language you choose. SBASIC is available from Karl Lunts website at http://www.seanet.com/~karllunt/tips.htm
Basic Structure
Most robot programs have a similar structure. It consists of 4 major parts.
- Declarations and Variables
- Subroutines and Functions
- Initialization
- The Main Loop
Each of these sections have common attributes that I will discuss along the way.
Declarations and Variables
The first part in most programs is used to include files that you need, to declare variables that you are going to use, and to define constants for use in your program. You should also put a comment at the top of the file that indicates what the program does. You will find that lots of robot programs look alike, so having comments is an important way to tell them apart. In SBASIC, comments are done using the tick mark. Everything from the start of the ' mark to the end of the line is a comment, and will be ignored by the compiler.
A typical declaration section looks something like this:
'*************************************************************** ' Declaration and Variables Section '*************************************************************** include "regs11.lib" declare SensorInput ' Variable that holds input sensor data declare BehaveState ' Variable that holds the previous behavior declare TurnDelayCount ' Variable for timing turns ' ' I like to name constants relative to their function. For example ' the following constants are used with the SensorInput variable, ' so I put the variable name in the constant name. This makes it ' easier to find functions that are affecting and affected by ' the value or the sensor. I also relate the function of the ' value in the name, as well as the group. For example, you ' can see which bits belong to the sensor variable, which ' are bumper related, and what their functions are. This is a good ' way to keep track of things. const Sensor_Port = portc const Sensor_Port_DDR = ddrc const Sensor_Bumper_Left = %00000001 const Sensor_Bumper_Right = %00000010 const Sensor_Bumper_Back = %00000100 const Sensor_Bumper_Mask = %00000111 const Sensor_Mask = %00000111 const Sensor_DDRC_Value = Sensor_Mask XOR $FF const Motor_Port = portb const Motors_Forward = %10100000 const Motors_Backward = %01010000 const Motors_Left = %10010000 const Motors_Right = %01100000 const Motors_Stop = %00000000 const Behave_Forward = 1 const Behave_Back_Left = 2 const Behave_Back_Right = 3 const Behave_Turn_Left = 4 const Behave_Turn_Right = 5 const Behave_Stop = 6
const Turn_Delay_Counter_Init = 300
There are a few things to point out in the file above. The "include" statement causes the contents of another file to be inserted at that point. This is how you can reuse parts of code from other program files.
Then three variables are declared. In SBASIC, variables are 16 bits long by default. Variables can be used by name in other parts of the program.
The next part defines a set of constants. Constant values are used to assign symbolic names to values. Rather than having to remember that the left bumper sensor is connected to pin 1 on PORTC, we define a name called Sensor_Bumper_Left which holds the bit value.
I find that defining constants works best when you incorporate their use or associated variable name into the name of the constant. Otherwise, you can generate bugs in your code by using a similar sounding name or value on your variable.
Subroutines and Functions
Writing a single long line of code can be tedious, and can also increase your chances of coding in a bug. It is often best to create subroutines for handling blocks of code. If you code things up properly, you can also reuse code quite easily. The bulk of your software is going to exist in subroutines and functions. Most subroutines are very short, and consist of a well modularized bit of code.
The most common way to read through these subroutines is to find the Main entry point (next section), and to read through what code gets executed.
'***************************************************************
' Subroutines and Functions section
'***************************************************************
'
' Move_Forward sets both drive motors into forward motion
' Again, I like to categorize functions into groups, and
' make them explicit in the name.
'
Motors_Move_Forward:
pokeb Motor_Port, Motors_Forward
return
Motors_Move_Backward:
pokeb Motor_Port, Motors_Backward
return
Motors_Turn_Left:
pokeb Motor_Port, Motors_Left
return
Motors_Turn_Right:
pokeb Motor_Port, Motors_Right
return
Motors_Move_Stop:
pokeb Motor_Port, Motors_Stop
return
StartTurnDelay:
TurnDelayCount = Turn_Delay_Counter_Init
return
StopTurnDelay:
TurnDelayCount = 0;
return
UpdateTurnDelay:
if TurnDelayCount <> 0
TurnDelayCount = TurnDelayCount - 1
endif
return
'
' Initialize_Robot is called once when the robot starts. It will
' put the robot into a known state.
'
Initialize_Robot:
'
' Setup the serial port for 9600 baud
'
pokeb baud, $30
pokeb sccr2, $0c
' Set the data direction register for the sensor inputs
pokeb Sensor_Port_DDR, Sensor_DDRC_Value
' Set the motors to the off position
gosub Motors_Move_Stop
TurnDelayCount = 0
BehaveState = Behave_Forward
SensorInput = 0
return
'
' Read sensors and put the values into a variable.
' This routine includes any 'processing' that needs
' to be done to the sensor data
'
Sensor_Read:
' Read the sensor values
SensorInput = peekb(Sensor_Port)
' Mask off any output pins
SensorInput = SensorInput AND Sensor_Mask
' Input switches are active low. Flip the sense of
' the bits so they are active high in the variable
' This allows them to be tested correctly in the
' rest of the program
SensorInput = SensorInput XOR Sensor_Bumper_Mask
return
'
' Behave is the routine that determines the robots behaviour. This
' routine will evaluate the sensors, determine the correct action,
' and make the robot do what it is supposed to
'
Behave:
if (SensorInput AND Sensor_Bumper_Left) <> 0
' We are hitting something on the front left.
' Back off of it
gosub Motors_Move_Backward
gosub StopTurnDelay
BehaveState = Behave_Back_Left
elseif (SensorInput AND Sensor_Bumper_Right) <> 0
' We are hitting something on the front right.
' Back off of it
gosub Motors_Move_Backward
gosub StopTurnDelay
BehaveState = Behave_Back_Right
elseif (SensorInput AND Sensor_Bumper_Back) <> 0
' We just backed into something, go forward
gosub Motors_Move_Forward
gosub StopTurnDelay
BehaveState = Behave_Forward
else
' The bumper switches are clear. Therefore, we
' set our state based on the behaviour information
select (BehaveState)
case Behave_Back_Left
' We were backing because of something on left
' Turn to the right
gosub Motors_Turn_Right
BehaveState = Behave_Turn_Right
gosub StartTurnDelay
endcase
case Behave_Back_Right
' We were backing because of something on right
' Turn to the left
gosub Motors_Turn_Left
BehaveState = Behave_Turn_Left
gosub StartTurnDelay
endcase
'
' The default action for the case statement.
'
' If the TurnDelayCount is Zero, then we can
' go forward. Otherwise, we will continue to
' turn in the TurnDelayCount direction.
if TurnDelayCount = 0
gosub Motors_Move_Forward
BehaveState = Behave_Forward
endif
endselect
endif
return
Initialization
All programs have a starting point. In SBASIC, the name of the subroutine that is called initially is Main:
The Main routine is called when the controller resets or is powered up. Its job is to initialize variables, setup ports, and get the robot into a known state. I usually like to get the initialization done, then print a message to either the serial port or to an LCD display if one exists. This lets you know that the program has started, and also lets you see if the CPU resets for some reason.
'***************************************************************
'
' Main is the starting point for the program. It will handle calling
' the appropriate initialization routine, print out an indication that
' it is alive, then go into the Infinite Loop
'
'***************************************************************
Main:
' Initialize the robot first thing
gosub Initialize_Robot
' Give some indication that the robot is starting.
print "Robot program has started"
'
' Being the infinite loop. Notice there is no way out of this loop
'
do
gosub Sensor_Read
gosub Behave
'
' Do any housekeeping chores
'
gosub UpdateTurnDelay
loop
'
' Code after the final loop should never get executed, and therefore
' shouldn't exist. If you are paranoid, then you should put in some
' code to stop the robot in its tracks, and shutdown.
'
end
The Main Loop
All computers systems end up with some sort of Main Loop. On your PC, there is a routine in the operating system called the Idle loop. This idle loop sits and waits for timers, disks, user input, and a host of other things. Robots also have a Main Loop. In the code sample above, you can see it labeled as the infinite loop. Its called that because it never ends. It calls Sensor_Read, Behave, UpdateTurnDelay, then goes back to Sensor_Read.
If you write a program on your PC and run it, the Main Loop (aka Idle Loop) will call your program to let it run. Your program will often have an 'end', 'exit', or 'return' statement at the end. This allows your program to return control to the Idle Loop.
On your robot, there is no Idle Loop to return control to. Therefore, you shouldn't have a return statement in your Main: routine.
Psuedo Code
A great trick used by professional software engineers is to start writing code by writing the comments first. This helps you get your program layout done before you invest too much time in getting all of the nitty-gritty details down. In Psuedo Code, you end up doing a layout for all of the routines, put in lots of comments as notes to yourself about what your program is about to do, and sometimes even add in little snippets of code that might explain your intent better than words. When you are done, you will have a skeleton outline for your program.
Quite often, you will find that you have overlooked some important point on your first stab. Thats OK. Since you haven't invested a bunch of time in writing the actual code, whacking out a large chunk of this Psuedo code is not a big deal.
Here, as example, is the Psuedo Code I did for the code in this article.
' ' A basic little robot program. ' '*************************************************************** ' Declaration and Variables Section '*************************************************************** include "regs11.lib"
declare SensorInput ' Variable that holds input sensor data declare BehaveState ' Variable that holds the previous behavior declare TurnDelayCount ' Variable for timing turns
'*************************************************************** ' Subroutines and Functions section '***************************************************************
'
' Move_Forward sets both drive motors into forward motion
' Again, I like to categorize functions into groups, and
' make them explicit in the name.
'
Motors_Move_Forward:
return
Motors_Move_Backward:
return
Motors_Turn_Left:
return
Motors_Turn_Right:
return
Motors_Move_Stop:
return
StartTurnDelay: return StopTurnDelay: return UpdateTurnDelay: ' If the turncountdelay is not zero, then decrement it ' by one.
return
'
' Initialize_Robot is called once when the robot starts. It will
' put the robot into a known state.
'
Initialize_Robot:
'
' Setup the serial port for 9600 baud
'
' Set the data direction register for the sensor inputs
' Set the motors to the off position
gosub Motors_Move_Stop
' Initialize all global variables
return
' ' Read sensors and put the values into a variable. ' This routine includes any 'processing' that needs ' to be done to the sensor data ' Sensor_Read:
' Read the sensor values
' Mask off any output pins
' Input switches are active low. Flip the sense of ' the bits so they are active high in the variable ' This allows them to be tested correctly in the ' rest of the program
return
' ' Behave is the routine that determines the robots behaviour. This ' routine will evaluate the sensors, determine the correct action, ' and make the robot do what it is supposed to ' Behave:
' if Sensors detect hit on front left, back off and turn right
' else if sensors detect hit front right, back and turn left
' else if sensors detect hit on back, go forward
' Otherwise, we have no current sensor input to deal with.
' The behaviour variables are evaluated. If last time through
' we were backing up, then we must be clear to make our turn.
'
' Based on the behave state, turn as appropriate. If a turn
' is initiated, set a counter variable to keep track of how
' long we have been turning (TurnDelayCount)
' Otherwise, if we are currently turning, check to see if
' we have turned far enough by checking the TurnDelayCount
'
'
' If the TurnDelayCount is Zero, then we can
' go forward. Otherwise, we will continue to
' turn in the TurnDelayCount direction.
if TurnDelayCount = 0
gosub Motors_Move_Forward
BehaveState = Behave_Forward
return
'*************************************************************** ' ' Main is the starting point for the program. It will handle calling ' the appropriate initialization routine, print out an indication that ' it is alive, then go into the Infinite Loop ' '*************************************************************** Main:
' Initialize the robot first thing
gosub Initialize_Robot
' Give some indication that the robot is starting.
print "Robot program has started"
'
' Being the infinite loop. Notice there is no way out of this loop
'
do
' Read Sensors
gosub Sensor_Read
' Do behaviour gosub Behave
' ' Do any housekeeping chores ' gosub UpdateTurnDelay
loop
'
' Code after the final loop should never get executed, and therefore
' shouldn't exist.
'
end
Tricks of the trade
Here is a little random collection of notes:
1) Always read your sensory data into variables in memory, THEN do ALL of your processing using these stored values. If you continually read raw sensor data, you may find your program doing random things based on electrical glitches, or subtle changes in the state of your program.
2) Try to keep some sort of state variable about what your robot thinks it is doing at the moment. It will make debugging much easier. You can print out this state variable in your Main Loop to help track what the program has decided to do on each iteration.
3) Be generous with your internal comments. Make ample notes to yourself why you are doing certain things. This helps you in two ways. By explaining it in comments, it helps clairify in your mind what you are doing. They can be a great design/debug aid as you are forced to explain in the third person what the program is supposed to do. Second, when you need to come back to look at your code, sometimes these notes are invaluable.
Summary
I hope you found this article to be useful. The key points I hope you see are that programs have structure, that subroutines are used to keep from having a single long mass of code, and that the controller program should have a Main Loop. You also should keep in mind the tip about reading all of your sensor data in one chunk, and using memory based variables to do your processing.
A text file with the complete BASIC code is in robot.bas
What else do you want to know? Send me mail and ask away. Kevin Ross