設計の微調整がしやすいハニカム構造の円筒を簡単に作る

こんにちは，pebbleと申します． 9月に買ったA1 mini(3Dプリンター)を，4月に開封しました． 半年も眠っていたとは思えないほど働いてくれるのでいろいろ遊んでいます．

初心者の私が作るものといえば，四角，三角，丸で構成された簡単な形ばかりですが，円筒をそのまま印刷するとフィラメントがもったいない気がするので，肉抜きのためにハニカム円筒を作ってみたくなりました．

ちなみに，こんな感じの肉抜きも一回やってみたんですが，あまり強度があるようには見えないです．

肉抜きした円筒


軽く調べてみたのですが，ハニカム円筒作るのって結構難しくてめんどくさいんですね．．． 調べたところ，一番オーソドックスなやり方は下の記事なのかな？

何回か作りかけたのですが，六角形の大きさをころころ変えたくなる落ち着きの無い人間なので，挫折（というか放棄）していました．そもそも，初心者にとって慣れない動作が多い，，

そんな時，先輩のポスト*1が目に入り，

どうやって作ってるのか聞いた所，スケッチと押し出しだけでつくっているようでこれならできそうだと私も参考にしながら作ってみました．

最初はただコピーして作ってみようと思っていたのですが，円筒の半径と直径間違えてしまったり，ころころ設計寸法を変えたりしていたためこちらもなかなか完成せず，， スケッチ一枚で切り出せればもっと簡単に設計変更できるのになと思ったのでやってみました．

制約

設計上の制約です．(といっても後付けですが)

  • 切り抜き，押し出し，結合といった基本操作だけでつくる
  • 六角形の切り抜きはスケッチ1枚からつくる

製作

  1. ボディの一部を作る

    スケッチ
    あとで円形形状で複製するために，360°の整数分の一のボディを作ります．今回60°としましたが，あとで変更しています． 今回，外径35，厚さ3mmの円筒がほしかったので，直径35mm，29mmの円，接する二等辺三角形を書きます．
    押し出し1
    欲しい高さだけ押し出します．
    押し出し2
    六角を書くための面ができました．

  2. 六角形を並べる

切り出す六角を書きます．

六角1
1周期(青丸)の部分だけ先に並べます．六角同士の距離，底面からの距離が決まれば一意に定まるはずなので，拘束をうまく使って六角を並べていきましょう．(たまにバグって黒線にならないことがありますが，ドラッグして動かせなければ正しく拘束できていると思います．)
3Dプリントする場合，30°回ると印刷のしやすさが変わると思われるため，六角の向きに気を付けてください．どっちが良いのかは場合によると思いますが，，，

1周期できたら，ミラーで気が済むまで複製します．

六角複製
両端に六角を半分付けておいた方がきれいになる気がします．ただ，これのせいで印刷時にサポート材が必要となったりするので，見栄えと相談してください．

  1. 切り出し

六角形を切り出していきます．後でテーパーをつけることがあるため(後述)，2回に分けて切り出します．

中央切り出し
中央を切り出して，
両端切り出し
両端も切り出します．

  1. 外形を整える

六角を切り出せたら，外径を整えます．最初に書いたスケッチを再利用しましょう．(おそらくスケッチ1が非表示になっているので，表示させます．)

外形整え1
円弧からはみ出た部分を選択して，切り抜きます．
外形整え2
外形が丸くなりましたね．

  1. 複製，結合

ここまでできたら，あとは円形状パターンで複製して結合するだけです． 複製して，

円形状パターン
結合します．
結合

  1. 側面の切り抜きテーパーをつける

正直，これで完成でもいいのですが，両端の切り抜きが円筒表面と乖離しているために，歪な形になっています．

テーパー前
これは，両端の切り抜きをフィーチャー編集して，テーパーをつけてやることで多少微調整できます．
フィーチャー編集
15°くらいつけました．
テーパー後
少しマシになりましたね． まあ，正六角形には見えないですが，対称性があるのでこれも味ということで（笑）

以上で，今度こそ完成です．

設計変更してみる

完成した後で，なんだか印刷が不安になったので六角形を小さくしたくなってしまい，設計変更しました． ただ，きちんと拘束されているので，仕様変更が苦じゃありません．

まず，スケッチ1の角度を小さくします．

スケッチ1の編集
そうすると，六角形が勝手に小さくなります． ただ，高さ方向に足りなくなるので，またミラーで増やします．
六角増設
度重なる寸法変更により増えすぎています（笑） できたら，適当に切り抜き直して，複製個数も直せば修正完了です．
複製個数編集
ちまちま直す必要がなくなりましたね．

印刷

適当な台にくっつけて印刷してみました．

ペーパータオルホルダー

上端でのみサポート材が必要になってしまい，造形した質量の16%(10g)程がサポート材にすわれてしまいました．もう少し改善の余地がありそうです． 意外に形は悪くないのでは？ 歪な部分も，言われなければ気がつかないと思います． 早速ペーパータオルホルダーとしてつかってみました．

見えない部分の造形に頭を悩ませていたことに気が付いて悲しくなりますね．

綺麗に作りたい方は最初に紹介した記事が良いと思います．もっと簡単な方法知っていたら教えてください．それでは．

*1:ツイートのこと

独ステの経路追従時の収束について考える

皆さんこんにちは，pebbleと申します． 学ロボ2023/ABURobocon2023のとよはし☆ロボコンズで独ステの制御してました．

この記事はロボコン独ステ Advent Calendar 2024の7日目の記事になります． アドベントカレンダーに参加するのは実は2回目です．


前回はかいとさんの記事でした．

ロマンの塊差動ステアの特性を使ってスワップによる実用性が与えられていて非常に興味深かったです(出力もかなりありそう)． 機械から制御までされていてすごいですね．


皆さんは独ステに対してどんな思想をお持ちでしょうか． 私はまあ，独ステに対して特に思想らしい思想はありません． 速くなければ独ステじゃないので．

本記事は独ステ走行の際の一番の時間喰い虫かつトラブルメーカーである収束について書けたらなと思います．

収束するの定義

よく考えたら収束という言葉の定義を良く考えたことがありませんでした． 私はロボットの移動で，最後の目標地点に接近・到達することをおおざっぱにまとめて収束と言っている気がします．

制御器設計で言う収束と同じ意味だと思います．

概念について考えてもあまり意味が無いですが，"収束した状態"の数値的な定義が必要です． 3つ条件を考えます

  • (x,y) の偏差ベクトルの大きさ(二乗和平方根)が収束半径以下
    • 収束半径は競技に必要な精度によって決められると思います．弊ERの回収機構では \pm20\rm mmでしたが， \theta の偏差よるズレや自己位置の誤差も考えて \pm10\rm mmに設定していました．
      5\rm mmとかでもだいたい収束できていましたが，タイヤが汚れて滑ってくると稀に収束が遅くなることがありました．
  • \theta の偏差が閾値以下
    • これの閾値決めづらくて厄介ですよね．適当にロボット揺らして \pm 0.015\rm rad (0.86度くらい?)にしてました．
  • 速度(速度ベクトルの大きさ)が0に等しい・近い
    • 初めて経路追従するときに忘れがちですが，目標地点を通過してオーバーシュートする場合があるので，きちんと速度の条件も入れましょう．

収束半径と \theta の閾値を (x,y,\theta) の偏差ベクトルの大きさ(ノルム)にまとめてしまいたくなりますが，回収動作等を行う場合，分けておいたほうが都合が良いかと思います．

収束点に向かわせる

収束制御では曲率と速度が小さいのでスリップアングルはほぼ必要なく，各独ステユニットの進行方向に操舵を向けることになります． 逆に操舵角が適切に制御されていれば，必ず収束点(ゴール地点)の真上を通過します． 足回りが大きく滑っているわけではないのに収束点からズレてしまう場合，原因は

  • 走行速度によって自己位置の誤差が変化して収束点が逃げている
  • 操舵のエンコーダーがズレた
  • 操舵エンコーダーと実際の操舵の間のバックラッシが大きすぎる

あたりですかね？ 独ステの問題は機械班にきちんと伝えて喧嘩しましょう．

ロボットの目標速度・角速度から独ステの操舵角を決定できます． 独ステの逆運動学については，昨日のかいとさんの記事の "おまけ：ステアの逆運動学" に記載されてあるもので間違いないです．

スリップアングルについては収束にはほとんど必要ないので，遠心力による軌道のふくらみが問題になってから考えても遅くないと思います．

あとは偏差に適当なゲインかけるなりして，速度軌道を生成します．

うまく停止できましたか？ ブラシレスモーターのダイレクトドライブで駆動されている独ステでは足回りの微分ゲインが相当高くない限りうまく停止できないのではないでしょうか．

惰性を殺す

今流行りのダイレクトドライブの独ステユニットの場合摩擦になる部分が無いため，Pゲイン優位に設定していると速度が0に近づくにつれ出力も0に近づき制動できないのでオーバーシュートしてしまいます． (あんなに疎ましく感じていたギアヘッドのクソでか摩擦が恋しいですね．)

微分ゲインなりをゴリゴリに高めても停止できるはずですが，微分のノイズなどに起因する他の問題が起こりそうなので加速度による電流FF項で制動してやりましょう．

一次元運動では，偏差 d，速度 vに対して，一定の加速度 -\frac{v^{2}}{2d}で減速させることによって目標地点で停止させることができます．

(x,y,\theta)の3次元運動でも，速度が収束点に向いていることを仮定すれば同様に，偏差の大きさ d，速度の大きさ vに対して，大きさが -\frac{v^{2}}{2d}で速度と逆向きの加速度を与えてやると目標地点で停止させることができます．が，制動の最後で滑るとマズいので，ABU2023では -\frac{v^{2}}{2d} \cdot \alpha　 (\alpha=1.1)の大きさを持つ加速度によって制動していました(d,vは常に更新される)．

自動車教習所でも，早めに強くブレーキを踏んでから徐々にゆるめて停止するのが正しいと習いますね．


ところで収束点を向いていない任意の速度を持った物体を任意の点で停止させる軌道って最適化計算無しで求められるんですかね？ もし知ってたり思いついたりしたら教えてほしいです．

加速度の分配

加速度を力に変換して，それぞれの独ステユニットに分配しましょう． 加速度を力に変換するためにロボットの質量と慣性モーメントが必要です．

重量制限のおかげでロボットの質量計測については皆さま慣れたものだと思うのですが，慣性ってどうやって測るのが正解なんですかね？二点吊り法とか？

ABU2023では機械班にCADで出してもらった慣性モーメントに(実際の質量)/(CAD上の質量)掛けた値を使っていました．(たまに動作みて適当に調整したり)

とりあえず，ロボットの加速度，角加速度にそんなこんなで出した質量と慣性モーメントを掛けると，動作に必要な力とモーメントが出ます． これを各独ステユニットユニットに分解してやります． 力はユニット数で割ってやればいいと思います． モーメントですが，重心からの距離に比例する形で振り分けることによって回転のための力を各ユニット同じにできます．

独ステは自由度が過剰なのでいろいろな振り分け方が考えられますが，これが一番シンプルで，特定のユニットにトルクが偏りにくいのではないでしょうか．


ロボットの力とモーメントを各独ステユニットの力に振り分けた後に，駆動力(タイヤ回転方向の成分)と横力(タイヤ回転方向に垂直な成分)に分解します． ホイールモーターの横力は滑らない拘束条件でかなり供給されるので自分から何かしなくても意外に走れると思います． ホイールモーターの駆動力だけ電流値として指令してやりましょう．

これできちんと制動できるかと思います．

終わり

いかがでしたか？

考えるというタイトルにしたにも関わらず，自分の感覚をつらつら書いた記事になってしまいました． レポートもブログもいつも意気揚々と書き始めるのですが，やる気が尻すぼみになっていつもギリギリになってしまいます，，

内容よりも完成したことについて評価いただけると非常にありがたいです．


話は変わりますが，とよはし☆ロボコンズには個人スポンサー制度があります． 現在学生ロボコン3連覇中のとよはし☆ロボコンズですが，他校にくらべ非常に人の入れ替わりの激しい団体です． 今も25世代が彼らの最初で最後の学生ロボコンに向けて頑張っているようなので，支援してあげてください．


それでは．

減速無しで離散的な経路の追従をする

こんにちは，pebbleです． とよはし☆ロボコンズに3年ちょっといました． (個人スポンサーになればとよはし☆ロボコンズのマル秘情報が見られるかも．．？)


この記事は学ロボ Advent Calendar 2024 - Adventarの6日目の記事になります． アドベントカレンダーに参加するのは初めてです．

昨日の記事はソフトウェアのオールラウンダーかつ最強のロボコン同期@eyrin君でした．

昨年の新入生教育で僕は新入生の能力に頼り切りでロボコンの楽しさを伝えきれなかったのですが，今年はプログラムを触ったことが無い子でも動かして楽しめるように考えられていてすごいなあと思います．

(卒業研究のプログラムのマルチタスク化で一昨日くらいまで沼っていたのでRTOSの勉強もすればよかったな，，)


高専ロボコン2019の近畿地区大会では自動機のプログラム担当してました． その時の経路追従の話を書いておこうかなと思います． ABU2023では走行や射出を主に担当していました． ABU2023で優勝した豊橋のERも似たようなアルゴリズムで走らせてます． 学ロボのアドベントカレンダーなので，最後にちょっとその話もします． (というか，その話をするための長い前置きです．)


自動走行させたことのある人は分かると思うのですが，経路追従は動き出しの加速と停止するときの減速が一番時間を喰っている感じがします． 制御で走行タイムを縮めるには，走行途中で停止する動作を無くすのが第一歩かなと思っています．

ちなみに，こんなロボットでした．

予選ルールの満点を出すことはできたものの，1回戦で敗北してしまったため地区の予選リーグで敗退しました．

交流ロボコン等でPure pursuitアルゴリズムで経路追従をしている高専や大学をよく見かけます． Pure pursuitアルゴリズムは経路の最適性や制約条件をぼかしてしまっているので私はあまり好きではないですが，強ければなんでも良いと思います．

経路追従

位置PID制御にフィードフォワード項(以降FF項)を足していました．

narajin_senzu
以上です．

電圧，電圧指令と書いていますが，実際には電圧の次元の要素を (x, y, \theta) に持つベクトルです．

FF項は経由点毎に持たせています．

I制御は切ってあったので実際はPD制御ですね． 非常停止解除後に暴走する可能性があるのでI制御は強めの出力制限を設けるか，使わないのが吉と思っています．

経由点から離れているときはPID制御が優位(PID制御器の出力が大きい状態)になります． 経由点に近づくとPID制御器の出力が小さくなり，FF項優位になってFF項の示すベクトルの向きへ進みます．(この時はFF項の示すベクトルの向きは前の経由点から目標点の延長線上，つまり通ってきた経路の延長線上にしてました．)

1本の経路に対して経由点が10個程度のカクカクした経路なのですが，惰性で軌道が膨らんでいい感じになるとかならないとか，，

線形なので x, y, \theta それぞれに対して分解して計算することができます． PID+FF項(電圧の次元の大きさと向きを持つベクトル)をオムニの向きに成分分解して足のモーターの指令電圧にしてます．

本題はこれで終わりなんですが，しょうもなすぎるのでついでに他の話もしようと思います．

経由点の選択，通過の判定

経路は複数の経由点からできています． はじめの経由点を通過すると，目標位置を次の経由点へ進めます．

keiro_image

(前の経由点から次の経由点のベクトル)と(前の経由点からロボットへのベクトル) の内積が正で通過と判定し，目標経由点を次に進めます．

tuuka_hantei

シンプルですが，これ以上必要無いですね． 最後の経由点(終着点)では通過の判定ではなく別で収束の判定を行っています．

余談ですが当時高専1年生で，2回目の留年確定により退学も確定していたので，ロボコンとオンラインゲームに明け暮れていました． 当然勉強なんてしておらず内積なんて知り得ないので実装はこれと等価ですが汚いです．

また余談ですが，学ロボ2023ルール発表前の開発*1では経路に対して数百の経由点があり，経由点同士が近すぎたので目標速度ベクトルとの内積によって判定していました．

走行中の姿勢の決定

このロボットですが，スタート直後に頭を振って走行するのが特徴的かなと思います．

速度の出るロボットの前後左右いずれかの向きが，経路の直線と平行になるように姿勢制御してます． そのせいで，頭を振るような動きをしています．

omuni_hayaimuki

これによって，全てのオムニにほぼ最大電圧をかけてマシンスペック上での最高速度に近い速度で走行することができます． ロボットの重量に対してあまり強くないモーター(マブチモーターさんの555とかだっけ？)が使われていたので，この制御でいくらか速くなったと記憶しています．(何秒早くなったかは記憶していませんが何秒か早くなりました．)

PIDゲインの切り分け

高専ロボコン2019の競技テーマが洗濯物干しだったのですが，シーツを掛ける動作でシーツが引っかかってロボットが引っ張られ，経路からずれてしまう問題がありました． 同点の場合干された洗濯物の美しさ(とかだったかな？)で勝敗が決まるので，ずれによるシーツのしわ対策のために偏差を経路と平行，垂直方向に分解して，それぞれのパラメーターを持たせてPIDをかけていました．

pid_bunkai
経路に垂直方向のゲインが高いので，走行中経路直線から大きく外れることはありませんでした．

ABU2023(チェイヨー) 豊橋ER

豊橋のABU2023のERも似たような感じのシステムで走らせてました． ただ，電圧FF項，電圧指令の代わりに速度FF項，速度指令があります．

abu_senzu
こちらもPID制御器が組んでありますが，P制御以外はゲインを0にして切ってあります．十分な制御周期の中で速度のPゲインが十分高ければオーバーシュートは殆ど発生しません．

速度指令でswervedriveの操舵角度，加速度指令で操舵角速度とホイールモーターのトルクを決定して走行します．(swervedriveの操舵角度はロボットの現在速度から決定されるべきで，しばらく現在速度を使用していましたが，低速走行時のがたつきと接地エンコーダーのトラブル時の暴走の回避を考慮すると速度指令を用いるほうが都合が良い，という判断になりました)

経由点は位置・速度・加速度目標の情報を持ちます． 経由点の選択はロボットに最も近い点が使われていました．


学ロボ2023ルール発表前の開発の時は速度目標ベクトルと位置の偏差の内積によって目標経由点を進めており，経路が自己交差していても問題なく走行できる，経由点を進めていくので探索が発生しないなどの利点があります． が，数千程度の経由点の探索の計算時間なんて知れてるし自己交差している経路を走ることなんてまずないので実用的に違いは無く放置していました．

また，ABU2022豊橋の先輩方のロボットは連続した経路に追従していたので離散的な経路を持たせるのは改悪なのですが，どのみち離散時間での制御を行っているので連続した経路を細かく分割して経由点を十分多く取ればこちらも実用的に違いありません．


ERの曲率が高い経路に対する追従は学外の人からすると結構レア動作かもしれません． 動画を探したのですが，走行試験する時は限界までタイヤが汚れてから行っていたので滑ってる動画しか持ってませんでした．

www.youtube.com

NHKのこれが一番見やすい動画なのかなあ，，ERが5秒くらい走ってますね． NHKフィールドのグリップが高くて大会中ずっと振動しながら走行してます． とはいえ離散的な経路であることを感じさせない程度には割と滑らかに走れていると思います． やっぱり曲率があると経路追従っぽさが増しますね．

言い訳ですが世界大会仕様ではもう少し加速度が上がってます．誰に対しての言い訳なのか分かりませんが，，

おわり

だいぶ昔のことを書いてしまいました．勝っていないロボットのことを長々書くのもなと思ったのですが，ABU2023の制御のベースになっていた部分も多いロボットなので書いてみました．

おかしいところとか理解不能なところでもし興味があれば私のXか何かにdmで質問いただければ有難いです．この記事のコメントはめったに見ないと思います．

そんなに珍しいことをしている訳ではないと思うのですが，2019年以降の交流会とかに特に参加していないので他チームの実情がよく分かっていません．


ABUロボコン2023豊橋の某チームメンバーが，「足回りが動かないのは競技進行以前の問題で論外」，「なんで毎年やってることができないの？」などと随分勝手なことを言ってくれていたのですが，実際その通りだと思います．経路追従のような使いまわせる部分の開発は早めに終わらせて，ルールやロボット固有の問題に開発リソースを割かなければいけません．(解っていても楽しいので時間を割いてしまいますが，，)

この記事がどれくらい役に立つかは知りませんが，足回りの事故や不調によって学ロボの予選リーグで機能を見せられないまま敗退してしまうチームが少しでも減れば良いなという願いがこめられていたり，，するかも？

明日はロボコン独ステ Advent Calendar 2024に投稿させていただく予定です． まだ記事かけてないので今日のより酷いかもしれませんが，ご容赦ください．

それでは．

*1:この時期に豊橋が何かしらの交流ロボコンで大敗したという噂があるそうです．

esp32でロボマスモーター(M2006)を回しました．

初めまして，pebbleです．

最近，ロボマスモーターがロボコン界隈で流行っているようで，卒業研究で使いたかったのでesp32を使って回してみました．

esp32，今までまともに使ったことはなかったのですが，stm32に比べて検索した際の情報量が多くて，簡単にbluetoothが使えてワイヤレスでのデバッグもやりやすいので便利かなと思って選びました． ロボコンでのメイン基板やROS-アクチュエーター間の通信窓口としてならばstm32 H7等と比べてそれほどかさばるものでもないので，ある程度使える気がします．知らんけど

準備

買ったもの

  • esp32
    • wroom32を買ったつもりがwrover32を買っていました．(どちらでもできました．)
  • mcp2562 FD
    • キャントランシーバー(と多分終端抵抗)をつながないとちゃんと送信エラーで停止してくれるので，テストするときもつなぎましょう．
  • ユニバーサル基盤
  • M2006(ロボマスモーター)
  • C610(ESC)

回路図

ちゃんとした回路図作るのが面倒になったのでkicadのグリッドを使ったユニバーサル基盤用の下書きを載せておきます．

回路の図
他のブログの記事でも見られるような普通の接続ですね．

終端抵抗忘れないようにしましょう．(C610についているスライドスイッチをONにすれば十分です．) STBYピンはちゃんとGNDに落とします． たまに聞かれるのですが，CANトランシーバーは別に受信機ではないので，TxはマイコンのTxと，RxはマイコンのRxとつなげばいいです．

送信テスト

Arduino CANというライブラリを使用してサンプルプログラムで送信テストしました． ライブラリマネージャーにいろいろあって，特にパスの設定もしなくて良いのでありがたいですね． ループが回っていれば送信はできていると思います．

ロボマスモーターが1ms毎にデータを送信してくるせいか受信用のサンプルプログラムは落ちてしまいました．(まあ，送信ができればできるでしょ)

CAN.beginの前にCAN.setPins(rx_pin, tx_pin);を加えて通信用のピンをいじれます． 34番ピンを使う予定で基盤を作ってしまったのですが，34,35番ピンが読み取り専用でTxピンに使えませんでした(確認不足) 今回rx pinに19番，tx pinに32番を使用しました．

ボード選択でesp32 wrover module(か，esp32 dev module)を選択しました．

コンパイルエラーが出たり，書き込み時にフラッシュ関連のエラーが出たりしていたのですが，ライブラリを入れたり消したりArduino IDEを入れたり消したりしているとなぜか上手くいきました． 最終的に落ち着いたボードライブラリとライブラリのバージョンをメモしておきます．

  • esp32 by Espressif v2.0.17
    • v3以降だと'esp_intr.h'が無いようで，CANのライブラリを読み込んだ時にコンパイルエラーが出ました．
  • CAN by Sandeep Mistry v0.3.1

ボーレートが半分になるトラブル

新しいesp32で？ボーレートが半分になるトラブルがあるようです． オシロスコープで見ると，信号が時間軸方向へ倍延びてました． 分周周りの設定とかTimeSegmentとかいじってなんとかしようとしたのですが，奇数パルスで1bit送ってるようで無理でした．

優秀な友人に教えてもらったのですが，issueが立っていました．

github.com

Following the thread about the hardware limitation (https://esp32.com/viewtopic.php?t=2142) just recently I read that in Rev2 or up versions of the chip they changed the meaning of bit 4 in IER, from "Wake-up interrupt" to "divide BRP by 2". That explains why the CAN ran in half speed (for some reason bit 4 was set) in newer ESP32.

外国の言葉が書いてあって分かりませんね． とりあえず，Arduino-CANライブラリのESP32SJA1000.cppからbegin()関数を探し出して，修正します．

// writeRegister(REG_IER, 0xFF);   // enable all interrupts
writeRegister(REG_IER, 0b11101111);   // fixed

16進数が分からず，毎回レジスタに書き込む値は10進数か2進数で書いてしまいます．

プログラム

ロボマスモーターを回して，速度とケツのローターの角度を取ってくるプログラムです． 綺麗に，汎用的にクラス化して書こうと思っていたのですが，Arduino IDEを使い慣れておらず，VSCodeのセットアップするのも面倒になって適当に書いてしまいました． githubに上げるまでもないのでここに．

#include <CAN.h>
//#include "c610.hpp"

float vel = 0;
float pos = 0;

int counter = 0;
bool data_size_error_flag_ = false;


constexpr int motor_id = 2;

void send_cur(float cur) {
  constexpr float MAX_CUR = 10;
  constexpr int MAX_CUR_VAL = 10000;

  float val = cur * (MAX_CUR_VAL / MAX_CUR);
  if (val < -MAX_CUR_VAL) val = -MAX_CUR_VAL;
  else if (val > MAX_CUR_VAL) val = MAX_CUR_VAL;
  int16_t transmit_val = val;

  uint8_t send_data[8] = {};

  send_data[(motor_id - 1) * 2] = (transmit_val >> 8) & 0xFF;
  send_data[(motor_id - 1) * 2 + 1] = transmit_val & 0xFF;
  CAN.beginPacket(0x200);
  CAN.write(send_data, 8);
  CAN.endPacket();
}

void can_callback(int packetSize) {
  static std::array<uint8_t, 8> received_data;
  if (CAN.packetId() == 0x200 + motor_id) {
    counter++;
    int data_size_counter = 0;
    while (CAN.available()) {
      received_data[data_size_counter % 8] = CAN.read();
      data_size_counter++;
    }
    data_size_error_flag_ = (data_size_counter != 8);  // 8byte以上のデータが溜まってないか一応チェック

    pos = static_cast<int16_t>((received_data[0] << 8) + received_data[1]) / 8192.; //0~1に
    vel = static_cast<int16_t>((received_data[2] << 8) + received_data[3]) / 60.;//rpmをrpsに
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  while (!Serial)
    ;

  CAN.setPins(19, 32);
  if (!CAN.begin(1000E3)) {
    Serial.println("Starting CAN failed!");
    while (1)
      ;
  }

  CAN.onReceive(can_callback);
}

void loop() {
  int i = 0;
  for (float cur = -0.5; cur < 0.5; cur += 0.01) {
    send_cur(cur);
    delay(10);
    i++;
    if (!(i % 10)) { // シリアルプロッタを伸ばしすぎないために1/10に制限
      Serial.print("pos:");
      Serial.println(pos);
      Serial.print("vel:");
      Serial.println(vel);
    }
  }
  for (float cur = 0.5; cur > -0.5; cur -= 0.01) {
    send_cur(cur);
    delay(10);
    i++;
    if (!(i % 10)) { // シリアルプロッタを伸ばしすぎないために1/10に制限
      Serial.print("pos:");
      Serial.println(pos);
      Serial.print("vel:");
      Serial.println(vel);
    }
  }

  /*
  //デバッグ
  Serial.print("counter:");
  Serial.print(counter);
  Serial.print(" error:");
  Serial.println(data_size_error_flag_);
  */
}

ほんで，読めてました． 静止摩擦の影響が見て取れる良いグラフですね．

シリアルプロッタ

canのcallback関数内ではどれくらいの処理ができるんでしょうね． 1ms毎にC610からのフィードバックが返ってくるので，Serial.printなんかは避けたいです．

おわり

基板作成含めて2日程度で終わるとおもっていたのですが，結構スケジュールを押してしまいました． 見切り発車で基板を作るのではなく，一度ブレッドボード上で試せばよかったなと毎回思います．

そういえば，初めてブログを完成させました． 読みやすいように真面目に書こうと思っていたのですが，途中で面倒になって気づけばブログとしても備忘録としても微妙になっていました．

それでは．