Lego KinNXT


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()
Comments (14)

  1. Anonymous says:

    Cool 🙂 You have your VS .sln handy?

  2. Anonymous says:

    Thx!

  3. Anonymous says:

    Hi there, I cant get your programs to work, my visual c# just says it does not support fsproj files. what do I do wrong???

  4. Anonymous says:

    Hi again, with a little help from a friend we solved the problems, now I just have one question, how do i get it all to talk together ???

  5. Anonymous says:

    I get these errors when i try to run your code:

    Error 1 The namespace or module 'LegoBot' is not defined C:UsersNemoLegoKinNXTProgram.fs 1 6 KinNXT

    Error 2 The namespace 'Research' is not defined C:UsersNemoLegoKinNXTProgram.fs 2 16 KinNXT

    Error 3 The type 'LegoBot' is not defined C:UsersNemoLegoKinNXTProgram.fs 5 15 KinNXT

    Error 7 The target "Build" does not exist in the project. C:UsersNemoLegoKinNXTKinNXT.fsproj 1 1 KinNXT

  6. Anonymous says:

    @Nemo How did you fix the .fsproj error?

  7. Anonymous says:

    I just downloaded the visual f# and installed it on top of the visual c# shell that is linked in this article.

  8. Anonymous says:

    I know I am spamming you and I am sorry for that, but I need this for a shcool project this saturday, and I would really like to get this working, I have the SDK and visual C# and F# but I cant get it to talk together or anything.

  9. Anonymous says:

    @Nemo thanks, i'll try that and i also need this for a School project so time is of the essence!!

  10. Note to those trying to get this working in their environments: It depends on Kinect SDK 1.0 beta 2. Also, the .fsproj references .NET 4.5 and F# 3. That’s not necessary though if you’d rather change the project to target .NET 4.

    If all else fails, you should be able to "start fresh" by creating a new F# project, include the two .fs files and reference Kinect SDK (/Program Files/Microsoft SDKs/Kinect/v1.0 Beta 2/Assemblies/Microsoft.Research.Kinect.dll).

    Of course after all of this, you’ll have to get your Lego brick working on some COM port and change that line in the code to match.

    You can test the Kinect independently with some of the SDK apps (e.g. the skeletal tracking one) and can test the Lego connectivity in the LabView software that comes with the Lego kit (be sure to close it before trying to connect on the same COM port from the F# program).

    Hope that helps. Have fun!

  11. Anonymous says:

    Hi i was trying to get your coding up and running. I finally figured out what to do and i think i ran into a problem. when the program runs, i establishes the connection with the NXT, and it turns on the kinect, But for some reason the NXT isn't responding to my movements. I even tried doing the same movements the kid in the video did and nothing happened still. It would mean a lot if you could help me with this problem.

  12. Anonymous says:

    Thanks for this fun and instructive post. I took your suggestion to create a new F# project from the scratch with the files you posted on GitHub. I was able to connect to the NXT over bluetooth and run some basic commands in FSI. Please keep posting other experiments with NXT. BTW, I use only NXT and not the Kinect.