【MATLAB】MATLAB Support Package for Raspberry PiでSunFounderを動かす

# はじめに
[MATLAB Support Package for Raspberry Pi Hardware](https://jp.mathworks.com/matlabcentral/fileexchange/45145-matlab-support-package-for-raspberry-pi-hardware "MATLAB Support Package for Raspberry Pi Hardware")を使用すると、MATLABを実行しているコンピューターとRaspberry Piとの通信を簡単に行うことができます。
例えば、Raspberry Piボード上のGPIOやI2CをMATLABから制御することが可能です。
本記事では、[MATLAB Support Package for Raspberry Pi Hardware](https://jp.mathworks.com/matlabcentral/fileexchange/45145-matlab-support-package-for-raspberry-pi-hardware "MATLAB Support Package for Raspberry Pi Hardware")の使用例として、MATLABからロボットカー学習キットの[SunFounder](https://www.sunfounder.com/products/smart-video-car "SunFounder")(Smart Video Car Kit V2.0 for Raspberry Pi)を動かしてみた例について紹介します。

# 想定読者
MATLAB初級者から中級者レベルで、MATLAB Support Package for Raspberry Pi HardwareでのI2C通信処理に興味のある方を想定しています。

# 開発環境
OS: Windows 10
MATLAB: R2021a Update 5 ([MATLAB Support Package for Raspberry Pi Hardware](https://jp.mathworks.com/matlabcentral/fileexchange/45145-matlab-support-package-for-raspberry-pi-hardware "MATLAB Support Package for Raspberry Pi Hardware") ver.21.1.4)
Board: Raspberry Pi 3B (MathWorks Raspbian Image([mathworks_raspbian_R21.1.0](https://github.com/mathworks/Raspbian_OS_Setup/releases/tag/R21.1.0 "mathworks_raspbian_R21.1.0")))
Network Settings: Connect to wireless network
Controlled Car: [SunFounder](https://www.sunfounder.com/products/smart-video-car "") (Smart Video Car Kit V2.0 for Raspberry Pi)

# MATLABコード
## MATLABとRaspberry Piの接続
最初にMATLABを実行しているコンピューターとRaspberry Piとの接続を確立します。
```matlab
% Raspberry Piと接続します。
% パラメーターは各自の環境に合わせて修正してください。
ip_address = '192.168.2.63';
user_name = 'pi';
password = 'raspberry';
mypi = raspi(ip_address, user_name, password);
```

## Raspberry Pi上の I2C デバイスに接続
SunFounderは、I2C接続のPWM/サーボドライバーであるPCA9685を使用して、駆動用モーターとステアリング用モーターを制御します。
Raspberry Pi 上の I2C デバイスに接続するため、下記のコードを実行します。
```matlab
% Raspberry Pi 上の I2C デバイスに接続します。
% パラメーターは各自の環境に合わせて修正してください。
bus = 'i2c-1';
i2c_address = '0x40';
PCA9685 = i2cdev(mypi, bus, i2c_address);
```
:::note info
参考：PCA9685のアドレス設定
私の環境における I2C デバイスアドレスは、下図のようにA0からA5まですべて Open = 0 ですので、 2進数で「100 0000」、16進数で「0x40」となります。
<img width="300" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1400007/0bebdc4c-3c69-4132-13ca-fc92bd3b0cbd.png">
:::

## PCA9685の設定
### PCA9685の初期設定
PCA9685 の初期設定を行います。
```matlab
% PCA9685 のレジスタ名とレジスタ番号を紐付け
MODE1_REGISTER = hex2dec('0x00');
MODE2_REGISTER = hex2dec('0x01');
ALL_LED_ON_L_REG = hex2dec('0xFA');
ALL_LED_ON_H_REG  = hex2dec('0xFB');
ALL_LED_OFF_L_REG = hex2dec('0xFC');
ALL_LED_OFF_H_REG = hex2dec('0xFD');
OUTDRV = hex2dec('0x04');
ALLCALL = hex2dec('0x01');

% ALL_LED full ON/OFF をクリアします。
PCA9685.writeRegister(ALL_LED_ON_L_REG, 0, 'uint8');
PCA9685.writeRegister(ALL_LED_ON_H_REG, 0, 'uint8');
PCA9685.writeRegister(ALL_LED_OFF_L_REG, 0, 'uint8');
PCA9685.writeRegister(ALL_LED_OFF_H_REG, 0, 'uint8');

% MODE1とMODE2を再設定します。
PCA9685.writeRegister(MODE2_REGISTER, OUTDRV, 'uint8');
PCA9685.writeRegister(MODE1_REGISTER, ALLCALL, 'uint8');
pause(0.05);
```
:::note info
参考：PCA9685データシート
![mode_register1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1400007/77863fca-d687-de26-9377-a2927620ae9f.png)
![mode_register2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1400007/b7b999a4-ef46-5b7d-e7db-78771339c4a2.png)
![all_led_on_and_off_control_register.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1400007/68761e1a-e090-6943-0ff8-d4b3c7496caf.png)
:::

### PCA9685のプレスケール値（周波数）設定
PWM出力計算で使用するプレスケール値を計算します。
```matlab
osc_clock = 25e6;   % [Hz]
update_rate = 60;   % [Hz]
resolution = 4096;  
prescale_value = uint8(round(osc_clock / (resolution * update_rate)) - 1);
```

プレスケール値設定を可能にするためには、MODE1レジスタのSLEEPビットを「1」に設定します。
```matlab
old_mode = PCA9685.readRegister(MODE1_REGISTER, 'uint8');
new_mode = bitor(bitand(old_mode, hex2dec('0x7F')), hex2dec('0x10'));
PCA9685.writeRegister(MODE1_REGISTER, new_mode);   % Sleepモード設定
```

PRE_SCALEレジスタに、先程計算したプレスケール値を書き込み、Sleepモードを解除します。
```matlab
PRE_SCALE_REGISTER = hex2dec('0xFE');
RESTART = hex2dec('0x80');
PCA9685.writeRegister(PRE_SCALE_REGISTER, prescale_value);	% プレスケール値の書き込み
PCA9685.writeRegister(MODE1_REGISTER, old_mode);            % Sleepモード解除
pause(0.05);                                                % wait for oscillator
PCA9685.writeRegister(MODE1_REGISTER, bitor(old_mode, RESTART));
```
:::note info
参考：PCA9685データシート
![pwm_frequency_prescale.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1400007/9989953b-8d98-e925-7251-d622ff703595.png)
![pre_scale_register.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1400007/afd5f86a-8695-6b6b-3160-f9c4aed58fd3.png)
:::

### ステアリング制御サーボモーター用PWM出力チャネルのレジスタ定義
ステアリング用サーボモーター用 PWM 出力には CH0 を利用しているため、PCA9685 で使用するレジスタは LED0_ON_L から LED0_OFF_H になります。
```matlab
LED0_ON_L_REG = hex2dec('0x06');
LED0_ON_H_REG = hex2dec('0x07');
LED0_OFF_L_REG = hex2dec('0x08');
LED0_OFF_H_REG = hex2dec('0x09');
```
:::note info
参考：PWM出力チャネル
私の環境では、ステアリング用サーボモーターは CH0 に、駆動用DCモーターは CH4、5 に接続されています。
![PCA9685_board_connection.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1400007/adfc9103-c156-a86c-9419-49bf76e0b5cb.png)
:::

:::note info
参考：PCA9685データシート
![register_definitions_led0.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1400007/5a89c954-c667-6a61-0bca-b3bdc342df4c.png)
:::

## ステアリング制御の確認
### 初期位置への移動
ステアリングを初期位置に移動させるためのPWM出力値（デューティ）を計算し、PCA9685で出力します。
```matlab
duty_cycle_max = 2.25;  % [ms]
duty_cycle_min = 0.75;  % [ms]
max_travel = 140;       % [deg]
pwm_period = 1 / update_rate * 1000;	% [ms]
pwm_max = duty_cycle_max / pwm_period * resolution;
pwm_min = duty_cycle_min / pwm_period * resolution;
control_angle = 120;     % [deg]
pwm_duty = uint16(pwm_min + (pwm_max - pwm_min) / max_travel * control_angle);    % 角度をPWM出力値に変換
pause(1);
PCA9685.writeRegister(LED0_ON_L_REG, 0, 'uint8');   % Delay time = 0%
PCA9685.writeRegister(LED0_ON_H_REG, 0, 'uint8');   % Delay time = 0%
PCA9685.writeRegister(LED0_OFF_L_REG, bitand(pwm_duty, uint16(hex2dec('0xFF'))), 'uint8');	
PCA9685.writeRegister(LED0_OFF_H_REG, bitshift(pwm_duty, -8), 'uint8');     % 右8ビットシフト
```
### 左右に操舵
PCA9685 を制御して、ステアリングを左右に動かしてみます。
```matlab
steer_offset = -30;
for steer_angle = (120:-5:60) + steer_offset
    pwm_duty = uint16(pwm_min + (pwm_max - pwm_min) / max_travel * steer_angle);	% 角度をPWM出力値に変換
    PCA9685.writeRegister(LED0_ON_L_REG, 0, 'uint8');   % Delay time = 0%
    PCA9685.writeRegister(LED0_ON_H_REG, 0, 'uint8');   % Delay time = 0%
    PCA9685.writeRegister(LED0_OFF_L_REG, bitand(pwm_duty, uint16(hex2dec('0xFF'))), 'uint8');	
    PCA9685.writeRegister(LED0_OFF_H_REG, bitshift(pwm_duty, -8), 'uint8');
    pause(0.1);
end
pause(1);
for steer_angle = (60:5:120) + steer_offset
    pwm_duty = uint16(pwm_min + (pwm_max - pwm_min) / max_travel * steer_angle);	% 角度をPWM出力値に変換
    PCA9685.writeRegister(LED0_ON_L_REG, 0, 'uint8');   % Delay time = 0%
    PCA9685.writeRegister(LED0_ON_H_REG, 0, 'uint8');   % Delay time = 0%
    PCA9685.writeRegister(LED0_OFF_L_REG, bitand(pwm_duty, uint16(hex2dec('0xFF'))), 'uint8');	
    PCA9685.writeRegister(LED0_OFF_H_REG, bitshift(pwm_duty, -8), 'uint8');
    pause(0.1);
end
pause(1);
for steer_angle = (120:-5:90) + steer_offset
    pwm_duty = uint16(pwm_min + (pwm_max - pwm_min) / max_travel * steer_angle);	% 角度をPWM出力値に変換
    PCA9685.writeRegister(LED0_ON_L_REG, 0, 'uint8');   % Delay time = 0%
    PCA9685.writeRegister(LED0_ON_H_REG, 0, 'uint8');   % Delay time = 0%
    PCA9685.writeRegister(LED0_OFF_L_REG, bitand(pwm_duty, uint16(hex2dec('0xFF'))), 'uint8');	
    PCA9685.writeRegister(LED0_OFF_H_REG, bitshift(pwm_duty, -8), 'uint8');	    
    pause(0.1);
end
pause(1);
```

:::note info
参考：サーボモーター（SF0180）データシート
![SF0180_spec.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1400007/73fc1a16-9cef-83f1-f5f3-82ce9642f2f6.png)
:::

## 前後進制御の確認
駆動制御DCモーター用 PWM 出力は、PCA9685 PWMドライバー基板の CH4、5 を使用しています。

### 駆動用DCモーター制御用PWM出力レジスタの設定
PCA9685 PWMドライバー基板の CH4、5 に相当する PCA9685 レジスタは、LED4_ON_L から LED5_OFF_H になります。
```matlab
LED4_ON_L_REG = hex2dec('0x16');
LED4_ON_H_REG = hex2dec('0x17');
LED4_OFF_L_REG = hex2dec('0x18');
LED4_OFF_H_REG = hex2dec('0x19');
LED5_ON_L_REG = hex2dec('0x1A');
LED5_ON_H_REG = hex2dec('0x1B');
LED5_OFF_L_REG = hex2dec('0x1C');
LED5_OFF_H_REG = hex2dec('0x1D');
```
:::note info
参考：PCA9685データシート
![led4_led5_register.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1400007/5f2036e8-9f0a-d6cf-2bd1-929ba914d3bb.png)
:::

### DCモータードライバーの回転方向設定 Input のチャネル設定
DCモータードライバーの回転方向設定は、Raspberry Pi の "GPIO 17" と "GPIO 27" を使用しています。
```matlab
MA = 17;
MB = 27;
mypi.configurePin(MA, 'DigitalOutput');
mypi.configurePin(MB, 'DigitalOutput');
```
:::note info
参考：PiCar-V User Manual
![robot_hat.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1400007/aed0bd1d-cdcf-eada-d418-6a2547c1d061.png)
![module_connect.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1400007/2f06cce9-2804-5eab-d435-abef5ec0baf0.png)
:::

### 前進駆動の確認
DCモーター制御基板に制御信号を入力して、左右駆動輪の回転制御を確認します。
まず前進駆動から確認します。
```matlab
% 駆動方向設定
mypi.writeDigitalPin(MA, false);
mypi.writeDigitalPin(MB, false);
% 加速
for speed = 5:10:95
    pwm_duty = uint16(speed / 100 * resolution);
    % 右後輪の駆動
    PCA9685.writeRegister(LED4_ON_L_REG, 0, 'uint8');
    PCA9685.writeRegister(LED4_ON_H_REG, 0, 'uint8');
    PCA9685.writeRegister(LED4_OFF_L_REG, bitand(pwm_duty, uint16(hex2dec('0xFF'))), 'uint8');
    PCA9685.writeRegister(LED4_OFF_H_REG, bitshift(pwm_duty, -8), 'uint8');
    % 左後輪の駆動
    PCA9685.writeRegister(LED5_ON_L_REG, 0, 'uint8');
    PCA9685.writeRegister(LED5_ON_H_REG, 0, 'uint8');
    PCA9685.writeRegister(LED5_OFF_L_REG, bitand(pwm_duty, uint16(hex2dec('0xFF'))), 'uint8');
    PCA9685.writeRegister(LED5_OFF_H_REG, bitshift(pwm_duty, -8), 'uint8');
    pause(1);
end
% 減速
for speed = 95:-10:5
    pwm_duty = uint16(speed / 100 * resolution);
    % 右後輪の駆動
    PCA9685.writeRegister(LED4_ON_L_REG, 0, 'uint8');
    PCA9685.writeRegister(LED4_ON_H_REG, 0, 'uint8');
    PCA9685.writeRegister(LED4_OFF_L_REG, bitand(pwm_duty, uint16(hex2dec('0xFF'))), 'uint8');
    PCA9685.writeRegister(LED4_OFF_H_REG, bitshift(pwm_duty, -8), 'uint8');
    % 左後輪の駆動
    PCA9685.writeRegister(LED5_ON_L_REG, 0, 'uint8');
    PCA9685.writeRegister(LED5_ON_H_REG, 0, 'uint8');
    PCA9685.writeRegister(LED5_OFF_L_REG, bitand(pwm_duty, uint16(hex2dec('0xFF'))), 'uint8');
    PCA9685.writeRegister(LED5_OFF_H_REG, bitshift(pwm_duty, -8), 'uint8');
    pause(1);
end
% DCモーターの回転を停止します。
PCA9685.writeRegister(LED4_ON_L_REG, 0, 'uint8');
PCA9685.writeRegister(LED4_ON_H_REG, 0, 'uint8');
PCA9685.writeRegister(LED4_OFF_L_REG, 0, 'uint8');
PCA9685.writeRegister(LED4_OFF_H_REG, 0, 'uint8');
PCA9685.writeRegister(LED5_ON_L_REG, 0, 'uint8');
PCA9685.writeRegister(LED5_ON_H_REG, 0, 'uint8');
PCA9685.writeRegister(LED5_OFF_L_REG, 0, 'uint8');
PCA9685.writeRegister(LED5_OFF_H_REG, 0, 'uint8');
```

### 後進駆動の確認
後進駆動の確認をします。
```matlab
% 駆動方向設定
mypi.writeDigitalPin(MA, true);
mypi.writeDigitalPin(MB, true);
% 加速
for speed = 5:10:95
    pwm_duty = uint16(speed / 100 * resolution);
    % 右後輪の駆動
    PCA9685.writeRegister(LED4_ON_L_REG, 0, 'uint8');
    PCA9685.writeRegister(LED4_ON_H_REG, 0, 'uint8');
    PCA9685.writeRegister(LED4_OFF_L_REG, bitand(pwm_duty, uint16(hex2dec('0xFF'))), 'uint8');
    PCA9685.writeRegister(LED4_OFF_H_REG, bitshift(pwm_duty, -8), 'uint8');
    % 左後輪の駆動
    PCA9685.writeRegister(LED5_ON_L_REG, 0, 'uint8');
    PCA9685.writeRegister(LED5_ON_H_REG, 0, 'uint8');
    PCA9685.writeRegister(LED5_OFF_L_REG, bitand(pwm_duty, uint16(hex2dec('0xFF'))), 'uint8');
    PCA9685.writeRegister(LED5_OFF_H_REG, bitshift(pwm_duty, -8), 'uint8');
    pause(1);
end
% 減速
for speed = 95:-10:5
    pwm_duty = uint16(speed / 100 * resolution);
    % 右後輪の駆動
    PCA9685.writeRegister(LED4_ON_L_REG, 0, 'uint8');
    PCA9685.writeRegister(LED4_ON_H_REG, 0, 'uint8');
    PCA9685.writeRegister(LED4_OFF_L_REG, bitand(pwm_duty, uint16(hex2dec('0xFF'))), 'uint8');
    PCA9685.writeRegister(LED4_OFF_H_REG, bitshift(pwm_duty, -8), 'uint8');
    % 左後輪の駆動
    PCA9685.writeRegister(LED5_ON_L_REG, 0, 'uint8');
    PCA9685.writeRegister(LED5_ON_H_REG, 0, 'uint8');
    PCA9685.writeRegister(LED5_OFF_L_REG, bitand(pwm_duty, uint16(hex2dec('0xFF'))), 'uint8');
    PCA9685.writeRegister(LED5_OFF_H_REG, bitshift(pwm_duty, -8), 'uint8');
    pause(1);
end
% 停止
PCA9685.writeRegister(LED4_ON_L_REG, 0, 'uint8');
PCA9685.writeRegister(LED4_ON_H_REG, 0, 'uint8');
PCA9685.writeRegister(LED4_OFF_L_REG, 0, 'uint8');
PCA9685.writeRegister(LED4_OFF_H_REG, 0, 'uint8');
PCA9685.writeRegister(LED5_ON_L_REG, 0, 'uint8');
PCA9685.writeRegister(LED5_ON_H_REG, 0, 'uint8');
PCA9685.writeRegister(LED5_OFF_L_REG, 0, 'uint8');
PCA9685.writeRegister(LED5_OFF_H_REG, 0, 'uint8');
```
## 終了処理
動作確認を終了します。
```matlab
clear PCA9685 mypi
```

# 実行例
<iframe width="640" height="360" src="https://www.youtube.com/embed/7S1nIybrc3U" title="MATLAB Support Package for Raspberry Pi を使用した SunFounder の動作例" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe>

# まとめ
MATLAB Support Package for Raspberry Pi Hardwareを使用して、MATLABからSunFounder(Smart Video Car Kit V2.0 for Raspberry Pi)を動かす例を紹介しました。MATLABからRaspberry Piを使用する際の参考になれば幸いです。
今後、Simulinkでの実行例についても紹介する予定です。

# 参考
1. [SunFounder_PiCar-V](https://github.com/sunfounder/SunFounder_PiCar-V "SunFounder_PiCar-V")
1. [SunFounder_PCA9685](https://github.com/sunfounder/SunFounder_PCA9685 "SunFounder_PCA9685")
1. [TB6612 DC Motor Driver Module](http://wiki.sunfounder.cc/index.php?title=TB6612_DC_Motor_Driver_Module "TB6612")