Mindstorm API in F# with the lego computation expression.

Mindstorm.fsx

open System
open System.Threading.Tasks
open System.Text

type Opcode =
    | UIRead_GetFirmware = 0x810a
    | UIWrite_LED = 0x821b
    | UIButton_Pressed = 0x8309
    | UIDraw_Update = 0x8400
    | UIDraw_Clean = 0x8401
    | UIDraw_Pixel = 0x8402
    | UIDraw_Line = 0x8403
    | UIDraw_Circle = 0x8404
    | UIDraw_Text = 0x8405
    | UIDraw_FillRect = 0x8409
    | UIDraw_Rect = 0x840a
    | UIDraw_InverseRect = 0x8410
    | UIDraw_SelectFont = 0x8411
    | UIDraw_Topline = 0x8412
    | UIDraw_FillWindow = 0x8413
    | UIDraw_DotLine = 0x8415
    | UIDraw_FillCircle = 0x8418
    | UIDraw_BmpFile = 0x841c
    | Sound_Break = 0x9400
    | Sound_Tone = 0x9401
    | Sound_Play = 0x9402
    | Sound_Repeat = 0x9403
    | Sound_Service = 0x9404
    | InputDevice_GetTypeMode = 0x9905
    | InputDevice_GetDeviceName = 0x9915
    | InputDevice_GetModeName = 0x9916
    | InputDevice_ReadyPct = 0x991b
    | InputDevice_ReadyRaw = 0x991c
    | InputDevice_ReadySI = 0x991d
    | InputDevice_ClearAll = 0x990a
    | InputDevice_ClearChanges = 0x991a
    | InputRead = 0x9a
    | InputReadExt = 0x9e
    | InputReadSI = 0x9d
    | OutputStop = 0xa3
    | OutputPower = 0xa4
    | OutputSpeed = 0xa5
    | OutputStart = 0xa6
    | OutputPolarity = 0xa7
    | OutputReady = 0xaa
    | OutputStepPower = 0xac
    | OutputTimePower = 0xad
    | OutputStepSpeed = 0xae
    | OutputTimeSpeed = 0xaf
    | OutputStepSync = 0xb0
    | OutputTimeSync = 0xb1
    | Tst = 0xff

type SystemOpcode =
    | BeginDownload = 0x92
    | ContinueDownload = 0x93
    | CloseFileHandle = 0x98
    | CreateDirectory = 0x9b
    | DeleteFile = 0x9c

module CommandType =
    let directReply = 0x00uy
    let directNoReply = 0x80uy
    let systemReply = 0x01uy
    let systemNoReply = 0x81uy

type OutputPort =
    | A 
    | B
    | C
    | D

type Parameter =
    | Byte of uint8
    | Short of int16
    | UShort of uint16
    | Int of int32
    | UInt of uint32
    | String of string
    | GlobalIndex of uint8
   

type Command =
    | Direct of Opcode * Parameter list
    | SystemCommand of Opcode * Parameter list


let port p =
    p
    |> List.fold (fun v p ->
        v ||| match p with
              | A -> 0x01uy
              | B -> 0x02uy
              | C -> 0x04uy
              | D -> 0x08uy) 0uy
    |> Byte


let opcodeLength code = if code > Opcode.Tst then 2 else 1

let paramLength = function
    | Byte _ | GlobalIndex _ -> 2
    | Short _ | UShort _ -> 3
    | Int _ | UInt _ -> 5
    | String l -> Encoding.UTF8.GetByteCount l + 2

let length =
    function
    | Direct(code, parameters) ->
        opcodeLength code + List.sumBy paramLength parameters 

let serializeAll f items buffer = List.fold (fun b v -> f v b) buffer items

module ArgumentSize =
    let byte = 0x81uy
    let short = 0x82uy
    let int = 0x83uy
    let string = 0x84uy

module Buffer =
    let inline set value (pos, buffer) =
        Array.set buffer pos value
        pos + 1, buffer

    let inline sets (value:uint16) (pos, buffer) =
        Array.set buffer pos (uint8 value)
        Array.set buffer (pos+1) (uint8 (value >>> 8))
        pos + 2, buffer

    let inline seti (value:uint32) (pos, buffer) =
        Array.set buffer pos (uint8 value)
        Array.set buffer (pos+1) (uint8 (value >>> 8))
        Array.set buffer (pos+2) (uint8 (value >>> 16))
        Array.set buffer (pos+3) (uint8 (value >>> 24))
        pos + 4, buffer

    let inline setb b (pos, buffer) =
        let len = Array.length b
        Array.Copy(b,0,buffer,pos,len)
        pos + len, buffer

    let iff condition f b =
        if condition then
            f b
        else
            b

let serializeOpcode op buffer =
    buffer
    |> Buffer.iff (op > Opcode.Tst) (Buffer.set (uint8 (op >>> 8)))
    |> Buffer.set (uint8 op)

let serializeParam op buffer =
    match op with
    | Byte v -> 
        buffer
        |> Buffer.set ArgumentSize.byte
        |> Buffer.set v
    | UShort v ->
        buffer
        |> Buffer.set ArgumentSize.short
        |> Buffer.sets v
    | Short v ->
        buffer
        |> Buffer.set ArgumentSize.short
        |> Buffer.sets (uint16 v)
    | Int v ->
        buffer
        |> Buffer.set ArgumentSize.int
        |> Buffer.seti (uint32 v)
    | UInt v ->
        buffer
        |> Buffer.set ArgumentSize.int
        |> Buffer.seti v
    | String s ->
        buffer
        |> Buffer.set ArgumentSize.string
        |> Buffer.setb (Encoding.UTF8.GetBytes s) 
        |> Buffer.set 0uy
    | GlobalIndex v -> 
        buffer
        |> Buffer.set 0xe1uy
        |> Buffer.set v

let serializeCommand command buffer=
    match command with
    | Direct (op, p) ->
        buffer
        |> serializeOpcode op
        |> serializeAll serializeParam p

let serialize sequence commandType globalSize commands =
    let length = 5 + List.sumBy length commands
    let buffer = Array.zeroCreate (length + 2)
    
    (0, buffer)
    |> Buffer.sets (uint16 length)
    |> Buffer.sets sequence
    |> Buffer.set commandType
    |> Buffer.sets globalSize
    |> serializeAll serializeCommand commands
    |> snd

type Power = Power of uint8
let power p =
    if p < -100 || p > 100 then
        invalidArg "p" "Power should be between -100 and 100"
    Power (uint8 p)

type Brake =
    | Brake
    | NoBrake
    with
    static member toByte b =
        match b with
        | Brake -> 0x01uy
        | NoBrake -> 0x00uy
        |> Byte 

//let beginCommand  

let outputReady ports = Direct(Opcode.OutputReady, [ Byte 0uy; port ports;])
let startMotor ports = Direct(Opcode.OutputStart, [Byte 0uy; port ports])
let stopMotor ports brake  = Direct(Opcode.OutputStop, [Byte 0uy; port ports; Brake.toByte brake ])
let turnMotorAtPower ports (Power power) = Direct(Opcode.OutputPower, [Byte 0uy; port ports; Byte power])
let turnMotorAtSpeedForTime' ports speed msRampUp msConstant msRampDown brake =
    Direct(Opcode.OutputTimeSpeed, [Byte 0uy;port ports;Byte (byte speed);UInt msRampUp;UInt msConstant;UInt msRampDown; Brake.toByte brake])
let turnMotorAtSpeedForTime ports speed msDuration brake =
    turnMotorAtSpeedForTime' ports speed 0u msDuration 0u brake
let playTone volume frequency duration = Direct(Opcode.Sound_Tone, [Byte volume; UShort frequency; UShort duration ])
type Brick() =
    let brick = new IO.Ports.SerialPort("COM5",115200)
    let received = Event<_>()
    member __.Connect() =
        brick.DataReceived |> Event.add (fun e ->
           if e.EventType = IO.Ports.SerialData.Chars then
            let reader = new IO.BinaryReader(brick.BaseStream)
            let size = reader.ReadInt16()
            let data = reader.ReadBytes (int size)
            received.Trigger data
            ) 
        brick.Open()

    member __.Write data = brick.BaseStream.Write(data,0,data.Length)
    member __.AsyncWrite data = brick.BaseStream.AsyncWrite(data,0,data.Length)

    [<CLIEvent>]
    member __.ReportReceived = received.Publish

    static member sendTo (brick:Brick) (bytes: byte[])  =
        brick.Write bytes

    static member sendToAsync (brick:Brick) (bytes: byte[])  =
        brick.AsyncWrite bytes

    interface IDisposable with
        member __.Dispose() = brick.Close()

let brick = new Brick()

type LegoF<'a> = Brick * uint16 -> Async<'a>
let send command =
    fun (brick, sequence) ->
        command
        |> serialize sequence CommandType.directNoReply 0us
        |> Brick.sendToAsync brick 

type LegoBuilder() =
    member __.Bind(command : LegoF<'a>, f : 'a -> LegoF<'b> ) : LegoF<'b> = 
        fun (brick, sequence) ->
            async.Bind(command (brick,sequence), fun x -> f x (brick, sequence + 1us))

    member __.Bind(command : Command list, f : 'a -> LegoF<'b> ) : LegoF<'b> = 
        fun (brick, sequence) ->
            async.Bind(send command (brick,sequence), fun x -> f x (brick, sequence + 1us))

    member __.Bind(command : Command, f : 'a -> LegoF<'b> ) : LegoF<'b> = 
        fun (brick, sequence) ->
            async.Bind(send [command] (brick,sequence), fun x -> f x (brick, sequence + 1us))

    member __.Bind(command : Async<'a>, f : 'a -> LegoF<'b> ) : LegoF<'b> = 
        fun (brick, sequence) ->
            async.Bind(command, fun x -> f x (brick, sequence + 1us))

    member __.Return x = fun (brick, sequence) -> async.Return x
    member __.ReturnFrom x = x 
    member __.For<'T>(values : 'T seq, body: 'T -> LegoF<unit>) = 
        fun (brick, sequence) ->
            async.For(values, fun t -> body t (brick, sequence))
    member __.Combine(x, y) : LegoF<'a>=
        fun ctx ->
            async.Combine(x ctx, y ctx)
    member __.Delay(f: unit -> LegoF<'a>) =
        fun ctx ->
            async.Delay(fun () -> f () ctx)

    member __.Zero() = fun ctx -> async.Zero()

    member __.Using(d, f) = fun ctx ->
        async.Using(d, f ctx)

let run brick f = 
    let cancelToken = new Threading.CancellationTokenSource()
    Async.Start(f (brick, 1us), cancelToken.Token)
    cancelToken

let lego = LegoBuilder()
brick.Connect()

type Agent<'T> = MailboxProcessor<'T>

type Dispatch =
    | Request of sequence: uint16 * (byte[] -> unit)
    | Forward of sequence: uint16 * byte[]


let responseDispatcher =
    Agent.Start
    <| fun mailbox ->
        let rec loop requests =
            async {
                let! message = mailbox.Receive()
                let newMap =
                    match message with
                    | Request(sequence, reply) ->
                        Map.add sequence reply requests
                    | Forward(sequence, response) ->
                        match Map.tryFind sequence requests with
                        | Some reply -> 
                            reply response
                            Map.remove sequence requests
                        | None -> requests
                return! loop newMap }
        loop Map.empty
 
brick.ReportReceived
|> Event.add ( fun report -> 
    let sequence = BitConverter.ToUInt16(report, 0)
    responseDispatcher.Post(Forward(sequence, report)))     

type ReplyType =
| DirectReply = 0x02
| SystemReply = 0x03
| DirectReplyError = 0x04
| SystemReplyError = 0x05

let request commands globalSize f =
    fun (brick,sequence) ->
        async {
            do! commands
                |> serialize sequence CommandType.directReply globalSize
                |> Brick.sendToAsync brick 

            let! response = responseDispatcher.PostAndAsyncReply(fun reply -> Request(sequence, fun response -> reply.Reply(response)))
            let replyType = enum<ReplyType> (int response.[2])
            if replyType = ReplyType.DirectReplyError || replyType = ReplyType.SystemReplyError then
                failwith "An error occured"
            return f response
        }

type InputPort =
    | In1
    | In2
    | In3
    | In4
    | InA
    | InB
    | InC
    | InD

type ReadDataType =
    | SI
    | Raw
    | Percent



type ReadValue =
    | SI of single
    | Raw of int
    | Percent of int

let readDataTypeLen = function
    | ReadDataType.SI -> 4
    | ReadDataType.Raw -> 4
    | ReadDataType.Percent -> 1

let inputPort = function 
                | In1 -> 0x00uy
                | In2 -> 0x01uy 
                | In3 -> 0x02uy
                | In4 -> 0x03uy
                | InA -> 0x10uy
                | InB -> 0x11uy
                | InC -> 0x12uy 
                | InD -> 0x13uy
                >> Byte

let readData data pos = function
    | ReadDataType.SI -> BitConverter.ToSingle(data, pos) |> SI
    | ReadDataType.Raw -> BitConverter.ToInt32(data, pos) |> Raw
    | ReadDataType.Percent -> Percent (int data.[pos])
    
let readOpcode = function
    | ReadDataType.SI -> Opcode.InputDevice_ReadySI
    | ReadDataType.Raw -> Opcode.InputDevice_ReadyRaw
    | ReadDataType.Percent -> Opcode.InputDevice_ReadyPct


let mapPos f start list =
    let outList, totalLen =
        List.fold (fun (l,pos) e -> 
            let result,len = f e pos
            (result :: l), (pos + len)) ([],start) list
    List.rev outList, totalLen

type Mode =
    | TouchMode of TouchMode
    | ColorMode of ColorMode
    | IRMode of IRMode
and TouchMode = Touch | Bumps
and ColorMode = Reflective | Ambient | Color | ReflectiveRaw | ReflectiveRgb | Calibration
and IRMode = Proximity | Seek | Remote | RemoteA | SAlt | Calibrate

let modeToUInt8 = function
    | TouchMode Touch -> 0uy
    | TouchMode Bumps -> 1uy   
    | ColorMode Reflective -> 0uy
    | ColorMode Ambient -> 1uy
    | ColorMode Color -> 2uy
    | ColorMode ReflectiveRaw -> 3uy
    | ColorMode ReflectiveRgb -> 4uy
    | ColorMode Calibration -> 5uy
    | IRMode Proximity -> 0uy
    | IRMode Seek -> 1uy
    | IRMode Remote -> 2uy
    | IRMode RemoteA -> 3uy
    | IRMode SAlt -> 4uy
    | IRMode Calibrate -> 5uy



let read (inputs: (InputPort * ReadDataType * Mode) list)  = 
    let commands, globalSize =
        inputs
        |> mapPos (fun (inPort, dataType, mode) pos -> 
            Direct(readOpcode dataType , [Byte 0uy; inputPort inPort; Byte 0uy; Byte (modeToUInt8 mode); Byte 1uy; GlobalIndex (uint8 pos)]), readDataTypeLen dataType) 0
    request commands (uint16 globalSize) (fun data -> 
        inputs
        |> mapPos (fun (inPort, dataType, mode)  pos->
                          (inPort, readData data pos dataType, mode), readDataTypeLen dataType) 3
        |> fst )

let (|Pushed|Released|) input =
    if input = SI 1.f then Pushed else Released



type Color =
    | Transparent
    | Black
    | Blue
    | Green
    | Yellow
    | Red
    | White
    | Brown

let (|Color|) = function
    | SI 1.f -> Black
    | SI 2.f -> Blue
    | SI 3.f -> Green
    | SI 4.f -> Yellow
    | SI 5.f -> Red
    | SI 6.f -> White
    | SI 7.f -> Brown
    | _ -> Transparent

#r "System.Speech"
open System.Speech.Synthesis
open System.Speech.Recognition
let syn = new SpeechSynthesizer()
let say s = syn.Speak(s: string)



lego {
    for i in 0..3 do
        do! [ turnMotorAtSpeedForTime [A] 50 1000u NoBrake;outputReady [A];turnMotorAtSpeedForTime [A] -50 1000u NoBrake; outputReady [A] ] 
    do! stopMotor [A] NoBrake
    } |> run brick

let rec loop color = 
    lego {
        let! results = read [ In3, ReadDataType.SI, ColorMode Color]
        match results with
        | [ _, Color c, _] when c <> color && c <> Transparent ->
            match c with
            | Black -> "Noir"
            | Blue -> "Bleu"
            | Green -> "Vert"
            | Yellow -> "Jaune"
            | Red -> "Rouge"
            | White -> "Blanc"
            | Brown -> "Marron"
            | _ -> ""
            |> say
            printfn "Color: %A" c
            do! Async.Sleep 100 
            return! loop c
        | _ -> 
            do! Async.Sleep 100
            return! loop color
    }
loop Transparent |> run brick

let recog = new SpeechRecognizer()
let builder = new GrammarBuilder()
builder.Append(new Choices("Hélicoptere", "Avance", "Arrète", "Tourne à droite", "Tourne à gauche", "Recule"))
builder.Culture <- new Globalization.CultureInfo("fr-FR")
recog.LoadGrammar(new Grammar(builder))


lego {
    for i in 0 .. 100 do
        let! reco = Async.AwaitEvent recog.SpeechRecognized
        match reco.Result.Text with
        | "Hélicoptere" -> do! turnMotorAtSpeedForTime [A] 50 1000u NoBrake
        | "Avance" -> do! [ turnMotorAtPower [B;C] (power 100); startMotor [B;C]]
        | "Recule" -> do! [ turnMotorAtPower [B;C] (power -100); startMotor [B;C]]
        | "Arrète" -> do! stopMotor [B;C] Brake
        | "Tourne à droite" -> do! [ turnMotorAtSpeedForTime [B] 100 500u NoBrake; turnMotorAtSpeedForTime [C] -50 500u NoBrake;]
        | "Tourne à gauche" -> do! [ turnMotorAtSpeedForTime [B] -100 500u NoBrake; turnMotorAtSpeedForTime [C] 50 500u NoBrake;]
        | _ -> ()
} |> run brick


lego {
    for i in 1 ..50 do
        let! results = read [ In4, ReadDataType.Raw, IRMode SAlt]

        printfn "%A" results
        do! Async.Sleep 500
    }
|> run brick

let tokenArg =
    let rec loopArg color = 
        lego {
            let! results = read [ In3, ReadDataType.SI, ColorMode Color]
            match results with
            | [ _, Color c, _] when c <> color && c <> Transparent ->
                match c with
                | Black -> "Noir"
                | Blue -> "Bleu"
                | Green -> "Vert"
                | Yellow -> "Jaune"
                | Red -> 
                    "ça m'énerve"
                | White -> "Blanc"
                | Brown -> "Marron"
                | _ -> ""
                |> say
                if c = Red then
                    do! [turnMotorAtSpeedForTime [A] 5 3000u NoBrake]

                printfn "Color: %A" c
                do! Async.Sleep 100 
                return! loopArg c
            | _ -> 
                do! Async.Sleep 100
                return! loopArg color
        }
    loopArg Transparent |> run brick

tokenArg.Cancel()
Async.CancelDefaultToken()



let tokenSpeed =
    lego {
        do! startMotor [A]
        let rec loopArg() = 
            lego {
                let! results = read [ In4, ReadDataType.Percent, IRMode Proximity]
                match results with
                | [ _, Percent p, _] ->
                    do! [turnMotorAtPower [A] (power (100 - p)) ]

                    do! Async.Sleep 500
                    return! loopArg()
                | _ -> 
                    do! Async.Sleep 100
                    return! loopArg()
            }
        do! loopArg() 
    } |> run brick

lego { do! stopMotor [A] NoBrake} |> run brick 
tokenSpeed.Cancel()

Console.ReadLine()