Nerves LivebookでRoller485を動かしてみた

はじめに

core MP135でNerves livebookが動くようになりました。
何か動かしてみたかったので、最近入手した、Roller 485を動かしてみました。

Roller485とは

本家のTutorialに写真付きの説明があります。

単なるモータなんですが、モーター制御の仕組みがはいっていて、４つの事がコマンドを送るだけでできるという優れものです。

mode	働き
Speed	指定した回転速度で回転させる。 RPMで指定します（一分間に何回転させるか）
Position	指定した確度に移動させる。 確度は積算される。720度を指定すると2回回ったところで止まります
Current	指定した電流でモータを回す モータを回す力を制御した場合利用
Encoder	角度の取得のみ。モーターは停止

Roller485制御モジュール

単純にコマンドを送るだけです。
4Byteのデータを送る時の順番が、リトルインディアンなのが注意点です。

<<x::32-little>>を使いました

今回使用した、position_mode関係だけ実装してあります。その他のモードも I2Cのコマンド に説明はあるので今後実装してみようと思います。

defmodule Roller485 do
  def open(addr) do
    {:ok, bus} = Circuits.I2C.open("i2c-1")
    {bus,addr}
  end

  def write({bus, addr}, reg, data) do
    Circuits.I2C.write(bus, addr, [reg, data])
  end
  
  def write4({bus, addr}, reg, data) do
    Circuits.I2C.write(bus, addr, <<reg::8, data::32-little>>)
  end

  def read4({bus, addr}, reg) do
    {:ok, <<x::32-little>>} = Circuits.I2C.write_read(bus, addr, [reg], 4)
    x
  end

  def set_position_mode(handle, target_position, max_power \\ 100 ) do
    Roller485.write(handle, 0,1)
    Roller485.write(handle, 1,2)
    Roller485.write4(handle, 0x20,max_power*100)
    Roller485.write4(handle, 0x80,target_position*100)    
  end

  def position_readback(handle) do
    Roller485.read4(handle, 0x90)/100
  end
end

LCDへの表示

モータの動きに合わせてなにか表示してみたかったので、LCDに図形を表示するプログラムを作ってみました。
２ｘ４に区分けしてそれぞれの領域に色を塗っています。

フレームバッファーは1画素2byteですが、このインディアン合っていなくて、byteを入れ替える必要がありました。

      <<a,b>>=<<r::5,g::6,b::5>>
      <<b,a>>

r,g,bをまとめる記述とイディアン変換を一度に書けないかなぁと思ったんですが、わかりませんでした。いい書き方ご存じの方、教えてください。

defmodule Image do
  @display_width 320
  @display_height 240
  @display_pos_x 160-60
  @display_pos_y 120-60
  @width 120
  @height 120
  def line(buf,x,y,s) when x > 0, do: line([{x,y}|>rotate(s)|>get_color()|buf],x-1,y,s)
  def line(buf,x,_,_) when x == 0, do: buf

  def clear() do
    pixels = List.duplicate(<<0,0>>, @display_width*@display_height)
    data = :erlang.list_to_bitstring(pixels)
    File.open("/dev/fb1", [:binary, :write], fn file ->
      IO.binwrite(file, data)
    end)
  end
  def paint(theta) do
    theta = theta * :math.pi() / 180
    File.open("/dev/fb1", [:binary, :write], fn file ->
      draw(file, theta)
    end)
  end
  
  def draw(file,theta) do
    cos_theta = :math.cos(theta)
    sin_theta = :math.sin(theta)
    
    for y<- 0..(@height-1) do
      data = :erlang.list_to_bitstring(line([],@width,y,{cos_theta, sin_theta}))
      pixsize = 2
      offset = ((y+@display_pos_y)*@display_width+@display_pos_x)*pixsize
      :file.position(file, {:bof, offset})
      IO.binwrite(file, data)
    end
  end


  def rotate({x,y},{cos_theta, sin_theta}) do
    x0 = x - @width/2
    y0 = y - @height/2
    x1 = trunc(x0*cos_theta  - y0*sin_theta + @width/2)
    y1 = trunc(x0*sin_theta + y0*cos_theta + @height/2)
    {x1,y1}
  end

  def get_color({x,y}) do
    center_x = 60
    center_y = 60
    size_x = 80
    size_y = 80
    if abs(x-center_x) >= size_x/2 or  abs(y-center_y) >= size_y/2 do
      <<0,0>>
    else
      r = if y-center_y>0 , do: 0x1F, else: 0
      bg = div(x-center_x+div(size_x,2), div(size_x,4))
      g = if (bg == 2 or bg == 3), do: 0x3F, else: 0
      b = if (bg == 1 or bg == 3), do: 0x1F, else: 0
      <<a,b>>=<<r::5,g::6,b::5>>
      <<b,a>>
    end
  end
end

あとは実行するだけ

Roller485を初期設定して、あとは角度を取得、画像を描画を無限に繰り返してます。

handle = Roller485.open(100)
Roller485.set_position_mode(handle, 3600)
Image.clear()

demo = fn (demo, handle) ->
    x = Roller485.position_readback(handle)
    Image.paint(-x)
    demo.(demo, handle)
end

demo.(demo, handle)

無限ループするには、関数の定義が必要ですが、これには、moduleにしないといけないです。
livebookで無限ループ書くのって、ちょっと面倒なんですよね。
名前なし関数で書く事ができるんです。引数に自分自身を入れるというトリッキーな事が必要でしたが、できました。
チャットGPTに教えてもらった事は内緒です。

動いてる様子は、↓を見てください