Languages

Talk to Your Robot
Create Motion with the Forth language

by Even Rosen

Synopsis

This article is the third in a series on robotics and the Atari computer. The first two parts appeared in the December 1983 and January 1984 issues of Antic. The main program listing accompanying this article requires that you have the language Forth. All program listings in this article work on any Atari computer.

Last time (Antic, "Has Your Robot Hugged You Today?", P. 38, January 1984), I told you how to use a BASIC listing and your Atari computer to make a servo-robot dance. If you were able to locate the required hobby servo, battery, two diodes and joystick-port connector, you may have already done so. This time, I'll discuss a roughly equivalent program in Forth and present a short sample program for each of the two languages. We'll also talk about additional axes and sensors.

Because most versions of Forth for the Atari computers are written in the fig (Forth Interest Group) dialect, our listing is in fig-Forth. Assembler mnemonics vary slightly between various Forth implementations, but the replacement of EQ with 0 = should be the only change needed to accommodate your fig-Forth.

Screen 100 defines the constants PORTA and PACTL. The bits of PORTA correspond to pins 1-4 on Ports 1 and 2. PACTL, or "Port A Control," permits the control of these pins for input or output. Since we're sending pulses to a servo, we'll want to make at least one pin, pin 1 on Port 2, an output pin.

PORTSET on line 6 of screen 100 flips a bit in PACTL, programs one bit of PORTA for output, and then flips the same bit in PACTL again to finish the change.

The variables SERVO, OPULSE and TOP, defined in lines 10-12, are parameters that control the behavior of the servo. SERVO contains the one-byte rotational position of the servo. In theory, it ranges from 0 to 255, but in practice (on actual servos) you usually hit the top somewhere in the range of 180-220. The highest useful value of SERVO is contained, after calibration, in TOP. OPULSE is another calibration; it sets the zero-point of the servo's rotational travel.

The word LABEL at the end of screen 100 is defined in order to simplify the creation of the machine-code fragment DRIVER on screen 101.

Forth's Approach to Assembly

If you're familiar with 6502 machine code, but not with Forth assemblers, the code for DRIVER should give you an idea of how Forth approaches assembly. First of all - since Forth's stack invites reverse Polish notation - things like STA PORTA become PORTA STA. Second, you'll find that most mnemonics are followed by commas (i.e., "LDX,"). Because mnemonics such as ADC or DEC also can be legitimate hexadecimal values, Forth 6502 mnemonics are marked with commas so they won't block interpretation of a hex number a programmer is trying to use. There are other approaches, of course, but this one has been used historically with the 6502.

You usually won't find any explicit branch instructions or corresponding labels in Forth assembly code. Forth assemblers tend to enforce structured programming, so smart pseudo-operations such as BEGIN, WHILE, EQ, 0 =, and UNTIL are used instead. In combination, these words cause the necessary branch or jump instructions to be compiled automatically.

DRIVER produces the pulses that drive your servo. Starting on line 5 of screen 101, the byte in OPULSE is loaded into the X register of the 6502. X is incremented to allow for a zero value in OPULSE. We then load the accumulator of the 6502 with 10 (hexadecimal), and store it in PORTA. This sends about five volts to pin 1 of Port 2, which begins sending pulses to the servo.

Next, a delay loop is entered. The loop consists of two NOP, or "no operation;' instructions; a DEX, instruction to decrement the X register; and a test and branch (EQ UNTIL,) back to BEGIN, unless X has been decremented to zero. This loop is the "fixed length" portion of the servo pulse. (Remember that, in order to drive the servo between its endpoints, these pulses must range from a minimum duration of about 1 msec to a maximum of approximately 2 msec.)

Line 9 of screen 101 loads the value in SERVO into the X register, increments the register, and starts another delay loop. This second loop creates the "variable length" portion of the servo pulse, and thus contains positional information. That is the reason that this part of the pulse is proportional to the value in SERVO.

Finally, we load 0 into the accumulator and then store it in PORTA to send the pulse to the servo. The jump to the address at $E463 on line 14 uses the normal vertical blank deferred-exit point.

Moving Forth

If you have a fig-Forth system, you should be able to type in this listing, load it, and run DEMO. DEMO will make your servo oscillate, if the servo is connected according to the instructions in the January issue of Antic. (If you don't have the January issue, the connections were as follows: The black wire on your servo is connected to the negative side of the 6-volt lantern battery and, through a connector, to pin 8 of Port 2. The positive terminal of the battery is connected through two diodes in series, to provide about a one-volt drop, and the second diode is connected to the red (or orange) wire on the servo. The third wire on the servo is attached - through the connector - to pin 1 on Port 2.)

Try DEMO. Exit it by pressing the terminal key on your machine. The location of this key depends on which implementation of Forth you use.

If you store values in SERVO, you can control your servo directly. Set SERVO to 0, and then tweak the value of OPULSE up or down. This will cause your servo to go all the way to one stop without straining against it. Then see how high you can make SERVO before you reach the stop in the other direction, and put this value into TOP. Now, run DEMO again. The servo should oscillate through its full allowable arc at this point.

Gaining Keyboard Control

Another quick program will give us keyboard control of the servo. The Forth program is on screen 103. Load it, and then type KEYBOARD. Next, press and hold the [+] or [*] key. This will cause the servo to move slowly in one direction or the other. Press the [X] key exit.

To use the BASIC listing, enter the BASIC code from the January issue and run it to install the machine-language routine. The new code can be added to the old code and run GOTO 2000. Use the [+] or [*] key (as in Forth) to make your servo move.

In both versions, the slow action is created by the 10-persecond auto-repeat as you hold down the keys.

More Axes?

There are several ways to control more than one axis at a time. But if we stick to standard servos, we'll need at least one wire for each servo. One way to handle this is to wire a servo to each I/O pin of PORTA; this allows us to use up to eight servos - if we write some tricky software that jiggles the bits at the right time.

Another solution is to use a shift-register to take pulses from a single wire and decode them for a number of servos. This involves the use of the count-down interrupt timers on the POKEY chip; otherwise, the 6502 would spend all of its time in delay loops.

The first approach has been coded in Forth, and can control up to eight axes simultaneously. The translation to BASIC is being prepared. I'll cover one or both of these in an upcoming issue. One of the advantages of this technique, which doesn't use POKEY, is that the code can be used on other 6502 machines if a 60-hz interrupt is used. Such an interrupt can be set up on most Apple computers, for instance, with a couple of clips and a wire.

How Does Your Robot Feel?

If you've ever taken a joystick apart, you know that there are five switches inside: one for each of the four directions, and one for the trigger button. The joystick ports sense when these switches are closed.

By substituting other switches, you can add a sense of touch to your rudimentary robot while using very few additional parts. D-9 female connectors are available at a dollar or two apiece, and inexpensive, low-force switches can often be obtained at electronics surplus stores. You can also make your own. All you really need are two pieces of connector that touch when pressed together and separate when the pressure is released.

If you're in BASIC, you can read these switches by means of the BASIC commands STICK and STRIG. From Forth, you may want to simply byte-fetch from location 54017 for pins 1-4 on Ports 3 and 4, and from locations 53266 and 53267 for the respective triggers.

Think, Instead

But putting sensors on a robot is not the hard part at all. The real question is what to do with sensory data once you have it. So when you start to write the software for these switches, do yourself a favor: Don't write, think instead...

Imagine that you are blind and deaf, have no sense of direction, and lack a sense of touch except for a few points on your body. Think about how you would have to use the sensory information you got from those few areas of contact with the outside world.

If you think there isn't very much of interest that you can do with your servo and the software tools that are currently available, you may be right. But think about the kind of software you'd need to even begin writing the ultimate robotics program you envision. Spend a week at it. Or a year. Most of the members of the "artificial intelligence" community have been at it quite a bit longer than that, but each of them had to begin somewhere. You're at the starting line now.

Evan Rosen is the co-author of Val-FORTH from Valpar International.

2000 POKE SERVO,TOP/2
2010 TEMP=PEEK(SERVO)
2020 K=PEEK(764):POKE 764,255
2030 IF K=6 THEN TEMP=TEMP+1
2040 IF K=7 THEN TEMP=TEMP-1
2050 IF TEMP<0 OR TEMP>TOP THEN GOTO 2
010
2060 POKE SERVO,TEMP
2070 IF K<>22 THEN GOTO 2010

Scr # 100
 0 ( Port setup and variables )
 1 DECIMAL
 2
 3 54016 CONSTANT PORTA
 4 54018 CONSTANT PACTL
 5
 6 : PORTSET ( -- )
 7   PACTL C@ DUP 4 - PACTL C!
 8   16 PORTA C! PACTL C! ;
 9
10 128 VARIABLE SERVO
11 120 VARIABLE OPULSE
12 150 VARIABLE TOP
13
14 :  LABEL 0 VARIABLE, -2 ALLOT;
15

Scr # 101
 0 ( Driver routine )
 1 HEX ASSEMBLER
 2 100 DP C@ - ALLOT ( PAGE BNDRY )
 3
 4 LABEL DRIVER ( -- )
 5   OPULSE LDX, INX,
 6   10 # LDA, PORTA STA,
 7   BEGIN, NOP, NOP, DEX, EQ
 8   UNTIL, ( END FIXED LENGTH )
 9   SERVO LDX, INX,
10   BEGIN, NOP, NOP, NOP, NOP,
11    DEX , EQ
12   UNTIL,  ( END VARIABLE LENGTH )
13   0 # LDA, PORTA STA,
14   E463 @ JMP, ( EXIT VBLANK )
15

Scr # 102
 0 ( Oscillating Demo )
 1 DECIMAL
 2
 3 : DEMO (--)
 4   PORTSET
 5   0 54286 C! DRIVER 548 !
 6   64 54286 C! ( INSTALL VBI RTN )
 7    BEGIN TOP @ 0
 8    DO I SERVO C!
 9     100 0 DO LOOP ( DELAY )
10    LOOP
11    0 TOP @
12    DO I SERVO C!
13     100 0 DO LOOP ( DELAY )
14    -1 +LOOP ?TERMINAL
15     UNTIL ;

Scr # 103
 0 ( Keyboard control demo )
 1
 2 : KEYBOARD ( -- )
 3   TOP @ 2 / SERVO !
 4   BEGIN
 5    764 C@ > R 255 764 C!
 6    SERVO C@
 7    R 6 =
 8    IF 1 +
 9    ELSE R 7 =
10     IF 1 -
11     ENDIF
12    ENDIF 0 MAX TOP @ MIN
13     SERV0 !
14    R > 22 =
15   UNTIL ;

Listing 1: ROBOT.4TH Download / View

Listing 2: ROBOT.BAS Download