はじめに

Simulink Support Package for Raspberry PiでSunFounderを動かす(1)(2)の続き記事です。
これまでの記事では、Simulinkモデルの構造と実行制御部の説明、および、初期化処理サブシステムについて説明しました。
本記事では、周期処理サブシステムの概要について説明します。

周期処理サブシステムの概要

Sunfounderのステアリング制御と前後進制御を行う周期処理サブシステムは、下図のように作成しています。

右側には、ステアリング制御と前後進制御の処理単位ごとに複数のFunction-Call Subsystemを配置し、左側にあるサブシステムで、ステアリング制御と前後進制御の処理の実行順を制御するようにしています。

ステアリング制御と前後進制御のFunction-Call Subsystemの内容は下表の通りです。
もう少しサブシステムをまとめることも可能だと思いますが、動かすことを優先したため、このような構成になっています。

#	Function-Call Subsystemの処理内容（上から）
1	ステアリング初期位置を設定
2	ステアリングを左方向に少しずつ操舵
3	ステアリングを右方向に少しずつ操舵
4	加速制御
5	減速制御
6	前後進の停止
7	駆動方向の設定

実行順を制御するモデルの概要

実行順を制御するモデルは、下図のように作成しました。

Switch Caseブロックを使用して、実行ステップを制御するようにしています。
あらかじめ決めていおいた実行回数を実行したら、次のステップに移行するようにしています。
各ステップの概要を下表に示します。

#	実行ステップ	次の実行ステップ	次ステップの実行回数
1	ステアリング初期位置設定	ステアリング左方向制御	1
2	ステアリング左方向制御	ステアリング右方向制御	1
3	ステアリング右方向制御	制御ステップ変更	1
4	制御ステップ変更	前進加速駆動制御	9
5	前進加速駆動制御	前進減速駆動制御	9
6	前進減速駆動制御	前後進の停止	1
7	前後進の停止	後進加速駆動制御	9
8	後進加速駆動制御	後進減速駆動制御	9
9	後進減速駆動制御	前後進の停止	1
10	前後進の停止	制御ステップ変更	1

実行制御のステップは、以下のような列挙型定義ファイルで設定しています。

enumSunFounderOperation.m

classdef enumSunFounderOperation < Simulink.IntEnumType
    enumeration
        ChangeStep(0),                      % 実行ステップ変更
		SteerToInitPosition(1),             % ステアリングを初期位置に設定
		SteerToLeft(2),                     % ステアリングを左に操舵
        SteerToRight(3),                    % ステアリングを右に操舵
        AccelerateInForwardDirection(4),    % 前進方向の加速
        DecelerateInForwardDirection(5),    % 前進方向の減速
        AccelerateInBackwardDirection(6),   % 後退方向の加速
        DecelerateInBackwardDirection(7),   % 後退方向の減速
        StopMovingForwardAndBackward(8)     % 前後進の停止
    end

    methods (Static)
        function defaultValue = getDefaultValue()
            defaultValue = enumSunFounderOperation.SteerToInitPosition;
        end

        function dScope = getDataScope()
            dScope = 'Auto';
        end

        function desc = getDescription()
            desc = 'SunFounder動作確認用Simulinkモデルで使用する呼び出し機能名';
        end

        function fileName = getHeaderFile()
            fileName = '';
        end

        function flag = addClassNameToEnumNames()
            flag = true;
        end
    end
end

以下、各Switch Case Action Subsystem内を示します。

ステアリング初期位置設定呼び出し

ステアリング左方向制御呼び出し

ステアリング右方向制御を呼び出し

制御ステップ変更

前進加速駆動制御を呼び出し

回転方向設定

前進減速駆動制御を呼び出し

前後進の停止

次制御ステップ選択

次制御ステップは、前ステップが前進減速駆動制御ステップか後進減速駆動制御ステップかにより変わります。

前ステップが前進減速駆動制御ステップの場合

前ステップが前進減速駆動制御ステップの場合の次ステップは、後進加速駆動制御ステップになります。

前ステップが後進減速駆動制御ステップの場合

前ステップが後進減速駆動制御ステップの場合の次ステップは、制御ステップ変更になります。

後進加速駆動制御を呼び出し

後進減速駆動制御を呼び出し

ステアリング制御

「MATLAB Support Package for Raspberry PiでSunFounderを動かす」の記事で書いたMATLABプログラムと同等の処理をSimulinkでモデリングしています。

ステアリング初期位置の設定

ステアリング初期位置を設定するモデルを以下に示します。

以下の4つのレジスタを順番に設定するため、本例ではFunction-Call Generatorブロックの反復回数を4に設定して、それぞれのレジスタを設定するFunction-Call Subsystemを実行しています。

LED0_ON_L_REG
LED0_ON_H_REG
LED0_OFF_L_REG
LED0_OFF_H_REG

レジスタを設定するFunction-Call Subsystem内では、Simulink Support Package for Raspberry Pi HardwareのI2C Master Writeブロックを使用し、I2C通信でPCA9685のレジスタを設定するようにしています。

I2C Master Writeブロックのパラメーター設定例を下図に示します。

レジスタアドレスの値は、モデルワークスペースにMATLAB変数として定義しています。

ステアリング用サーボモーターを駆動するPWM信号のデューティ比を計算するサブシステムは、以下のように作成しています。

ステアリング左方向操舵

ステアリングを左方向に少しずつ操舵する処理は、For Iterator Subsystemを使用して、以下のようにモデリングしています。

For Iterator Subsystem内のモデル構造としては、初期位置設定用のFunction-Call Subsystemとほぼ同じです。
ステアリング制御角度を5度刻みで入力する箇所と待機処理部が異なります。

待機処理

ステアリングを少しずつ操舵するための待機処理は、下図のようにモデリングしています。
時刻取得には、ROS ToolboxのCurrent Timeブロックを使用しています。
本記事内では行いませんが、ホストPCとRaspberry PiをROSネットワークに接続して、ホストPCからRaspberry Piを制御することも考えていたため、ROS Toolboxを使用しています。

ステアリング右方向操舵

ステアリングを右方向に少しずつ操舵する処理は、ステアリング制御角度を設定するConstantブロックの値（[0:5:30]）を除いて、左方向への操舵と同じようにモデリングしています。

前後進制御

ステアリング制御部と同様に、「MATLAB Support Package for Raspberry PiでSunFounderを動かす」の記事で書いたMATLABプログラムと同等の処理をSimulinkでモデリングしています。

加速制御

加速制御部のモデルを以下に示します。
シミュレーションステップごとに速度テーブルの値を読み進めていくようにしています。
速度テーブルは、モデルワークスペースに定義しています。

前後進制御用PWM出力値を計算

速度テーブルの値からモーター制御用PWM信号のデューティ比を計算します。

後輪駆動制御モデル

右後輪を制御するモデルを以下に示します。

以下の4つのレジスタを順番に設定するため、本例ではFunction-Call Generatorブロックの反復回数を4に設定して、それぞれのレジスタを設定するFunction-Call Subsystemを実行しています。

LED4_ON_L_REG
LED4_ON_H_REG
LED4_OFF_L_REG
LED4_OFF_H_REG

左後輪駆動部では、以下のレジスタを設定しています。

LED5_ON_L_REG
LED5_ON_H_REG
LED5_OFF_L_REG
LED5_OFF_L_REG

減速制御

減速制御のモデル構造は、加速制御のモデルと同じです。
異なる箇所は、速度テーブルの値だけです。

前後進駆動の停止

前後進駆動の停止処理モデルを以下に示します。

左右後輪の駆動処理モデルは、加速制御と減速制御で使用したものと同じです。
入力のデューティ比を0%にすることで、回転を停止するようにしています。

駆動方向の設定

前後進の駆動方向を設定するFunction-Call Subsystemを以下に示します。
GPIO17とGPIO27の値で、駆動方向を設定しています。

次の記事に続きます。

参考