目指せロボット -RBoardで逆運動学 2リンクロボット-

はじめに

今回は昔作ったロボットのリベンジです。
以前は逆運動学なんて無視して角度決め打ち動作で動かしていました。
mruby/cではmathクラスも用意されているので今回計算してみようと思います。
2リンクから…

RBoard(mruby/c)で4足歩行 pic.twitter.com/jfpdrEgYzW

— 小倉(愛川) (@morisuteisyon) February 18, 2021

ファームウェアアップデート

今回はmruby/c3.2対応のファームウェアを使用していく。
理由はPWMを使用するため。
初期バージョンではPWMにおけるデューティ比の指定がパーセントのみであること。
しかもパーセントはfloatではなくint…

基本的にはRBoardのドキュメントに書いてあるが失敗した点。
間違えてPIC32MX174F256Bを選択していた。
PIC32MX170F256Bと一文字しか違わないので間違えやすい。

スピードが速いと書き込めないことがあるらしいのでSettingsからLowスピードへ変更しておく。

SG90サーボ動作確認

トラブル
ファームウェアにread_voltageなんてなかった

最新のmasterにはある。
そこでundefined methodが出てPWMへ初期値与えていないのでSG90が変な動きしてた。
→ レギュレータが爆熱を放って動作しなくなる。
壊れたかと思ったけどレギュレータの熱が下がったら正常に動作したので安心。

可変抵抗とLCDをGROVE経由で搭載して動作を確認してみます。
LCDのサンプルコードはRBoardのドキュメントから引用。
このLCDは5V動作なので3.3VのRBoardではいろいろ動かない。
(クリア, 改行など結構重要なとこが動かない。致命傷)

pwm1 = PWM.new( 0 )
pwm1.frequency( 50 )
adc1 = ADC.new( 20 )
$lcd = I2C.new
sleep(1)

def rgb(r,g,b)
  $lcd.write(0x62,0x00,0x00)
  $lcd.write(0x62,0x08,0xFF)
  $lcd.write(0x62,0x01,0x20)
  $lcd.write(0x62,0x04,r)
  $lcd.write(0x62,0x03,g)
  $lcd.write(0x62,0x02,b)
end

def send_txt(txt)
  $lcd.write(0x3E,0x80,0x02)
  sleep_ms(5)
  $lcd.write(0x3E,0x80,0x0C)
  sleep_ms(5)
  cnt = txt.size
  i = 0
  while(i < cnt)
  if(i == 15)
    $lcd.write(0x3E,0x80,0xC0)
  end
  $lcd.write(0x3E,0x40,(txt[i]).ord)
  i = i+1
  end
end

rgb(255,255,255)

while(true)
  v1 = adc1.read()
  # 3.3V時パルス幅が最大1900us+500us
  x = v1 * 1900 / 3.3
  send_txt(v1.to_s[0, 3])
  pwm1.pulse_width_us( 500 + x.to_i )
end

計算式

参考記事を見ていただければ途中式も書いてあるので非常にわかりやすいです。
ただ、今回の媒体は中心点がy+の方向へずれている(24.5mm)のでその点だけ補正をします。

参考記事：

今回使うロボット寸法:

動作

プログラム

$pwm1 = PWM.new( 0 )
$pwm2 = PWM.new( 1 )
$pwm1.frequency( 50 )
$pwm2.frequency( 50 )

# 土台高さ
L1 =  24.5

# 長さ
L2 = 47.5
L3 = 80

# 初期角度
rad1 = 90
rad2 = 180

def move(x, y)
  # 目標高さ - 台の高さ
  y -= L1

  phi=Math.atan2( y, x )
  th1=Math.acos((x*x+y*y+L2*L2-L3*L3)/(2*L2*Math.sqrt(x*x+y*y))) + phi
  rad1 += 180 * th1 / Math::PI
  puts(rad1)

  th2=Math.atan2(y-L2*Math.sin(th1),x-L2*Math.cos(th1)) - th1
  rad2 += 180 * th2 / Math::PI
  puts(rad2)
  
  # 1900us
  dx = rad1 * 1900 / 180
  dy = rad2 * 1900 / 180
  $pwm1.pulse_width_us( 500 + dx.to_i )
  $pwm2.pulse_width_us( 500 + dy.to_i )
  sleep(1)
end

while(true)
  move(120,0)
  sleep(5)
  move(80,0)
  sleep(5)
end

出力値

寸法図

動作動画

後日。