CAN通信レーダーを使ってみる4

今回はArduino/MCP2515でデータを読み取ったCANの情報を処理して行きたいと思います。テキストで2.4mとか出しても良いですが、やはり(x,y)プロットのように位置関係を可視化していくのが良いと思うのでCAN通信を読み込む+データ表示を目標にしました。
前貼った添付画像をイメージしていますが、複数レーダーの処理に対応できるように改良して行きます。

まずは表示用ソフトとしてprocessingを使いました。C言語やArduinoは使えるので、言語も近いので使いやすいかな？と思い選定しました。

まずはシリアル通信でArduino/MCP2515からもらったIDやデータ情報を読み取ります。今回のレーダーは座標x,yと車速vx,vyがわかるようなのでこれらの情報を処理して行きます。

最終的には車の四隅にレーダーをつける想定の元、4つのレーダーからの情報をそれぞれのIDに結びつけて出力できました！
三角関数をかなりたくさん使ってます…
一部テスト用のコメントアウトもありますが無視していただければ…
丸プロットが止まってる位置を、四角プロットが移動している位置を、矢印が速度を表しています。ここまで行くと実際に動いてる物に対してある程度信憑性のある動きをするのか屋外でテストをしてみたくなります。
室内では一応それっぽい表示ができていますが広さの兼ね合い上動いてる物の向きまで正しいのかはわからず…（室内ではダミーで金属板を動かしてそれっぽく出してます。壁の乱反射？もあり上手く動いているのか不明です。）