I’ve been having some fun playing with the Kinect SDK and the Lego NXT kit. The protocol to talk to the Lego brick over Bluetooth is pretty straight forward. Below is a little F# module for most of the basic commands. I’ll fill out the full set soon and put it up on GitHub.

Using this along with Kinect skeletal tracking makes for a quick, pretty cool little project with the boys!

     

The code is just:

open LegoBot
open Microsoft.Research.Kinect.Nui

printfn "Connecting..."
let bot = new LegoBot "COM3"

printfn "Initializing Kinect..."
if Runtime.Kinects.Count = 0 then failwith "Kinect missing"
let kinect = Runtime.Kinects.Item 0
kinect.Initialize(RuntimeOptions.UseDepth ||| RuntimeOptions.UseSkeletalTracking)
kinect.SkeletonEngine.IsEnabled <- true
kinect.SkeletonEngine.TransformSmooth <- true
kinect.SkeletonFrameReady.Add(fun frame ->
    let drive port position =
        let power = position * 2.f * 100.f |> int
        bot.SetOutputState power port OutputMode.MotorOn RegulationMode.Idle 0 RunState.Running 0ul
    let joints = frame.SkeletonFrame.Skeletons.[0].Joints
    let left = joints.[JointID.HandLeft]
    let right = joints.[JointID.HandRight]
    printfn "Left: %A Right %A" left right
    drive 0 left.Position.Y
    drive 2 right.Position.Y)

System.Console.ReadLine() |> ignore

bot.Disconnect()

Given the LegoBot defined below of course. One issue is the latency and the “chattiness” of the protocol. I tried and couldn’t get the “Segway Bot” to work. Next, I’m thinking of doing a Forth to run directly on the brick and use the Lego brick’s message box protocol to communicate back to a PC only as needed.

Here’s the F# module. Have fun with it!

module LegoBot

open System
open System.IO
open System.IO.Ports
open System.Text

type SensorKind =
    | None          = 0x0
    | Switch        = 0x1
    | Temperature   = 0x2
    | Reflection    = 0x3
    | Angle         = 0x4
    | LightActive   = 0x5
    | LightInactive = 0x6
    | SoundDB       = 0x7
    | SoundDBA      = 0x8
    | Custom        = 0x9
    | LowSpeed      = 0xA
    | LowSpeed9V    = 0xB
    | Color         = 0xD

type SensorMode =
    | Raw             = 0x00
    | Boolean         = 0x20
    | TransitionCount = 0x40
    | PeriodCounter   = 0x60
    | PCTFullScale    = 0x80
    | Celsius         = 0xA0
    | Fahrenheit      = 0xC0
    | AngleSteps      = 0xE0
    | SlopeMask       = 0x1F

type InputValue = {
    IsValid      : bool
    IsCalibrated : bool
    Kind         : SensorKind
    Mode         : SensorMode
    Raw          : int
    Normalized   : int
    Scaled       : int
    Calibrated   : int }

[<Flags>]
type OutputMode =
    | None      = 0
    | MotorOn   = 1
    | Brake     = 2
    | Regulated = 4

type RegulationMode =
    | Idle       = 0
    | MotorSpeed = 1
    | MotorSync  = 2

[<Flags>]
type RunState =
    | Idle     = 0x00
    | RampUp   = 0x10
    | Running  = 0x20
    | RampDown = 0x40

type OutputState = {
    Power         : int
    Mode          : OutputMode
    Regulation    : RegulationMode
    Turn          : int
    Run           : RunState
    Limit         : uint32
    TachoCount    : int
    BlockCount    : int
    RotationCount : int }

type DeviceInfo = {
    Name      : string
    BTAddress : byte[]
    Signal    : int32
    Memory    : int32 }

type VersionInfo = {
    Protocol : float
    Firmware : float }

type LegoBot(port : string) =
    let reader, writer =
        let com = new SerialPort(port)
        com.Open()
        com.ReadTimeout <- 1500
        com.WriteTimeout <- 1500
        let stream = com.BaseStream
        new BinaryReader(stream), new BinaryWriter(stream)

    let send (message : byte[]) =
        int16 message.Length |> writer.Write
        writer.Write message
        writer.Flush()

    let expect (bytes : byte[]) =
        let actual = reader.ReadBytes bytes.Length
        if actual <> bytes then failwith "Invalid response"

    let file (name : string) =
        if name.Length > 19 then failwith "Name too long."
        let bytes = (Seq.map byte name |> List.ofSeq)
        bytes @ List.init (20 - bytes.Length) (fun _ -> 0uy)

    let bytesToString bytes =
        let len = Array.findIndex ((=) 0uy) bytes
        Encoding.ASCII.GetString(bytes, 0, len)

    let intToBytes i = [byte i; i >>> 8 |> byte; i >>> 16 |> byte; i >>> 24 |> byte]

    let shortToBytes (s : int16) = [byte s; s >>> 8 |> byte]

    member x.KeepAlive () = send [|0x80uy; 0x80uy; 0x0Duy|]

    member x.GetDeviceInfo () =
        send [|1uy; 0x9Buy|]
        expect [|33uy; 0uy; 2uy; 0x9Buy; 0uy|]
        { Name = Encoding.ASCII.GetString(reader.ReadBytes 15)
          BTAddress = reader.ReadBytes 7
          Signal = reader.ReadInt32()
          Memory = reader.ReadInt32() }

    member x.GetVersion () =
        send [|1uy; 0x88uy|]
        expect [|7uy; 0uy; 2uy; 0x88uy; 0uy|]
        let readMajorMinor () = Double.Parse(sprintf "%i.%i" (reader.ReadByte()) (reader.ReadByte()))
        { Protocol = readMajorMinor (); Firmware = readMajorMinor () }

    member x.GetBatteryLevel () =
        send [|0uy; 0xBuy|]
        expect [|5uy; 0uy; 2uy; 0xBuy; 0uy|]
        (reader.ReadInt16() |> float) / 1000.

    member x.SetBrickName (name : string) =
        let truncated = Seq.map byte name |> Seq.take (min name.Length 15) |> List.ofSeq
        [1uy; 0x98uy] @ truncated @ [byte truncated.Length] |> Array.ofList |> send
        expect [|3uy; 0uy; 2uy; 0x98uy; 0uy|]

    member x.PlayTone frequency (duration : TimeSpan) =
        writer.Write [|6uy; 0uy; 0x80uy; 3uy|]
        int16 frequency |> writer.Write
        int16 duration.TotalMilliseconds |> writer.Write
        writer.Flush()

    member x.SetInputMode port (kind : SensorKind) (mode : SensorMode) =
        send [|0x80uy; 5uy; byte port; byte kind; byte mode|]

    member x.GetInputValues port =
        send [|0uy; 7uy; byte port|]
        expect [|16uy; 0uy; 2uy; 7uy; 0uy|]
        reader.ReadByte() |> ignore
        { IsValid = (reader.ReadByte() = 1uy)
          IsCalibrated = (reader.ReadByte() = 1uy)
          Kind = reader.ReadByte() |> int |> enum
          Mode = reader.ReadByte() |> int |> enum
          Raw = reader.ReadInt16() |> int
          Normalized = reader.ReadInt16() |> int
          Scaled = reader.ReadInt16() |> int
          Calibrated = reader.ReadInt16() |> int }

    member x.ResetInputScaledValue port = send [|0x80uy; 8uy; byte port|]

    member x.SetOutputState port power (mode : OutputMode) (regulation : RegulationMode) turn (run : RunState) (limit : uint32) = // port 0xFF means 'all'
        writer.Write [|12uy; 0uy; 0uy; 4uy; byte port; byte power; byte mode; byte regulation; byte turn; byte run|]
        writer.Write limit
        writer.Flush()
        expect [|3uy; 0uy; 2uy; 4uy; 0uy|]

    member x.GetOutputState port =
        send [|0uy; 6uy; byte port|]
        expect [|25uy; 0uy; 2uy; 6uy; 0uy|]
        reader.ReadByte() |> ignore
        { Power = reader.ReadByte() |> int32
          Mode = reader.ReadByte() |> int32 |> enum
          Regulation = reader.ReadByte() |> int32 |> enum
          Turn = reader.ReadByte() |> int32
          Run = reader.ReadByte() |> int32 |> enum
          Limit = reader.ReadUInt32()
          TachoCount = reader.ReadInt32()
          BlockCount = reader.ReadInt32()
          RotationCount = reader.ReadInt32() }    

member x.ResetMotorPosition port relative = send [|0x80uy; 0xAuy; byte port; (if relative then 1uy else 0uy)|] member x.MessageWrite box (message : string) = let truncated = Seq.map byte message |> Seq.take (min message.Length 59) |> List.ofSeq [0x0uy; 0x09uy; byte box] @ [byte truncated.Length + 1uy] @ truncated @ [0uy] |> Array.ofList |> send expect [|3uy; 0uy; 2uy; 0x09uy; 0uy|] member x.StartProgram name = [0uy; 0uy] @ file name |> Array.ofList |> send expect [|3uy; 0uy; 2uy; 0uy; 0uy|] member x.StopProgram () = send [|0uy; 1uy|] expect [|3uy; 0uy; 2uy; 1uy; 0uy|] member x.Disconnect () = List.iter (fun p -> x.SetInputMode p SensorKind.None SensorMode.Raw) [0..3] List.iter (fun p -> x.SetOutputState p 0 OutputMode.MotorOn RegulationMode.Idle 0 RunState.Idle 0ul) [0..2] reader.Close() writer.Close()