API tools faq
paste
Advertisement
Guest User

Sauropoda

a guest
May 11th, 2014
201
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
F# 19.65 KB | None | 0 0
raw download clone embed print report
  1. open System
  2. open Trik
  3. open Microsoft.FSharp.Core.Operators
  4. open Microsoft.FSharp.Control
  5.  
  6. // ??????????????? ref ???????????????
  7. type RGB = {r: int; g: int; b: int} with
  8.     static member Zero = {r = 0; g = 0; b =0}
  9.  
  10. [<Literal>]
  11. let MagicSize = 4
  12.  
  13. let sleep (msec:int) = System.Threading.Thread.Sleep msec
  14.  
  15. [<EntryPoint>]
  16. let main args =
  17.     Helpers.I2C.init "/dev/i2c-2" 0x48 1
  18.     use gnd = new Trik.PowerMotor(0x16)
  19.     use stripe = new Trik.LedStripe(0x14, 0x15, 0x17, 0x16)
  20.     gnd.SetPower(100)
  21.  
  22.     use gSM = new Servomotor("/sys/class/pwm/ehrpwm.1:1", {stop = 0; zero = 1100000; min = 900000; max = 1400000; period = 20000000})
  23.  
  24.     use file = new IO.StreamReader("/tmp/dsp-detector.out.fifo")
  25.  
  26.     let mutable turnG = 0
  27.     let mutable turnGprev = 0
  28.  
  29.     // Для заполнения flagsMas[] и red[], gre[], blu[]
  30.     let createMagicArray() = Array.create MagicSize RGB.Zero
  31.  
  32.     // ??????????????? ref ???????????????
  33.     let currInput = ref (createMagicArray ())
  34.  
  35.     let rec updateCurrentFrameInfo () =
  36.         async {
  37.             let line = file.ReadLine() // hope we are faster to read than to process frames with DSP
  38.             let mas = line.Split [|' '|]
  39.             let tmp = Array.init MagicSize <| fun i ->
  40.                 { r = 0xFF &&& (int mas.[i + 3] >>> 16)
  41.                   g = 0xFF &&& (int mas.[i + 3] >>> 8)
  42.                   b = 0xFF &&& int mas.[i + 3] }
  43.  
  44.             // ??????????????? ref ???????????????
  45.             currInput := tmp // no explicit lock
  46.             do! updateCurrentFrameInfo()
  47.         }
  48.  
  49.     updateCurrentFrameInfo() |> Async.Start
  50.  
  51.     // ??????????????? ref ???????????????
  52.     let prevLED = ref RGB.Zero
  53.  
  54.     // Для определения следующего цвета ленты
  55.     let mutable nextObtained = createMagicArray()
  56.     let mutable prevObtained = createMagicArray()
  57.  
  58.     // red, gre, blu are value from 0 to 255
  59.     let changeColor prev cur =
  60.         let deltaR = cur.r - prev.r
  61.         let deltaG = cur.g - prev.g
  62.         let deltaB = cur.b - prev.b
  63.         let steps = 10
  64.         let sleep () = sleep <| 2000 / steps
  65.         for i = 1 to steps do        
  66.             let red, green, blue = (prev.r * steps + i * deltaR) / steps, (prev.g * steps + i * deltaG) / steps, (prev.b * steps + i * deltaB) / steps
  67.             stripe.SetPower(red, green, blue)
  68.             printfn "stripe.SetPower%A" (red, green, blue)
  69.             sleep()
  70.  
  71.     gSM.SetPower(0)
  72.  
  73.     while true do   // лучше пока не нажата кнопка
  74.  
  75.         let flags = Array.create MagicSize 0
  76.  
  77.         prevObtained <- nextObtained
  78.  
  79.         let steps = 10
  80.  
  81.         let sleep() = sleep <| 2000 / steps
  82.  
  83.         for counter = 1 to steps do
  84.             let prevInput = currInput  
  85.             let threshold = 10
  86.             for i = 1 to 3 do
  87.            
  88.                 // ??????????????? ref ???????????????
  89.                 if Array.forall2 (fun x y -> Math.Abs(x.r - y.r) <= threshold && Math.Abs(x.g - y.g) <= threshold && Math.Abs(x.b - y.b) <= threshold) !currInput !prevInput
  90.                 then    
  91.                     flags.[i] <- flags.[i] + 1
  92.  
  93.                     // ??????????????? ref ???????????????
  94.                     nextObtained <- !currInput
  95.  
  96.         printfn "mRedPrev mGrePrev mBluPrev %A" prevObtained
  97.         printfn "red gre blu %A" nextObtained
  98.  
  99.         let inline change (rgb: RGB) =
  100.             printfn "change (redPrev, grePrev, bluPrev) %A" prevLED
  101.             printfn "change (red, gre, blu) %A" (rgb.r, rgb.g, rgb.b)
  102.  
  103.             // ??????????????? ref ???????????????
  104.             changeColor !prevLED rgb
  105.             prevLED := rgb
  106.  
  107.  
  108.         let mutable changeOfColor1 = Array.create MagicSize 0
  109.         let mutable changeOfColor2 = Array.create MagicSize 0
  110.  
  111.         if Math.Abs(nextObtained.[1].r - prevObtained.[1].r) >= 20 || Math.Abs(nextObtained.[1].g - prevObtained.[1].g) >= 20 || Math.Abs(nextObtained.[1].b - prevObtained.[1].b) >= 20 then changeOfColor1.[1] <- 1
  112.         if Math.Abs(nextObtained.[1].r - prevObtained.[1].r) >= 20 || Math.Abs(nextObtained.[1].g - prevObtained.[1].g) >= 20 || Math.Abs(nextObtained.[1].b - prevObtained.[1].b) >= 20 then changeOfColor1.[2] <- 1
  113.         if Math.Abs(nextObtained.[1].r - prevObtained.[1].r) >= 20 || Math.Abs(nextObtained.[1].g - prevObtained.[1].g) >= 20 || Math.Abs(nextObtained.[1].b - prevObtained.[1].b) >= 20 then changeOfColor1.[3] <- 1
  114.    
  115.         // ??????????????? ref ???????????????
  116.         if Math.Abs(nextObtained.[1].r - (!prevLED).r) >= 20 || Math.Abs(nextObtained.[1].g - (!prevLED).g) >= 20 || Math.Abs(nextObtained.[1].b - (!prevLED).b) >= 20 then changeOfColor2.[1] <- 1
  117.         if Math.Abs(nextObtained.[1].r - (!prevLED).r) >= 20 || Math.Abs(nextObtained.[1].g - (!prevLED).g) >= 20 || Math.Abs(nextObtained.[1].b - (!prevLED).b) >= 20 then changeOfColor2.[2] <- 1
  118.         if Math.Abs(nextObtained.[1].r - (!prevLED).r) >= 20 || Math.Abs(nextObtained.[1].g - (!prevLED).g) >= 20 || Math.Abs(nextObtained.[1].b - (!prevLED).b) >= 20 then changeOfColor2.[3] <- 1
  119.  
  120.         // turnG = {-90; 0; 90}
  121.  
  122.     // 9 попаданий в один цвет из 12-ти за 2 секунды, скорее всего, говорят о его стабильности
  123.         if flags.[1] >= 9 && flags.[2] >= 9 && flags.[3] >= 9 then
  124.         // Цвета во всех трех областях стабильны (скорее всего, нет мельканий)
  125.  
  126.             // Если цвет изменился в этой области и относительно текущего, то все ОК
  127.             if changeOfColor1.[1] = 1 && changeOfColor2.[1] = 1 && turnG <> 90 then
  128.                 turnG <- 90
  129.                 gSM.SetPower(turnG)
  130.                 printfn "gSM.SetPower(%A)" turnG
  131.                 change nextObtained.[1]
  132.             elif changeOfColor1.[2] = 1 && changeOfColor2.[2] = 1 && turnG <> 0 then
  133.                 turnG <- 0
  134.                 gSM.SetPower(turnG)
  135.                 printfn "gSM.SetPower(%A)" turnG
  136.                 change nextObtained.[2]
  137.             elif changeOfColor1.[3] = 1 && changeOfColor2.[3] = 1 && turnG <> -90 then
  138.                 turnG <- -90
  139.                 gSM.SetPower(turnG)
  140.                 printfn "gSM.SetPower(%A)" turnG
  141.                 change nextObtained.[3]
  142.  
  143.             // Если цвет изменился в этой области, то все ОК
  144.             elif changeOfColor1.[1] = 1 && turnG <> 90 then
  145.                 turnG <- 90
  146.                 gSM.SetPower(turnG)
  147.                 printfn "gSM.SetPower(%A)" turnG
  148.                 change nextObtained.[1]
  149.             elif changeOfColor1.[2] = 1 && turnG <> 0 then
  150.                 turnG <- 0
  151.                 gSM.SetPower(turnG)
  152.                 printfn "gSM.SetPower(%A)" turnG
  153.                 change nextObtained.[2]
  154.             elif changeOfColor1.[3] = 1 && turnG <> -90 then
  155.                 turnG <- -90
  156.                 gSM.SetPower(turnG)
  157.                 printfn "gSM.SetPower(%A)" turnG
  158.                 change nextObtained.[3]
  159.  
  160.             // Если цвет отличен от текущего, то все ОК
  161.             if changeOfColor2.[1] = 1 && turnG <> 90 then
  162.                 turnG <- 90
  163.                 gSM.SetPower(turnG)
  164.                 printfn "gSM.SetPower(%A)" turnG
  165.                 change nextObtained.[1]
  166.             elif changeOfColor2.[2] = 1 && turnG <> 0 then
  167.                 turnG <- 0
  168.                 gSM.SetPower(turnG)
  169.                 printfn "gSM.SetPower(%A)" turnG
  170.                 change nextObtained.[2]
  171.             elif changeOfColor2.[3] = 1 && turnG <> -90 then
  172.                 turnG <- -90
  173.                 gSM.SetPower(turnG)
  174.                 printfn "gSM.SetPower(%A)" turnG
  175.                 change nextObtained.[3]
  176.  
  177.  
  178.             // Если цвет изменился в этой области и относительно текущего, то все ОК
  179.             if changeOfColor1.[1] = 1 && changeOfColor2.[1] = 1 then
  180.                 change nextObtained.[1]
  181.             elif changeOfColor1.[2] = 1 && changeOfColor2.[2] = 1 then
  182.                 change nextObtained.[2]
  183.             elif changeOfColor1.[3] = 1 && changeOfColor2.[3] = 1 then
  184.                 change nextObtained.[3]
  185.  
  186.             // Если цвет изменился в этой области, то все ОК
  187.             elif changeOfColor1.[1] = 1 then
  188.                 change nextObtained.[1]
  189.             elif changeOfColor1.[2] = 1 then
  190.                 change nextObtained.[2]
  191.             elif changeOfColor1.[3] = 1 then
  192.                 change nextObtained.[3]
  193.  
  194.             // Если цвет отличен от текущего, то все ОК
  195.             if changeOfColor2.[1] = 1 then
  196.                 change nextObtained.[1]
  197.             elif changeOfColor2.[2] = 1 then
  198.                 change nextObtained.[2]
  199.             elif changeOfColor2.[3] = 1 then
  200.                 change nextObtained.[3]
  201.  
  202.  
  203.  
  204.             // Цвет везде одинаковый и нигде не изменился
  205.             if turnG <> 90 then
  206.                 turnG <- 90
  207.                 gSM.SetPower(turnG)
  208.                 printfn "gSM.SetPower(%A)" turnG
  209.             elif turnG <> 0 then
  210.                 turnG <- 0
  211.                 gSM.SetPower(turnG)
  212.                 printfn "gSM.SetPower(%A)" turnG
  213.             elif turnG <> -90 then
  214.                 turnG <- -90
  215.                 gSM.SetPower(turnG)
  216.                 printfn "gSM.SetPower(%A)" turnG
  217.  
  218.  
  219.         // Цвет стабилен только в первых двух областях
  220.         elif flags.[1] >= 9 && flags.[2] >= 9 then
  221.             // Если цвет изменился в этой области и относительно текущего, то все ОК
  222.             if changeOfColor1.[1] = 1 && changeOfColor2.[1] = 1 && turnG <> 90 then
  223.                 turnG <- 90
  224.                 gSM.SetPower(turnG)
  225.                 printfn "gSM.SetPower(%A)" turnG
  226.                 change nextObtained.[1]
  227.             elif changeOfColor1.[2] = 1 && changeOfColor2.[2] = 1 && turnG <> 0 then
  228.                 turnG <- 0
  229.                 gSM.SetPower(turnG)
  230.                 printfn "gSM.SetPower(%A)" turnG
  231.                 change nextObtained.[2]
  232.  
  233.             // Если цвет изменился в этой области, то все ОК
  234.             elif changeOfColor1.[1] = 1 && turnG <> 90 then
  235.                 turnG <- 90
  236.                 gSM.SetPower(turnG)
  237.                 printfn "gSM.SetPower(%A)" turnG
  238.                 change nextObtained.[1]
  239.             elif changeOfColor1.[2] = 1 && turnG <> 0 then
  240.                 turnG <- 0
  241.                 gSM.SetPower(turnG)
  242.                 printfn "gSM.SetPower(%A)" turnG
  243.                 change nextObtained.[2]
  244.  
  245.             // Если цвет отличен от текущего, то все ОК
  246.             if changeOfColor2.[1] = 1 && turnG <> 90 then
  247.                 turnG <- 90
  248.                 gSM.SetPower(turnG)
  249.                 printfn "gSM.SetPower(%A)" turnG
  250.                 change nextObtained.[1]
  251.             elif changeOfColor2.[2] = 1 && turnG <> 0 then
  252.                 turnG <- 0
  253.                 gSM.SetPower(turnG)
  254.                 printfn "gSM.SetPower(%A)" turnG
  255.                 change nextObtained.[2]
  256.  
  257.  
  258.             // Если цвет изменился в этой области и относительно текущего, то все ОК
  259.             if changeOfColor1.[1] = 1 && changeOfColor2.[1] = 1 then
  260.                 change nextObtained.[1]
  261.             elif changeOfColor1.[2] = 1 && changeOfColor2.[2] = 1 then
  262.                 change nextObtained.[2]
  263.  
  264.             // Если цвет изменился в этой области, то все ОК
  265.             elif changeOfColor1.[1] = 1 then
  266.                 change nextObtained.[1]
  267.             elif changeOfColor1.[2] = 1 then
  268.                 change nextObtained.[2]
  269.  
  270.             // Если цвет отличен от текущего, то все ОК
  271.             if changeOfColor2.[1] = 1 then
  272.                 change nextObtained.[1]
  273.             elif changeOfColor2.[2] = 1 then
  274.                 change nextObtained.[2]
  275.  
  276.  
  277.             // Цвет везде одинаковый и нигде не изменился
  278.             if turnG <> 90 then
  279.                 turnG <- 90
  280.                 gSM.SetPower(turnG)
  281.                 printfn "gSM.SetPower(%A)" turnG
  282.             elif turnG <> 0 then
  283.                 turnG <- 0
  284.                 gSM.SetPower(turnG)
  285.                 printfn "gSM.SetPower(%A)" turnG
  286.  
  287.  
  288.         // Цвет стабилен только в 1 и 3 областях
  289.         elif flags.[1] >= 9 && flags.[3] >= 9 then
  290.             // Если цвет изменился в этой области и относительно текущего, то все ОК
  291.             if changeOfColor1.[1] = 1 && changeOfColor2.[1] = 1 && turnG <> 90 then
  292.                 turnG <- 90
  293.                 gSM.SetPower(turnG)
  294.                 printfn "gSM.SetPower(%A)" turnG
  295.                 change nextObtained.[1]
  296.             elif changeOfColor1.[3] = 1 && changeOfColor2.[3] = 1 && turnG <> -90 then
  297.                 turnG <- -90
  298.                 gSM.SetPower(turnG)
  299.                 printfn "gSM.SetPower(%A)" turnG
  300.                 change nextObtained.[3]
  301.  
  302.             // Если цвет изменился в этой области, то все ОК
  303.             elif changeOfColor1.[1] = 1 && turnG <> 90 then
  304.                 turnG <- 90
  305.                 gSM.SetPower(turnG)
  306.                 printfn "gSM.SetPower(%A)" turnG
  307.                 change nextObtained.[1]
  308.             elif changeOfColor1.[3] = 1 && turnG <> -90 then
  309.                 turnG <- -90
  310.                 gSM.SetPower(turnG)
  311.                 printfn "gSM.SetPower(%A)" turnG
  312.                 change nextObtained.[3]
  313.  
  314.             // Если цвет отличен от текущего, то все ОК
  315.             if changeOfColor2.[1] = 1 && turnG <> 90 then
  316.                 turnG <- 90
  317.                 gSM.SetPower(turnG)
  318.                 printfn "gSM.SetPower(%A)" turnG
  319.                 change nextObtained.[1]
  320.             elif changeOfColor2.[3] = 1 && turnG <> -90 then
  321.                 turnG <- -90
  322.                 gSM.SetPower(turnG)
  323.                 printfn "gSM.SetPower(%A)" turnG
  324.                 change nextObtained.[3]
  325.  
  326.  
  327.             // Если цвет изменился в этой области и относительно текущего, то все ОК
  328.             if changeOfColor1.[1] = 1 && changeOfColor2.[1] = 1 then
  329.                 change nextObtained.[1]
  330.             elif changeOfColor1.[3] = 1 && changeOfColor2.[3] = 1 then
  331.                 change nextObtained.[3]
  332.  
  333.             // Если цвет изменился в этой области, то все ОК
  334.             elif changeOfColor1.[1] = 1 then
  335.                 change nextObtained.[1]
  336.             elif changeOfColor1.[3] = 1 then
  337.                 change nextObtained.[3]
  338.  
  339.             // Если цвет отличен от текущего, то все ОК
  340.             if changeOfColor2.[1] = 1 then
  341.                 change nextObtained.[1]
  342.             elif changeOfColor2.[3] = 1 then
  343.                 change nextObtained.[2]
  344.  
  345.  
  346.             // Цвет везде одинаковый и нигде не изменился
  347.             if turnG <> 90 then
  348.                 turnG <- 90
  349.                 gSM.SetPower(turnG)
  350.                 printfn "gSM.SetPower(%A)" turnG
  351.             elif turnG <> -90 then
  352.                 turnG <- -90
  353.                 gSM.SetPower(turnG)
  354.                 printfn "gSM.SetPower(%A)" turnG
  355.  
  356.  
  357.         // Цвет стабилен только в 2 и 3 областях
  358.         elif flags.[2] >= 9 && flags.[3] >= 9 then
  359.             // Если цвет изменился в этой области и относительно текущего, то все ОК
  360.             if changeOfColor1.[2] = 1 && changeOfColor2.[2] = 1 && turnG <> 0 then
  361.                 turnG <- 0
  362.                 gSM.SetPower(turnG)
  363.                 printfn "gSM.SetPower(%A)" turnG
  364.                 change nextObtained.[2]
  365.             elif changeOfColor1.[3] = 1 && changeOfColor2.[3] = 1 && turnG <> -90 then
  366.                 turnG <- -90
  367.                 gSM.SetPower(turnG)
  368.                 printfn "gSM.SetPower(%A)" turnG
  369.                 change nextObtained.[3]
  370.  
  371.             // Если цвет изменился в этой области, то все ОК
  372.             elif changeOfColor1.[2] = 1 && turnG <> 0 then
  373.                 turnG <- 0
  374.                 gSM.SetPower(turnG)
  375.                 printfn "gSM.SetPower(%A)" turnG
  376.                 change nextObtained.[2]
  377.             elif changeOfColor1.[3] = 1 && turnG <> -90 then
  378.                 turnG <- -90
  379.                 gSM.SetPower(turnG)
  380.                 printfn "gSM.SetPower(%A)" turnG
  381.                 change nextObtained.[3]
  382.  
  383.             // Если цвет отличен от текущего, то все ОК
  384.             if changeOfColor2.[2] = 1 && turnG <> 0 then
  385.                 turnG <- 0
  386.                 gSM.SetPower(turnG)
  387.                 printfn "gSM.SetPower(%A)" turnG
  388.                 change nextObtained.[2]
  389.             elif changeOfColor2.[3] = 1 && turnG <> -90 then
  390.                 turnG <- -90
  391.                 gSM.SetPower(turnG)
  392.                 printfn "gSM.SetPower(%A)" turnG
  393.                 change nextObtained.[3]
  394.  
  395.  
  396.             // Если цвет изменился в этой области и относительно текущего, то все ОК
  397.             if changeOfColor1.[2] = 1 && changeOfColor2.[2] = 1 then
  398.                 change nextObtained.[2]
  399.             elif changeOfColor1.[3] = 1 && changeOfColor2.[3] = 1 then
  400.                 change nextObtained.[3]
  401.  
  402.             // Если цвет изменился в этой области, то все ОК
  403.             elif changeOfColor1.[2] = 1 then
  404.                 change nextObtained.[2]
  405.             elif changeOfColor1.[3] = 1 then
  406.                 change nextObtained.[3]
  407.  
  408.             // Если цвет отличен от текущего, то все ОК
  409.             if changeOfColor2.[2] = 1 then
  410.                 change nextObtained.[2]
  411.             elif changeOfColor2.[3] = 1 then
  412.                 change nextObtained.[3]
  413.  
  414.             // Цвет везде одинаковый и нигде не изменился
  415.             if turnG <> 0 then
  416.                 turnG <- 0
  417.                 gSM.SetPower(turnG)
  418.                 printfn "gSM.SetPower(%A)" turnG
  419.             elif turnG <> -90 then
  420.                 turnG <- -90
  421.                 gSM.SetPower(turnG)
  422.                 printfn "gSM.SetPower(%A)" turnG
  423.  
  424.  
  425.         // Цвет стабилен только в 1 области
  426.         elif flags.[1] > 9 then
  427.             if turnG <> 90 then
  428.                 turnG <- 90
  429.                 gSM.SetPower(turnG)
  430.                 printfn "gSM.SetPower(%A)" turnG
  431.             if changeOfColor2.[1] = 1 then
  432.                 change nextObtained.[1]
  433.  
  434.         // Цвет стабилен только во 2 области
  435.         elif flags.[2] > 9 then
  436.             if turnG <> 0 then
  437.                 turnG <- 0
  438.                 gSM.SetPower(turnG)
  439.                 printfn "gSM.SetPower(%A)" turnG
  440.             if changeOfColor2.[2] = 1 then
  441.                 change nextObtained.[2]
  442.  
  443.         // Цвет стабилен только в 3 области
  444.         elif flags.[3] > 9 then
  445.             if turnG <> -90 then
  446.                 turnG <- -90
  447.                 gSM.SetPower(turnG)
  448.                 printfn "gSM.SetPower(%A)" turnG
  449.             if changeOfColor2.[3] = 1 then
  450.                 change nextObtained.[3]
  451.     0
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!