Build Yourself a Bluetooth Controlled Six-Legged Robot

Introduction

I don't know why you can't easily find toy robots that can be controlled by Bluetooth. But that lack can be remedied by a few hours work. So you can see exactly what I mean, here is the robot I'm going to describe being controlled by me via a program running on a Mac:

As it's controlled from a much more powerful computer you can use sophisticated algorithms to direct it. And if you don't want to do that, it can easily be programmed to run autonomously. Either way, you can make use of a pair of infrared proximity sensors as input to your algorithms.

The first thing I must do is give credit where it's due. This is a modification of someone else's design. In fact, it's a sample project at Pololu Electronics and Robotics modified by addition of an off-the-shelf Bluetooth board.

You'll need:

1. Everything required to build the Pololu sample project. That includes Windows. I was developing on a Mac so I made use of VMWare. I can't vouch for any other Windows emulation because you'll need a USB port and I've experienced troubles with emulation and USB in the past. You'll only need Window for initial configuration of the robot. After that you can use the development platform of your choice. The list of parts is here. Order the partial kit for the motor controller. You'll need the header pins it comes with.
2. One BlueSMiRF Gold Bluetooth modem from Sparkfun.

The first step is to build the Pololu project exactly as described. At this point, you'll have a complete robot that can run autonomously. You can program it from Windows using Pololu's scripting language described in the manual.

Note that the battery used is this. Its connector is different to that described in the project. You simply need to solder a pair of pins rather than a special type of connector:

By the way, there is a minor flaw in Pololu's design. The sensor and servos are glued to the battery using hot glue. When the battery is charged it heats. You can guess the rest. So once everything works, you may want to switch to a different glue. I used epoxy.

The strands of wire from the servos are very fine. At one point I think some came astray causing a short-circuit. I was surprised the controller board still worked after I saw smoke rise up from it. So after that I used some dabs of hot glue to act as insulation around my soldering.

I found that I couldn't use the motor controller via an external USB hub, I always had to use a cable directly from my Mac.

In the process of making the robot you cut the connectors off the servos. Keep two of these with 2 or 3 inches of wire attached. We'll reuse them later.

Setting up Bluetooth

In its default mode, the robot is scriptable and controllable via the USB port. However, two pins on the board are attached to a serial port (running at 5V, not the usual RS232 voltage). These can be connected directly to a pair of pins on the BlueSMiRF board.

First we'll get the modem working independently of the robot. So go ahead and solder 6 header pins to the board like so:

Now we can connect it directly to the robot's battery using its connector to make the following circuit:

Conveniently, VCC and GND are next to each other, just like in the battery connector. The little red LED marked Stat should start flashing. This means it's ready to go. Take care not to reverse the polarity (this isn't Star Trek) or make an off by one pin error.

Now go ahead and pair it with your main computer. On the Mac it's the usual process. It'll probably appear as a device called Firefly-something. There have been various revisions of documentation for BlueSMiRFs. I guessed, correctly, that the correct passkey was 1234.

Once it's connected you should get a new serial port. Mine appeared as /dev/tty.FireFly-DB29-SPP. SPP is Serial Port Profile. Now you need a terminal application that can talk to a serial port. On Unix machines you can use screen followed by the path to the dev file. You'll find help on the web for other OSes. On Windows I expect you'll get a new COM port. It didn't seem to matter what baud rate I chose at this stage. I guess that SPP presents an interface that looks like a serial port but that the connection rate doesn't really mean anything. But I may be wrong and you may need to experiment and/or read documentation. It may take several attempt to successfully connect. (It likes to prove it has the power to say no if it wants.) When you do, you'll see the Conn LED light up green.

If you connect within one minute of powering up the BlueSMiRF you can configure it by entering its command mode. One revision of the documentation says you should type +++. My version used $$$. You won't see your keys echoed on the screen but it should reply CMD. Now type d and return to get a summary of its status. Now type SU,19200 (and return) to set the baud rate on the serial port side. It'll go much faster than this but it's worth being cautious to start with. It acknowledges this but it doesn't actually change speed until you restart the device (eg. by power cycling it). Quit command mode by typing ---. At this point, anything you type in the terminal will come out at 19200 baud on the TX pin. Quit screen by hitting ^A^X.

I'm not sure how you can easily test this short of completing the robot. I tested it using a program on a MSP430 Launchpad. I wish I had a fancy diagnostic device to read serial transmissions. I ought to build one.

By the way, talking to the BlueSMiRF as if it's a serial port is a form of backward compatibility. It should be possible to talk to it directly via the RFCOMM API. Among other things, this would allow better diagnostics.

Configuring the Robot

Now you need to configure the robot to use 19200 baud on its TX/RX pins. Connect via USB to the Windows Control Center application (like you've done before if you built the original design) and set it to use the UART at 19200 baud. Make sure CRC is off. Like so:

Once that is done you don't need Windows again unless you change the baud rate.

Connecting Modem to Robot

Now you need to connect the robot to the modem. You have 6 pins on the modem board so you need a 6 pin connector. I glued together the two three pin connectors I mentioned above to make a connector like this:  

The two outer wires can be soldered together because we're not using flow control, and so the BlueSMiRF is clear to send (CTS) whenever it is ready to send (RTS), The ends of the other four wires can be soldered directly to the motor controller board to make this circuit:

Now comes the iffy bit. I hot glued the BlueSMiRF board directly to the side of the servos like this:

That's iffy because it places electric motors directly next to a device using RF. Motors emit lots of RF noise. But they can't be that bad, after all servos have been used for years on radio controlled models, and Bluetooth probably has some error correction. It seemed to work for me. (But see note below.)

Talking to the Robot

Now we need to talk to the robot. Just about every development platform has a way of talking to serial ports. I used Haskell with the serialport package. As I had fixed the baud rate, I used the Pololu compact protocol. As the focus in this article is on hardware, I'll just give the code at the end. Once it's running you should be able to control the robot using

• f - take a step forward
• b - take a step back
• l - turn left
• r - turn right
• e - read the state of the two proximity sensors

Now you're free to code up whatever you want. I experimented with localization using a method I learnt from Eric Kidd.

Notes

I occasionally had dropped bytes. I haven't yet worked out a way to 100% reliably recover when this happens and I'm not sure if it's due to the RF noise I mentioned above. You can probably mess with the timeout parameter in the Pololu Control Center to ensure the robot returns to a known state if communications cease for a bit.

The Apple SPP driver is pretty crappy. It generally works (apart from the annoying need to make multiple attempts to connect), but if you interrupt your code at the wrong time with ^C (say) you can find it locks up so hard that you end up with an unkillable zombie process. The OS didn't even seem able to kill it on shutdown (maybe it would have eventually timed out) and I had to power down the Mac if I wanted to use the robot again. It only happened a couple of times in two weeks of intensive use, but it's annoying.

And if the robot stops working - remember these batteries are pretty small and are doing a lot, so you may just need a recharge.

Code

The code uses two threads, a server that talks to the serial port and a client for users. This means that independent threads can control the legs and eyes, say, without the serial port transactions becoming interleaved. This isn't very polished but should be good enough to start experimentation. I've only tested it under Mac OS X but it looks platform independent to me.

> module Robot where

> import Prelude hiding (Left, Right)
> import Control.Concurrent
> import Control.Monad.Trans
> import Control.Monad.Trans.Maybe
> import Control.Monad.Trans.Reader
> import Control.Exception
> import System.Hardware.Serialport


Server side using Pololu compact protocol.


> sendByte :: Int -> SerialPort -> IO ()
> sendByte b s = sendChar s (toEnum b)


> getByte :: SerialPort -> IO (Maybe Int)
> getByte s = do
>     b <- recvChar s
>     return $ fmap fromEnum b


> setTarget :: Int -> Int -> SerialPort -> IO ()
> setTarget channel target port = do
>     sendByte 0x84 port
>     sendByte channel port
>     let (b1, b0) = target `divMod` 0x80
>     sendByte b0 port
>     sendByte b1 port


> getPosition channel port = runMaybeT $ do
>     liftIO $ sendByte 0x90 port
>     liftIO $ sendByte channel port
>     lo <- MaybeT $ liftIO $ getByte port
>     hi <- MaybeT $ liftIO $ getByte port
>     return $ lo + 0x100*hi


> data SerialCommand = SetTarget Int Int
>                    | GetPosition Int (Maybe Int -> IO ())
>                    | End


> serialExec command port = 
>     case command of
>         SetTarget channel target -> setTarget channel target port
>         GetPosition channel continuation ->
>              getPosition channel port >>= continuation
>         otherwise -> return ()


> serialThread commandChannel port = do
>     command <- liftIO $ readChan commandChannel
>     serialExec command port
>     case command of
>         End -> do
>             closeSerial port
>             return ()
>         otherwise -> serialThread commandChannel port

Client side

> data Servo = Servo { channel :: Int, loLimit :: Int, hiLimit :: Int }


> data Direction = Left | Right | Forward | Back


> start tty = try (openSerial tty defaultSerialSettings
>        { baudRate = B19200 }) >>=
>     either
>         (\ex -> do
>             print (ex :: IOException)
>             threadDelay 250000
>             start tty)
>         (\port -> do
>             commandChannel <- newChan
>             forkIO $ serialThread commandChannel port
>             return commandChannel)


> type RobotM a = ReaderT (Chan SerialCommand) IO a


> writeChan' = flip writeChan


> end = ReaderT $ writeChan' End


> setServo limit servo = ReaderT $ writeChan' $
>    SetTarget (channel servo) (limit servo)


> setLo = setServo loLimit
> setHi = setServo hiLimit


> readEye channel cont = ReaderT $ writeChan' $ GetPosition channel cont


> delay = 100*1000


> raise Left    = setLo midLegs
> raise Right   = setHi midLegs
> forward Left  = setLo leftLegs
> forward Right = setHi rightLegs
> back Left     = setHi leftLegs
> back Right    = setLo rightLegs


> opposite Left    = Right
> opposite Right   = Left
> opposite Forward = Back
> opposite Back    = Forward


> move Forward = forward
> move Back    = back


> halfCycle side direction0 direction1 = do
>     raise side
>     liftIO $ threadDelay delay


>     move direction0 Left
>     move direction1 Right
>     liftIO $ threadDelay delay


> motion direction0 direction1 = do
>     halfCycle Left  direction0 direction1
>     halfCycle Right (opposite direction0) (opposite direction1)


> walkCycle    = motion Forward Back
> reverseCycle = motion Back Forward
> turn Right   = motion Forward Forward
> turn Left    = motion Back Back


> withRobot tty cmds = do
>     commandChannel <- start tty
>     flip runReaderT commandChannel (cmds >> end)


> eyes = do
>     readEye 4 $ print . (("Left eye  = " ++) . show . fmap (<512))
>     readEye 3 $ print . (("Right eye = " ++) . show . fmap (<512))


> commandLoop = do
>     key <- liftIO $ getChar
>     case key of
>         'f' -> walkCycle
>         'b' -> reverseCycle
>         'l' -> turn Left
>         'r' -> turn Right
>         'e' -> eyes
>         otherwise -> return ()
>     if key=='q'
>         then return ()
>         else commandLoop 


> testRobot = do
>     withRobot tty $ do
>         liftIO $ print "Ready"
>         commandLoop


Here's some stuff for you to configure. In particular, set the limits of the servos so that the legs don't whack into the body of the robot, stripping the gears. These numbers will be a little different depending on exactly how you built the robot. You may want to start with the upper and lower limits closer to 6000, the middle of the range of the servo motion.


> tty = "/dev/tty.FireFly-DB29-SPP"


> rightLegs = Servo 0 5000 6500
> midLegs   = Servo 1 5000 6500
> leftLegs  = Servo 2 5000 6500


One Last Thing
Double check everything I say above. I could have easily made a mistake. In particular, make sure that I haven't inadvertently introduced short circuits by comparing my diagrams against the online documentation for the parts.

---

Generalising Gödel's Theorem with Multiple Worlds. Part III.

Fixed Points
A number of branches of mathematics have some sort of implicit function theorem. These guarantee that we can define functions implicitly, rather than explicitly. For example, we can define the function f from the positive reals to the positive reals by the relation f(x)^2 = x. In this case we can write an explicit formula, f(x) = x^1/2, but the implicit function theorem gives quite general conditions when the implicit equation defines a function uniquely, even when it is very hard to write an explicit formula. We have something similar in the theory of datatypes. We define the list type implicitly in terms of lists: L(X) = 1+X L(X). There is a unique smallest type (and unique largest type) that satisfies this equation and we use such equations in Haskell programs to uniquely specify our types. (Haskell uses the largest type.)

Provability logic also has a kind of implicit function theorem. Consider the following function:

> f0 p = Neg (Box p)

Suppose the equation p <-> f0 p had a solution. Then we would have a proposition that asserts its own unprovability. In fact, we know such a proposition: the Godel sentence from Godel's first incompleteness theorem. However, the Godel sentence uses a clever Quining technique to make a proposition that makes assertions about its own Godel number. We can't express such a thing in the language of provability logic. However, consider Godel's second incompleteness theorem. This tells us that if PA can prove its consistency then it is in fact inconsistent. Or to turn it around, if PA is consistent, then PA can't prove its consistency. Asserting that PA is consistent is the same as ◊⊤ (because asserting that ⊤ is consistent with the rest of PA is the same as saying the rest of PA is consistent). So Godel's second incompleteness theorem can be written in provability logic as ¬◻(◊⊤)→◊⊤. Using the methods of part one we can show that ◊⊤→¬◻(◊⊤) is also valid. So Dia T is the solution to p <-> f0 p:

> test0 = valid $ let p = Dia T in p <-> f0 p

Considered as a data structure, notice how f0 places its argument inside a Box. Consider just the functions Prop -> Prop that (1) simply copy p into various places in its return value (ie. that don't analyse p in any way) and that (2) put all copies of its arguments somewhere inside a Box (possibly in a deeply nested way). We'll call these 'modalising' functions. Then there is an amazing result due to Solovay:

The Fixed Point Theorem
If f is modalizing then it has a unique fixed point in provability logic. (Unique in the sense that if p and q are fixed points for f, then p <-> q is valid.

Our goal in the next post in this series will be to implement an algorithm to find these fixed points.

Think about what this means. We can make up any old function that has this property and find a corresponding Godel-like theorem. For example, pick

> f1 p = Box p --> Box (Neg p)

This has a fixed point p = Box (Box F) -> Box F which we can test with

> test1 = valid $ let p = Box (Box F) --> Box F in p <-> f1 p

Or in English (quoting verbatim from Boolos) "a sentence is equivalent to the assertion that it is disprovable-if-provable if and only if it is equivalent to the assertion that arithmetic is inconsistent if the inconsistency of arithmetic is provable". (That's a good argument for using provability logic instead of English!) These are the generalisations of Godel's theorem in my title.

But there are many other important consequences. The solution to this fixed point equation doesn't involve any self-reference (because we can't do self-reference in provability logic as we have no way to talk about Godel numbers). Self-reference is less essential than you might think - we can solve these equations without using it.

At first the fixed point theorem seems like a positive thing: we can crank out as many of these theorems as we like. But there's also a flip side: it says that once we've proved Lob's theorem, there aren't any more techniques we need in order to construct fixed points. So there aren't any funky new variations on what Godel did, waiting to be discovered, that will give us a bunch of new fixed points. We have them all already.

In order to find these fixed points I'm going to use the runTableau function we wrote last time. But that's for the next article in the series. In this post I want to put it to a simpler use:

Automatically Drawing Tableau Diagrams
First we'll need a very simple library of functions for drawing ASCII art diagrams. We'll represent a drawing simply as a list of ASCII strings, one for each row. Think of diagrams as forming a box. The width of a box is the length of its longest row:

> width box = foldr max 0 (map length box)

Sometimes we'll want to pad the length of the rows so that they all have the same length:

> pad len b a = a ++ replicate (len - length a) b

The aside function allows us to 'typeset' one box next to another:

> aside a b = let w = width a
>                 h = max (length a) (length b)
>                 a' = pad h [] a
>                 b' = pad h [] b
>                 a'' = map (pad w ' ') a'
>             in zipWith (++) a'' b'

We can draw nice frames around our boxes:

> frame a = let w = width a
>               h = length a
>               strut = replicate h "|"
>               rule = "+" ++ replicate w '-' ++ "+"
>           in [rule] ++ strut `aside` a `aside` strut ++ [rule]

And the rule for disjunctions requires us to draw side by side boxes with a vertical line between them:

> alt a b = let h = max (length a) (length b)
>               strut = replicate h " | "
>           in a `aside` strut `aside` b

Generating diagrams is now a matter of generating a diagram for each of the 'hooks' in our algorithm:

> diagramRules = TableauRules {

We'll modify the rules so that instead of simply returning a Bool to indicate closure we return a pair. The first element is a list of strings representing the rows of the ASCII art, but the second element plays the same role as before:

>     closes             = snd,

These are the functions that handle discovery of a contradiction. They draw a "X" in a circle to indicate this:

>     foundF             = \_ -> (["(X)"], True),
>     foundContradiction = \_ -> (["(X)"], True),

>     open               = \_ -> ([], False),

We deal with conjunctions simply by listing the two subpropositions that went into them:

>     conjRule           = \a b -> first ([show a, show b] ++),

For disjunctions we draw the diagrams:

>     disjRule           = \p a b (tl, lb) (tr, rb) ->
>                             (([show a] ++ tl) `alt` ([show b] ++ tr), lb && rb),

>     doubleNegation     = \q -> first ([show q] ++),
>     combineWorlds      = \(t0, b0) (t1, b1) -> (t0 ++ t1, b0 || b1),
>     processWorld       = \_ t -> t,

When we create a box for a new world we 'import' some propositions from the parent world. We mark these with an asterisk:

>     tableau            = \br (t, b) -> (frame (map (("* " ++) . show) br ++ map ("  "++) t), b)
> }

> diagram p = do
>     let (t, b) = runTableau diagramRules [Neg p]
>     mapM_ putStrLn t
>     print b

Now we can prove that we really do have a fixed point of f1:

> diagram1 = diagram $ let p = Box (Box F) --> Box F in p <-> f1 p

You may want to pick a really small font and stretch your terminal wide before you run that:

See how every box ends in an (X), just as we want.

note
To run the code above, just append this article to the previous one to make a single literate Haskell program.