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()
Comments
- Anonymous
January 05, 2012
Cool :) You have your VS .sln handy? - Anonymous
January 06, 2012
@CeTheWe, sure, here you go: lkjsdf.com/.../KinNXT.zip - Anonymous
January 06, 2012
Thx! - Anonymous
January 07, 2012
Also dropped it in: github.com/.../KinNXT - Anonymous
January 18, 2012
The comment has been removed - Anonymous
January 18, 2012
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 ??? - Anonymous
January 18, 2012
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 KinNXTError 2 The namespace 'Research' is not defined C:UsersNemoLegoKinNXTProgram.fs 2 16 KinNXTError 3 The type 'LegoBot' is not defined C:UsersNemoLegoKinNXTProgram.fs 5 15 KinNXTError 7 The target "Build" does not exist in the project. C:UsersNemoLegoKinNXTKinNXT.fsproj 1 1 KinNXT - Anonymous
January 18, 2012
@Nemo How did you fix the .fsproj error? - Anonymous
January 18, 2012
I just downloaded the visual f# and installed it on top of the visual c# shell that is linked in this article. - Anonymous
January 18, 2012
The comment has been removed - Anonymous
January 19, 2012
@Nemo thanks, i'll try that and i also need this for a School project so time is of the essence!! - Anonymous
January 23, 2012
The comment has been removed - Anonymous
April 02, 2012
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. - Anonymous
April 24, 2012
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.