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 LegoBotopen Microsoft.Research.Kinect.Nui<br>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<br>kinect.Initialize(RuntimeOptions.UseDepth ||| RuntimeOptions.UseSkeletalTracking)<br>kinect.SkeletonEngine.IsEnabled <- truekinect.SkeletonEngine.TransformSmooth <- truekinect.SkeletonFrameReady.Add(fun frame -><br>    let drive port position =        let power = position * 2.f * 100.f |> int<br>        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]<br>    printfn "Left: %A Right %A" left right<br>    drive 0 left.Position.Y<br>    drive 2 right.Position.Y)<br>System.Console.ReadLine() |> ignore<br>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 LegoBotopen Systemopen System.IOopen System.IO.Portsopen System.Texttype SensorKind =<br>    | None          = 0x0<br>    | Switch        = 0x1<br>    | Temperature   = 0x2<br>    | Reflection    = 0x3<br>    | Angle         = 0x4<br>    | LightActive   = 0x5<br>    | LightInactive = 0x6<br>    | SoundDB       = 0x7<br>    | SoundDBA      = 0x8<br>    | Custom        = 0x9<br>    | LowSpeed      = 0xA<br>    | LowSpeed9V    = 0xB<br>    | Color         = 0xDtype SensorMode =<br>    | Raw             = 0x00<br>    | Boolean         = 0x20<br>    | TransitionCount = 0x40<br>    | PeriodCounter   = 0x60<br>    | PCTFullScale    = 0x80<br>    | Celsius         = 0xA0<br>    | Fahrenheit      = 0xC0<br>    | AngleSteps      = 0xE0<br>    | SlopeMask       = 0x1Ftype InputValue = {<br>    IsValid      : bool<br>    IsCalibrated : bool<br>    Kind         : SensorKind<br>    Mode         : SensorMode<br>    Raw          : int<br>    Normalized   : int<br>    Scaled       : int<br>    Calibrated   : int }<br>[<Flags>]type OutputMode =<br>    | None      = 0<br>    | MotorOn   = 1<br>    | Brake     = 2<br>    | Regulated = 4type RegulationMode =<br>    | Idle       = 0<br>    | MotorSpeed = 1<br>    | MotorSync  = 2<br>[<Flags>]type RunState =<br>    | Idle     = 0x00<br>    | RampUp   = 0x10<br>    | Running  = 0x20<br>    | RampDown = 0x40type OutputState = {<br>    Power         : int<br>    Mode          : OutputMode<br>    Regulation    : RegulationMode<br>    Turn          : int<br>    Run           : RunState<br>    Limit         : uint32<br>    TachoCount    : int<br>    BlockCount    : int<br>    RotationCount : int }type DeviceInfo = {<br>    Name      : string<br>    BTAddress : byte[]<br>    Signal    : int32<br>    Memory    : int32 }type VersionInfo = {<br>    Protocol : float<br>    Firmware : float }type LegoBot(port : string) =    let reader, writer =        let com = new SerialPort(port)<br>        com.Open()<br>        com.ReadTimeout <- 1500<br>        com.WriteTimeout <- 1500        let stream = com.BaseStream        new BinaryReader(stream), new BinaryWriter(stream)    let send (message : byte[]) =<br>        int16 message.Length |> writer.Write<br>        writer.Write message<br>        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)<br>        bytes @ List.init (20 - bytes.Length) (fun _ -> 0uy)    let bytesToString bytes =        let len = Array.findIndex ((=) 0uy) bytes<br>        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 () =<br>        send [|1uy; 0x9Buy|]<br>        expect [|33uy; 0uy; 2uy; 0x9Buy; 0uy|]<br>        { Name = Encoding.ASCII.GetString(reader.ReadBytes 15)<br>          BTAddress = reader.ReadBytes 7<br>          Signal = reader.ReadInt32()<br>          Memory = reader.ReadInt32() }    member x.GetVersion () =<br>        send [|1uy; 0x88uy|]<br>        expect [|7uy; 0uy; 2uy; 0x88uy; 0uy|]        let readMajorMinor () = Double.Parse(sprintf "%i.%i" (reader.ReadByte()) (reader.ReadByte()))<br>        { Protocol = readMajorMinor (); Firmware = readMajorMinor () }    member x.GetBatteryLevel () =<br>        send [|0uy; 0xBuy|]<br>        expect [|5uy; 0uy; 2uy; 0xBuy; 0uy|]<br>        (reader.ReadInt16() |> float) / 1000.    member x.SetBrickName (name : string) =        let truncated = Seq.map byte name |> Seq.take (min name.Length 15) |> List.ofSeq<br>        [1uy; 0x98uy] @ truncated @ [byte truncated.Length] |> Array.ofList |> send<br>        expect [|3uy; 0uy; 2uy; 0x98uy; 0uy|]    member x.PlayTone frequency (duration : TimeSpan) =<br>        writer.Write [|6uy; 0uy; 0x80uy; 3uy|]<br>        int16 frequency |> writer.Write<br>        int16 duration.TotalMilliseconds |> writer.Write<br>        writer.Flush()    member x.SetInputMode port (kind : SensorKind) (mode : SensorMode) =<br>        send [|0x80uy; 5uy; byte port; byte kind; byte mode|]    member x.GetInputValues port =<br>        send [|0uy; 7uy; byte port|]<br>        expect [|16uy; 0uy; 2uy; 7uy; 0uy|]<br>        reader.ReadByte() |> ignore<br>        { IsValid = (reader.ReadByte() = 1uy)<br>          IsCalibrated = (reader.ReadByte() = 1uy)<br>          Kind = reader.ReadByte() |> int |> enum<br>          Mode = reader.ReadByte() |> int |> enum<br>          Raw = reader.ReadInt16() |> int<br>          Normalized = reader.ReadInt16() |> int<br>          Scaled = reader.ReadInt16() |> int<br>          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|]<br>        writer.Write limit<br>        writer.Flush()<br>        expect [|3uy; 0uy; 2uy; 4uy; 0uy|]    member x.GetOutputState port =<br>        send [|0uy; 6uy; byte port|]<br>        expect [|25uy; 0uy; 2uy; 6uy; 0uy|]<br>        reader.ReadByte() |> ignore<br>        { Power = reader.ReadByte() |> int32<br>          Mode = reader.ReadByte() |> int32 |> enum<br>          Regulation = reader.ReadByte() |> int32 |> enum<br>          Turn = reader.ReadByte() |> int32<br>          Run = reader.ReadByte() |> int32 |> enum<br>          Limit = reader.ReadUInt32()<br>          TachoCount = reader.ReadInt32()<br>          BlockCount = reader.ReadInt32()<br>          RotationCount = reader.ReadInt32() }        member x.ResetMotorPosition port relative =<br>        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<br>        [0x0uy; 0x09uy; byte box] @ [byte truncated.Length + 1uy] @ truncated @ [0uy] |> Array.ofList |> send<br>        expect [|3uy; 0uy; 2uy; 0x09uy; 0uy|] member x.StartProgram name =<br>        [0uy; 0uy] @ file name |> Array.ofList |> send<br>        expect [|3uy; 0uy; 2uy; 0uy; 0uy|]    member x.StopProgram () =<br>        send [|0uy; 1uy|]<br>        expect [|3uy; 0uy; 2uy; 1uy; 0uy|]    member x.Disconnect () =<br>        List.iter (fun p -> x.SetInputMode p SensorKind.None SensorMode.Raw) [0..3]<br>        List.iter (fun p -> x.SetOutputState p 0 OutputMode.MotorOn RegulationMode.Idle 0 RunState.Idle 0ul) [0..2]<br>        reader.Close()<br>        writer.Close()