２．モータ駆動のためのUARTプロトコール作成プログラム
前回の記事で、Raspberry PiからBrickPiへモータを駆動するときのUARTプロトコルが判明したので、ここでは、そのプログラムを作成する。
UARTプロトコルの作成で面倒くさいのは、ビット操作が入ることである。モータを駆動させるための情報は、回転速度や回転方向などのいくつかのデータで構成されていたが、これらのデータは、一連のビット列の中に詰め込まれる。
モータの場合にはビット列は3バイトの長さであったが、センサの場合にはもっと長くなるかもしれない。そこで、長いビット列を表すために、HaskellのプログラムではIntegerで表すことにする。Integerは無限の整数を表せるので、ビット列の長さを気にする必要がない。
大まかな方針が決まったので、モータを表すのに必要なデータ型を用意する。まず、モータの制御には、回転速度(speed)と回転方向(direction)と駆動させているか否か(enable)の情報が必要なので、データ型Motorを次のように定める。また、回転方向についてもデータ型Rotationを用意する。
data Rotation = Positive | Negative deriving (Eq, Show) data Motor = Motor { speed :: Int, direction :: Rotation, enable :: Bool } deriving (Show)
また、テストデータとして、モータを前方向(positive)、後方向(negative)、中立(idling）させるときの値を次のように定める。
positive = Motor { speed=200, direction=Positive, enable=True } negative = Motor { speed=200, direction=Negative, enable=True } idling = Motor { speed=0, direction=Positive, enable=True }
BrickPiには4個のモータA,B,C,Dを備え付けられる。そこで、4つのモータの全ての(制御)情報を集めたデータ型Moveを用意する。
data Move = Move { motorA :: Motor, motorB :: Motor, motorC :: Motor, motorD :: Motor}
テストデータとして、モータA,Bによって車を駆動しているとし、車を前進(forward)、左折(left)、右折(right)、後退(backward)させるときの値を次のように定める。
forward = Move { motorA = positive, motorB = positive, motorC = idling, motorD = idling} left = Move { motorA = negative, motorB = positive, motorC = idling, motorD = idling} right = Move { motorA = positive, motorB = negative, motorC = idling, motorD = idling} backward = Move { motorA = negative, motorB = negative, motorC = idling, motorD = idling}
つぎに、Raspberry PiからBrickPiは、モータの制御情報を送り出す関数を用意する。前の記事で説明したように、モータAとBは一つのメッセージで制御され、CとDは別のメッセージで制御される。これらのメッセージを作成するための関数をMesg1,Mesg2とする。これらの関数は、モータの制御情報(Move)をもらい、これを8ビット毎にくくった列を生成する。Raspberry PiからBrickPiに転送するときは、それぞれの8ビットを文字として表すことにするが、ここでは、その前段階としてIntで表すこととする。
メッセージは、7バイトで構成されるので、mesg1は次のようになる。
mesg1 :: Move -> [Int] mesg1 mv = a1:a2:a3:a4:a5:a6:a7:[]
上記のmesg1は不完全で、a1からa7までを定める必要がある。ここで、a1は、モータA,Bを制御する先のアドレスである。a2は構成しているバイトの内容の総和(正確には、和を256で割った時の剰余)である。a3は、データの部分を表すのに必要なバイト数である。a4は制御の対象となっているモータやセンサのタイプである。その後のa5からa6が速度や方向などを詰め込んだビット列である。次のようにしてこれを得る。
m :: Move -> Integer m mv = spA * 2 ^ (8 + 6) + dirA * 2 ^ (8 + 5) + enA * 2 ^ (8 + 4) + spB * 2 ^ 4 + dirB * 2 ^ 3 + enB * 2 ^ 2 where spA = toInteger $ speed $ motorA mv dirA = if (direction $ motorA mv) == Positive then 0 else 1 enA = if enable $ motorA mv then 1 else 0 spB = toInteger $ speed $ motorB mv dirB = if (direction $ motorB mv) == Positive then 0 else 1 enB = if enable $ motorB mv then 1 else 0
また、整数の列から文字の列への変換は次のようになる。
mesg1' mv = map chr $ mesg 1
プログラムの全体を示すと次のようになる。
module BrickPiMotor (mesg1, mesg2) where import Data.Char data Rotation = Positive | Negative deriving (Eq, Show) data Motor = Motor { speed :: Int, direction :: Rotation, enable :: Bool } deriving (Show) positive = Motor { speed=200, direction=Positive, enable=True } negative = Motor { speed=200, direction=Negative, enable=True } idling = Motor { speed=0, direction=Positive, enable=True } data Move = Move { motorA :: Motor, motorB :: Motor, motorC :: Motor, motorD :: Motor} forward = Move { motorA = positive, motorB = positive, motorC = idling, motorD = idling} left = Move { motorA = negative, motorB = positive, motorC = idling, motorD = idling} right = Move { motorA = positive, motorB = negative, motorC = idling, motorD = idling} backward = Move { motorA = negative, motorB = negative, motorC = idling, motorD = idling} newtype Message = Message String deriving (Show) mesg1 :: Move -> [Int] mesg1 mv = a1:a2:a3:a4:a5:a6:a7:[] where a1 = 1 a2 = fromIntegral $ mod (a1 + a3 + a4 + a5 + a6 + a7) (2 ^ 8) a3 = 4 a4 = 3 a = m mv a7 = fromIntegral $ mod (div a (2 ^ (8 * 2))) (2 ^ 8) a6 = fromIntegral $ mod (div a (2 ^ 8)) (2 ^ 8) a5 = fromIntegral $ mod a (2 ^ 8) m :: Move -> Integer m mv = spA * 2 ^ (8 + 6) + dirA * 2 ^ (8 + 5) + enA * 2 ^ (8 + 4) + spB * 2 ^ 4 + dirB * 2 ^ 3 + enB * 2 ^ 2 spA = toInteger $ speed $ motorA mv dirA = if (direction $ motorA mv) == Positive then 0 else 1 enA = if enable $ motorA mv then 1 else 0 spB = toInteger $ speed $ motorB mv dirB = if (direction $ motorB mv) == Positive then 0 else 1 enB = if enable $ motorB mv then 1 else 0 mesg2 :: Move -> [Int] mesg2 mv = a1:a2:a3:a4:a5:a6:a7:[] where a1 = 2 a2 = fromIntegral $ mod (a1 + a3 + a4 + a5 + a6 + a7) (2 ^ 8) a3 = 4 a4 = 3 a = m mv a7 = fromIntegral $ mod (div a (2 ^ (8 * 2))) (2 ^ 8) a6 = fromIntegral $ mod (div a (2 ^ 8)) (2 ^ 8) a5 = fromIntegral $ mod a (2 ^ 8) m :: Move -> Integer m mv = spC * 2 ^ (8 + 6) + dirC * 2 ^ (8 + 5) + enC * 2 ^ (8 + 4) + spD * 2 ^ 4 + dirD * 2 ^ 3 + enD * 2 ^ 2 spC = toInteger $ speed $ motorC mv dirC = if (direction $ motorC mv) == Positive then 0 else 1 enC = if enable $ motorC mv then 1 else 0 spD = toInteger $ speed $ motorD mv dirD = if (direction $ motorD mv) == Positive then 0 else 1 enD = if enable $ motorD mv then 1 else 0 mesg1' mv = map chr $ mesg1 mv mesg2' mv = map chr $ mesg2 mv
実行結果は次のようになる。
Prelude> :load "BrickPiMotor.hs" [1 of 1] Compiling BrickPiMotor ( BrickPiMotor.hs, interpreted ) Ok, modules loaded: BrickPiMotor. *BrickPiMotor> mesg1 forward [1,218,4,3,132,28,50] *BrickPiMotor> mesg1' forward "\SOH\218\EOT\ETX\132\FS2" *BrickPiMotor> mesg1 left [1,250,4,3,132,60,50] *BrickPiMotor> mesg1 right [1,226,4,3,140,28,50] *BrickPiMotor> mesg1 backward [1,2,4,3,140,60,50]
３．検証
開発をさらに進めるために、上記のプログラムが正しい値を出力しているのかどうかを確認する必要がある。そこで、BrickPiの発売元であるDexter Industries社が提供しているテストプログラムを実行しテ確認する。Dexter Industries社はPythonとC言語用のテストプログラムを用意している。ここでは、C言語用に用意されているLego-Motor Test.cを利用する。このプログラムは次のようになっている。
/* * Jaikrishna * t.s.jaikrishna<at>gmail.com * Initial date: June 20, 2013 * Updated: Feb 17, 2015 (John) * Based on Matthew Richardson's Example for testing BrickPi * You may use this code as you wish, provided you give credit where it's due. * * This is a program for testing the RPi BrickPi driver with Lego Motor on Port1 */ #include <stdio.h> #include <math.h> #include <time.h> #include "tick.h" #include "BrickPi.h" #include <linux/i2c-dev.h> #include <fcntl.h> // Compile Using: // sudo gcc -o program "LEGO - Motor Test.c" -lrt -lm // Run the compiled program using: // sudo ./program int result,v,f; #undef DEBUG int main() { ClearTick(); result = BrickPiSetup(); printf("BrickPiSetup: %d\n", result); if(result) return 0; BrickPi.Address[0] = 1; BrickPi.Address[1] = 2; BrickPi.MotorEnable[PORT_A] = 1; BrickPi.MotorEnable[PORT_B] = 1; result = BrickPiSetupSensors(); printf("BrickPiSetupSensors: %d\n", result); v=0; f=1; if(!result){ usleep(10000); while(1){ result = BrickPiUpdateValues(); printf("BrickPiUpdateValues: %d\n", result); // added if(!result){ printf("%d\n",v); if(f==1) { if(++v > 300) f=0; BrickPi.MotorSpeed[PORT_A]=200; BrickPi.MotorSpeed[PORT_B]=200; BrickPiUpdateValues(); } else{ if(--v<0) f=1; BrickPi.MotorSpeed[PORT_A]=-200; BrickPi.MotorSpeed[PORT_B]=-200; BrickPiUpdateValues(); } } usleep(10000); } } return 0; }
これをコンパイルして、Rasberry PiからBrickPiへの出力を観察すればよい。そこで、出力が分かるようにするために、関数void BrickPiTXの最後に次の分を追加しておく。
i = 0; while(i < (ByteCount + 3)){ printf("i = %d ",i); printf("buffer=%d\n",tx_buffer[i]); i++; }
ところで、コンパイルのところで引っかかったのだが、Dexter Industries社は二種類のBrickPi.hを用意している。/home/pi/Desktop/BrickPi_CのところにCでの開発環境が用意されている。このディレクトリ直下のDriversにはヘッダーファイルが置かれている。BrickPi.hがBrickPiを利用するためのヘッダーファイルである。また、Sensor_Examplesには、テスト用のプログラムが置かれている。ここにもBrickPi.hがあるのだが、なんと二つのBrickPiはバージョンが異なっている。後者の方は2015年2月17日作成のもので、UARTとのインタフェースはunitstd.h,fcntl.h,termios.hを用いている。前者の方は2014年6月19日作成のもので、UARTのインタフェースはwiring.hを用いている。コンパイルするときは、前者は
sudo gcc -o program "LEGO - Motor Test.c" -lrt -lm -L/usr/local/lib
を、後者は
sudo gcc -o program "LEGO - Motor Test.c" -lrt -lm -L/usr/local/lib -lwiringPi
を、用いる。
実行結果は次のようになった。
最後の7行が前進に対する出力で、作成したプログラムと同じ出力であることが分かる。なお、最初の12行の出力はPickUpSensorの命令をした時の出力である。
また、次の14行の出力は最初のBrickUpDateValuesに対応するもので、全てのデータが0にリセットされた後、モータを駆動するという情報だけを与えての出力である。
なお、wiringPiを用いてコンパイルし、実行したときは、モータからの応答がなく失敗に終わった（何回行ったが結果は同じであった）。
