Arduino, Bluetooth, MATLABを使って足裏荷重センシング

#前置き
大学の授業[^1]の中で作った，足裏荷重センシングシステムの作り方を記しておきます．
特別難しいことをしているのではなく，センサー値をBluetooth経由で送信して可視化するということを行なっているだけなので，色々なものに応用が効くと思って記事にしました．
Bluetooth経由でMATLABにデータを送信する方法は[この記事](https://qiita.com/ketaro-m/items/108a5244068ef12e6fcb)に書いたので，こちらも参考にしていただけると嬉しいです．

#作ったもの
運動中の足裏荷重分布をリアルタイムで計測・可視化するウェアラブルセンシングデバイス
コードは[GitHub](https://github.com/ketaro-m/FootSens)に載せてあります．

**ポスター**
<img width="400" alt="Group 12 MEC178 Poster.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/2b2ceaa7-448e-db09-0ff4-d70fecaa041c.png">

**デモ**
<img width="500" alt="Group 12 MEC178 Poster.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/06850498-9075-6a6c-8ac6-c6d45d35800b.gif">




#材料・環境

- [Arduino UNO R3](https://www.amazon.com/Arduino-Uno-R3-Microcontroller-A000066/dp/B008GRTSV6/ref=sr_1_4?ie=UTF8&qid=1532009862&sr=8-4&keywords=%E2%80%A2%09Arduino+Uno++usb&dpID=61GN8biQEHL&preST=_SX300_QL70_&dpSrc=srch)
- [Bluetooth Module HC-05](https://www.amazon.com/gp/product/B01G9KSAF6/ref=ppx_yo_dt_b_asin_title_o01_s00?ie=UTF8&psc=1) ←MATLABがサポートしているのは"HC-05"と"HC-06"だけです．([参照](https://jp.mathworks.com/help/supportpkg/arduinoio/ug/arduino-bluetooth-setup-issues.html))
- [Force Sensing Resistor](https://www.amazon.com/gp/product/B00B887CLS/ref=ppx_yo_dt_b_asin_title_o03_s00?ie=UTF8&psc=1)
- 10kΩ抵抗 (FSR用)
- LED
- 150Ω抵抗 (LED用)
- MATLAB R2019a ([Instrument Control Toolbox](https://jp.mathworks.com/products/instrument.html)インストール済み)←MacでもWindowsでも可

#ハードウェア
##回路
回路は以下のように組みます．FSRは可変抵抗になっているので，10kΩ抵抗と組み合わせて，中間電位をアナログピンで測るように回路を組みます．詳しくは[このサイト](https://learn.adafruit.com/force-sensitive-resistor-fsr/using-an-fsr)に書いてあります．
<img width="800" alt="Screen Shot 2019-12-08 at 20.01.10.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/ec496a75-7ba1-a547-c857-7e712f0dcfe6.png">
##プロトタイプ
上の回路をまとめつつ，Bluetooth ModuleやFSRからのワイヤを差し込めるような部品を作りました．Arduinoにそのままはめることができるようになっています．
ガラクタの寄せ集め感満載ですが，とりあえずプロトタイプとしては許されるでしょう．一応、Arduinoの形状を考慮しながら，極力コンパクトに収まるようにはんだ付けしました．今回はまだセンサーが二つですが，もっと増やそうと思うとちゃんとプリント基板などを作る必要が出てくると思います．
<img width="400" alt="スクリーンショット 2019-12-02 15.42.26.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/1813de05-9048-8e80-ba94-95de122692d9.png">
FSRを靴下の中に縫い付け，デバイスも足首の周りにマジックテープで取り付けられるようにしました．
<img width="350" alt="スクリーンショット 2019-11-19 12.01.15.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/096d18ce-bfd2-99e6-b1b1-7a928e1cc8df.gif"> $\hspace{5mm}$<img width="300" alt="prototype2.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/b6199f34-0e7c-539e-0fb0-6ee3d2fa5191.png">
#ソフトウェア
##Arduino側のコード(荷重のセンシングとデータの送信)
Arduino側のコードは以下のようにします．なるべく処理を軽くしたいので，無駄な計算はさせずにアナログ入力値をそのまま送信する形にしました．
また，計測をしている間にLEDが光るようなオプションも入れています．

```c++:ForceSensor.ino
#include <SoftwareSerial.h>

#define N 2 //pin number
int TxD;
int RxD;
int data;
int fsrPin[N] = {0,1};     //the pins which the FSR and 10K pulldown are connected to
int fsrReading[N];     // the analog reading from the FSR resistor divider
SoftwareSerial bluetooth(TxD, RxD);

void setup() {
  Serial.begin(9600); 
  bluetooth.begin(9600);
}

void loop(){
  if(bluetooth.available() > 0){
    data = bluetooth.read();
    for(int i=0;i<N;i++){
      fsrReading[i] = analogRead(fsrPin[i]);
      Serial.println(fsrReading[i]);     // the raw analog reading
    }
    if(data == 1) {         
      digitalWrite(11, HIGH);
    }else if(data == 0){
      digitalWrite(11, LOW); 
    }
  }
}
```
ちなみに，**Arduinoにコードを書き込むときは，TXDとRXDの線は抜く**ようにしてください．接続された状態で書き込もうとするとエラーが出ます．(このしょうもないエラーに何十分格闘したことか$\cdots$)

##MATLAB側のコード(データの受信と可視化)
###データの受信
まずは，bluetoothで送られてきたデータを読み取る関数を作成します．(通信の詳しい仕組みについては[こちらの記事](https://qiita.com/ketaro-m/items/108a5244068ef12e6fcb)を参考にしてください．）
[このサイト](https://qiita.com/nyagato_00/items/2937a11630109a3db6e1)にあるように，Arduinoのアナログピンで取得されたデータは0~1023の整数値になっています．このサイトに則ってArduino側でこれを電圧値に補正してもよかったのですが，電圧値をさらに圧力値に変換しなければいけないことも鑑みてMATLAB側で補正することにしました．

####電圧値と荷重値の関係
先に書いた通り，FSRは可変抵抗であって荷重値をそのまま出力してくれるわけではありません．また，もちろん荷重値と電圧値は線形な関係にあるわけではないので，それらの関係を表す関数を見つけなくてはいけません．
とは言っても，これと言った関数が決まっているわけではないので，実験から得られたデータから近似曲線を見つけて，それを採用することにします．
[オフィシャルな製品表](https://www.sparkfun.com/datasheets/Sensors/Pressure/fsrguide.pdf)には，以下のようなグラフが載っているのですが，これはセンサーにそのまま力が加わったときの値であり，今回のように靴下の素材や荷重を和らげるクッション材を挟んだ場合は値が変わる可能性があります．
<img width="500" alt="fig2.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/8434e700-8d54-ad22-9e6a-11bb5a3be967.png">
よって，このデータは参考にしつつ，実際に自分たちで計測したデータを用いることにします．(教授にも，自分たちでテストをする重要性を何度も強調されました．)
実際に得られたデータと，対数でフィッティングした関数は以下のようです．上の製品表のデータ値と照らし合わせても違和感がないので，妥当な結果と言えるでしょう．
<img width="500" alt="fig3.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/9ad9b999-7cd3-984b-c0ad-0a029fe709f1.png">
$$V = 0.7687\log F -2.0269$$
この逆関数は次のようになるので，センサー値(中間電位)を荷重値に変換するためには，この関数を挟めばいいことになります．
$$F = \exp \biggl(\frac{V+2.0269}{0.7687}\biggr)$$
念のため，この関数のグラフも書いておきましょう．

<img width="250" alt="fig4.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/b375705d-3ec7-4821-3ac7-b0fce71231d7.png">
多少怪しい気もしますが，ある程度フィッティングできていることが確認できました．よってこの関数を採用することにします．

```matlab:get_value.m
function v = get_value(bt,pin_num)
% this function returns the velues sent from bluetooth
v=zeros(pin_num,1);
flushinput(bt); %remove data from input buffer
fprintf(bt,1); 
for i=1:pin_num
    v(i) = fscanf(bt,'%d');
    v(i) = (v(i)*5)/1023; %convert analog input 0~1023 to 0~5
    v(i) = exp((v(i)+1.2261)/0.6591)/1000; %convert voltage[V] to pressure[kg]
end
```

###ヒートマップの作成
荷重値のグラフだけでなく，視覚的にわかりやすいような荷重分布のヒートマップも作ることにします．完成イメージは以下の画像の左画面です．
<img height="300" alt="fig4.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/92f21433-212b-dfbe-ae1f-495d224703bc.png">


MATLABには[surf()](https://www.mathworks.com/help/matlab/ref/surf.html)という便利な関数があるので，それを利用することにします．
センサーからの荷重の広がりに関しては，$x,y$方向に等しい広がりを持った2次元ガウス分布をモデルとして採用することにします．(この分布に正解はないのですが，ガウシアンが一番妥当で見栄えもそれっぽくできます．)

というわけで，まずは「標準偏差$\sigma$，中心座標を入力すると，そこから広がる2次元ガウス分布を出力する関数」を作成しましょう．
等分散の2次元ガウス分布は以下のような関数でかけるので，それをそのまま書くだけです．
$$Z(x,y) = \frac{1}{2\pi\sigma^2} \exp \biggl(-\frac{(x-x_c)^2+(y-y_c)^2}{2\sigma} \biggr)$$


```matlab:gaussC.m
function val = gaussC(x, y, sigma, center)
xc = center(1);
yc = center(2);
exponent = ((x-xc).^2 + (y-yc).^2)./(2*sigma);
val       = (exp(-exponent));
```
この関数では，ピークの値を1になるように係数$\frac{1}{2\pi\sigma^2}$を消しています．

次に，この関数を使いながら，それぞれのセンサーのヒートマップを重ね合わせることで全体のヒートマップを作成する関数を作ります．

```matlab:loadMap.m
function Z = loadMap(X,Y,weight,position,sigma,ROI)
if length(weight)~=size(position,1)
    msg = 'Invalid input size.';
    error(msg)
end
tmp = gaussC(X,Y,sigma,position(1,:));
for i=2:length(weight)
    tmp = cat(3,tmp,gaussC(X,Y,sigma,position(i,:)));
end
tmp = tmp.*reshape(weight,[1,1,length(weight)]);
Z = sum(tmp,3);
Z = Z.*flipud(ROI);
```
最後の一行で，以下のようなROI画像を掛け合わせることでヒートマップの形を作っています．(この画像は，"footROI.mat"という名前で同じディレクトリに保存しています．)
<img height="250" alt="fig4.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/aa124711-0ff4-eba8-9bd4-23af49fc80a2.png">
これで，複数のセンサー値と座標から足裏荷重ヒートマップを作成する関数が完成しました．

###メイン関数
最後に，これらを統合して処理をまとめたメイン関数を作ります．
大まかな処理の流れは以下のようになっています．

1. センサーのピン番号や位置などの，初期パラメータの設定
2. 保存ファイル名と，計測時間のインプットダイアログの表示
3. モニター画面の初期化
4. センサー値の受信 & モニター表示
5. aviファイルへの書き込み

```matlab:main.m
%Input parameters
pin = {'A0','A1'};              %pin numbers
position = [-1,4.5;0.2,-6];      %pins' positions
sigma = 1.5;                    %pressure dispersion
monitorRange = 10;              %time range of the monitor [sec]
%

load('footROI');
prompt = {'Save file name (.avi):','Measurement time [sec]:'};
dlgtitle = 'Input';
dims = [1 50];
definput = {'Gait Cycle','20'};
answer = inputdlg(prompt,dlgtitle,dims,definput);
fname = answer{1};
interv = str2double(answer{2});

if strcmp(bt.Status,'closed')
    fopen(bt);
end
fprintf(bt,0);
x = -5:0.1:5;
y = -10:0.1:10;
[X,Y] = meshgrid(x,y);
n = length(pin);
data = cell(n,1);
figure('units','normalized','outerposition',[.1 .05 .6 .9]);
[ax_surf,ax] = monitor(pin);
frame = 1;
time = 0;
clear F
pause(3)
tic;
while(time<=interv)
    weight = get_value(bt,length(pin)); %receive data
    surf(ax_surf,X,Y,loadMap(X,Y,weight,position,sigma,ROI),'FaceAlpha',0.5)
    colormap(ax_surf,jet)
    colorbar(ax_surf)
    xticks(ax_surf,[])
    yticks(ax_surf,[])
    caxis(ax_surf,[0,10])
    zlim(ax_surf,[0,10])
    view(ax_surf,2)
    title(ax_surf,'Heat Map','Interpreter','latex')
    for i=1:n
        data{i} = [data{i},weight(i)];
        plot(ax{i},data{i})
        ylim(ax{i},[0,10])
        title(ax{i},pin{i},'Interpreter','latex')
        xlabel(ax{i},'time [sec]','Interpreter','latex');
        ylabel(ax{i},'Force [kg]','Interpreter','latex');
        yticks(ax{i},linspace(0,10,11))
        xticks(ax{i},linspace(0,fix(time)*frame/time,fix(time)+1))
        xticklabels(ax{i},ax{i}.XTick*time/frame)
        xlim(ax{i},[frame-frame/time*monitorRange frame])
        ax{i}.YGrid = 'on';
    end
    drawnow
    F(frame) = getframe(gcf);
    frame = frame+1;
    time = toc;
end
fprintf(bt,0);
fclose(bt);
video = VideoWriter(fname,'Uncompressed AVI');
video.FrameRate = frame/time;
open(video)
writeVideo(video,F)
close(video)
```
モニターの横軸を時間表示にしたり，書き込むaviファイルを正しいフレームレートにするためには，サンプリング周波数を算出する必要があります．
MATLABには計測時間を測る便利な関数があるので，それを上手く使うことでサンプリング周波数を算出できます．

ちなみに，モニターfigureのsubplotは，センサーの数を変えても正しく配置されるように以下のような関数にまとめました．

```matlab:monitor.m
function [ax_surf,ax] = monitor(pin)
% create subplot
n = length(pin);
tmp=[];
for i=1:n
    for j=1:fix(n/2)
        tmp = [tmp,(i-1)*(fix(n/2)+1)+j];
    end
end
ax_surf = subplot(n,fix(n/2)+1,tmp);
title(ax_surf,'Heat Map','Interpreter','latex')
xticks(ax_surf,[])
yticks(ax_surf,[])
for i=1:n
    ax{i} = subplot(n,fix(n/2)+1,(fix(n/2)+1)*i);
    title(ax{i},pin{i},'Interpreter','latex')
    xticks(ax{i},[])
    yticks(ax{i},[])
end
```

#実行
これで全てが完成しました．
最後に，実行の手順を書き記します．
##初期設定
まず一番初めに，PC自体にBluetoothデバイスを登録する必要があります．
その際にPIN番号を登録しなければいけないのですが，この製品のデフォルトは"1234"となっています．(多少異なる可能性があるので，購入したBlutooth Moduleの説明書を確認しましょう．)
<img width="400" alt="スクリーンショット 2019-12-02 15.48.28.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/7db05a12-2a88-9597-9d14-520e9c9e7c13.png">
PC本体への登録は一回してしまえば繰り返す必要はありません．

次に，MATLABを立ち上げたらコマンドラインで以下のコマンドを実行します．(objectの名前'DSD TECH HC-05'はデバイスによって微妙に異なっていたりするので，正しく打ち込みましょう．一行目はそれを確認するためのコマンドです．)

```matlab

>> instrhwinfo('Bluetooth','DSD TECH HC-05');
>> bt = Bluetooth('DSD TECH HC-05', 1);
>> fopen(bt);
```
エラーなくオブジェクトが開けることを確認しましょう．

##計測開始
ここまでできれば，あとは計測に移るのみです．
main.mのあるディレクトリに移動し，

```matlab
>> main
```
と入力してメイン関数を実行します．

初めに次のような入力画面が出てくるので，保存したいファイル名と計測時間[sec]を入力し，OKをクリックします．
<img width="300" alt="Screen Shot 2019-12-08 at 22.12.18.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/0f9453e5-d2c9-7194-3092-5c01bdf05adc.png">
その後，約1秒ほど経ってLEDが光ったら計測開始です．
計測中はリアルタイムでデータのグラフやヒートマップが出るので，荷重分布の遷移を眺めることができます．
計測が終わるとLEDが消滅し，同じディレクトリに計測モニターを保存したaviファイルが保存されます．また，計測の実データやフレームレートもワークスペースに保存されているので，後の解析に使うことができます．

**実行画面**
<img width="600" alt="demo.gif" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/505262/31ee205e-98d5-f615-a16e-a843d6219071.gif">

#改善点
現状はセンサーを二つ使っていますが，より正確な荷重分布を調べるためには，より多くのセンサーを配置する必要があります．その際には，ワイヤーの配置も含めArduinoにどう繋ぐかをもう少し検討する必要がありそうです．
また，そもそもArduinoである必要があるのかという問題もあります．現状ではただセンサー値をBluetooth Moduleを介して送信しているだけの処理なので，それに特価したような小型のマイコンを使えばもっと小型化・軽量化ができるでしょう．
そして，現状の最大の課題はフレームレートです．このプロトタイプではフレームレート約2.5fpsとなっていて，例えばランニング中の荷重推移を測ろうと思ったら全く使い物になりません．Arduinoからの値の送信は高々整数値2つを送っている程度なので，問題はソフトウェア側にあると思われます．また，Bluetooth Moduleを使わずにUSBコネクタを使ってデータ送信をした場合は滑らかな波形が得られるので，グラフ描写に時間がかかっているわけではなく，データの受け取りの際にかかっている時間が大きいのだと考えられます．実際，pythonにあるシリアル通信のライブラリを使えば，もう少し早くできそうです．

このように改善点はまだまだたくさんありますが，プロトタイプとしてはまずまずかなと思います．

#参考サイト
- [DSD TESH Official website](http://www.dsdtech-global.com/search/label/HC-05)
- [Force Sensing Resistor Integration Guide and Evaluation Parts Catalog](https://www.sparkfun.com/datasheets/Sensors/Pressure/fsrguide.pdf)
- [Using an FSR](https://learn.adafruit.com/force-sensitive-resistor-fsr/using-an-fsr)
- [Force Sensing Resistor (FSR) with Arduino Tutorial](https://www.makerguides.com/fsr-arduino-tutorial/)
- [How to use Bluetooth with MATLAB for Wireless Communication](https://circuitdigest.com/microcontroller-projects/how-to-use-bluetooth-with-matlab-for-wireless-communication##targetText=Here%2C%20the%20MATLAB%20uses%20the,anything%20connected%20to%20the%20Arduino.)
- [Plotting Live Data of a Temperature Sensor using Arduino and MATLAB](https://jp.mathworks.com/videos/plotting-live-data-of-a-temperature-sensor-using-arduino-and-matlab-121317.html)
- [Arduino -> MATLABのBluetooth送受信の仕組み](https://qiita.com/ketaro-m/items/108a5244068ef12e6fcb)


[^1]: 企業をクライアントにしたプロジェクト(僕のグループはあるラボがクライアントでしたが)なので，もしかしたらこのように記事にすることに問題があるかもしれません．ただ，この手の製品は世に余るほど出回っているので，オープンソースにしたところで問題ないと個人的に思っています．もし怒られたらこの記事は削除します．(日本語で書いてあるのは教授に読まれないようにするためでもあります．)