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

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

<blockquote class="twitter-tweet" data-media-max-width="560"><p lang="ja" dir="ltr">RBoard(mruby/c)で4足歩行 <a href="https://t.co/jfpdrEgYzW">pic.twitter.com/jfpdrEgYzW</a></p>&mdash; 小倉(愛川) (@morisuteisyon) <a href="https://twitter.com/morisuteisyon/status/1362381562893922304?ref_src=twsrc%5Etfw">February 18, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script> 

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


基本的にはRBoardのドキュメントに書いてあるが失敗した点。
間違えてPIC32MX174F256Bを選択していた。
PIC32MX170F256Bと一文字しか違わないので間違えやすい。
![PIC32MX174.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/195542/1aaf948b-2311-9918-55ea-d816c78d4b8d.png)


スピードが速いと書き込めないことがあるらしいのでSettingsからLowスピードへ変更しておく。
![PIC32MX174speed.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/195542/6bd66b67-c261-ba70-102a-8520f5d389c4.png)

# SG90サーボ動作確認

:::note warn
トラブル
ファームウェアにread_voltageなんてなかった
:::
最新のmasterにはある。
そこでundefined methodが出てPWMへ初期値与えていないのでSG90が変な動きしてた。
 → レギュレータが爆熱を放って動作しなくなる。
壊れたかと思ったけどレギュレータの熱が下がったら正常に動作したので安心。


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

```ruby
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
```

<iframe width="488" height="867" src="https://www.youtube.com/embed/AA6D8rxKQik" title="SG90動作確認" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>


# 計算式

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

参考記事：

https://tajimarobotics.com/kinematics-two-link-model-2/

今回使うロボット寸法:

![20231217_195448.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/195542/5dedb93f-9eb8-670c-1f66-cb0aa8d0a065.jpeg)

# 動作

## プログラム
```ruby
$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
```

## 出力値
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/195542/4eba299f-c2d7-2620-0f7d-5a9275cec8bb.png)

## 寸法図
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/195542/32b1e32c-0a7f-d762-991d-2f05d3e236a6.png)

## 動作動画
後日。