月別アーカイブ: 2014年12月

WiringPiでPWM制御する

Raspberry Piには、ハードウェアのPWMが1チャンネル開放されています。 速度制御の目的であれば1チャンネルでも可能ですが、より細かな制御が必要となった時の為にWiringPiのsoftpwmを検証しておきたいと思います。

開発中

簡単なコードを書いて実行してみると、 ん?なんか予想と違う動き…。

PWM出力を設定したCのプログラムが終了するとPWMも停止してしまいます。 希望としては停止コマンドを送るまでは継続して欲しいのですが…。 コードのミス?仕様?

作者のサイトをよくよく確認すると、こんな注意書きが。

There is currently no way to disable softPWM on a pin while the program in running.
You need to keep your program running to maintain the PWM output!

PWMを実行中に無効にすることはできず、出力を維持するにはプログラムを実行し続ける必要がある。 って、仕様だったのです。

実際に組み込む際はこの辺りを考慮するとして、今回は実験用のコードを書いてみました。

#include <stdio.h>
#include <wiringPi.h>

#define MOTOR1 22			// GPIO22
#define MOTOR2 23			// GPIO23
#define MOTOR3 24			// GPIO24
#define MOTOR4 25			// GPIO25
#define RANGE 100			//PWM最大値
#define LEFT_STR "l"
#define RIGHT_STR "r"
#define FWRD_STR "f"
#define BACK_STR "b"
#define STOP_STR "s"

int main(int argc, char **argv)
{

	// 引数をチェック
	if ( argc < 4 ) return;

	// 速度を設定
	int pwm = atoi(argv[2]);

	// WiringPi初期化
	if(wiringPiSetupGpio() == -1) return;

	// softPWM設定
	softPwmCreate(MOTOR1,0,RANGE);
	softPwmCreate(MOTOR2,0,RANGE);
	softPwmCreate(MOTOR3,0,RANGE);
	softPwmCreate(MOTOR4,0,RANGE);

	if ( strcmp( argv[1], STOP_STR ) == 0 ){
		//停止
		softPwmWrite(MOTOR1, 0);
		softPwmWrite(MOTOR4, 0);
		softPwmWrite(MOTOR2, 0);
		softPwmWrite(MOTOR3, 0);
		return;
	} else if ( strcmp( argv[1], FWRD_STR ) == 0 ){
		//前進
		softPwmWrite(MOTOR2,pwm);
		softPwmWrite(MOTOR4,pwm);
	} else if ( strcmp( argv[1], BACK_STR ) == 0 ){
		//後退
		softPwmWrite(MOTOR1,pwm);
		softPwmWrite(MOTOR3,pwm);
	} else if ( strcmp( argv[1], LEFT_STR ) == 0 ){
		//左ターン
		softPwmWrite(MOTOR1,pwm);
		softPwmWrite(MOTOR4,pwm);
	} else if ( strcmp( argv[1], RIGHT_STR ) == 0 ){
		//右ターン
		softPwmWrite(MOTOR2,pwm);
		softPwmWrite(MOTOR3,pwm);
	}
	// 時間待ち
	delay(atoi(argv[3]) * 100);

	// ブレーキ
	softPwmWrite(MOTOR1,RANGE);
	softPwmWrite(MOTOR4,RANGE);
	softPwmWrite(MOTOR2,RANGE);
	softPwmWrite(MOTOR3,RANGE);
	delay(500);
	// 停止
	softPwmWrite(MOTOR1, 0);
	softPwmWrite(MOTOR4, 0);
	softPwmWrite(MOTOR2, 0);
	softPwmWrite(MOTOR3, 0);

	return 0;
}

実行は、第1引数に移動方向(f,b,l,r)、第2引数に速度(0~100)、第3引数に実行時間(0.1s単位)を指定します。
例えば

./motor_pwm f 75 30

と、指定することにより、75%の速度で3秒間前進します。

動きは滑らかで良いのですが、ソフトPWMでいくか、ハードPWMにするかはもう少し検討してみます。

 

ジャイロセンサーを使う

小型の電動ヘリが出始めの頃、ジャイロセンサーは高級なイメージだったように思います。 ジャイロといえば、地球ゴマが一定の姿勢を保つ動きが連想されますね。(古い?)

現在は、携帯機器の普及で簡単に電子工作にも手軽に使えるパーツになりました。

ジャイロセンサーの利用は初めてでしたが、漠然と「角度の偏移が出力される」などと考えていたのですが、実際は「角速度」という馴染みのない物でした。

今回は、秋月電子のL3GD20 3軸ジャイロセンサーモジュールを使用しました。

ジャイロセンサー L3GD20

ジャイロセンサー L3GD20

角速度と角度の関係について、詳しく解説している動画があります。 大変参考になったのですが難しい…。

角速度センサーを使って角度を求める方法

私なりの理解は、角速度と角度の関係は、速度と距離の関係に似ているということでした。

gyro_g

一定時間単位で角速度を計測し、前回計測値からの移動量を積算していく。 上のグラフの青色の面積を求めれば近似値が得られるということですね。 簡略積分法です。

論理はともかくまずは実験ということでとりかかったのですが、これがなかなか大変でかなり時間を費やしました。

静止時の変動や振動などによるドリフト、値はなかなか安定しないのですが、先人たちの知恵と実験につぐ実験により、なんとか使えるコードが書けました。

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>

#define MOTOR1 22			// GPIO22
#define MOTOR2 23			// GPIO23
#define MOTOR3 24			// GPIO24
#define MOTOR4 25			// GPIO25
#define LEFT_STR "l"		// 左旋回指示用文字
#define RIGHT_STR "r"		// 右旋回指示用文字

int main(int argc, char **argv)
{
	int i2c_fd;				// I2Cデバイスファイル
	int i2cAddress = 0x6a;	// L3GD20のI2Cアドレス
	int i,regH,regL,out_z,adj,backup,kai;
	float data,deg;

	// I2Cデバイスファイルをオープン
	i2c_fd = wiringPiI2CSetup(i2cAddress);

	// L3GD20 イニシャライズ
	if((wiringPiI2CWriteReg8(i2c_fd,0x20,0x0f))<0){
		printf("write error register 0x20");
	}
	printf("write register:0x20 = 0x0f\n");

	// 旋回指定値の取得
	kai = atoi(argv[2]);

	// WiringPi イニシャライズ
	if(wiringPiSetupGpio() == -1) return;

	// モーター停止
	digitalWrite(MOTOR1, 0);
	digitalWrite(MOTOR2, 0);
	digitalWrite(MOTOR3, 0);
	digitalWrite(MOTOR4, 0);

	// 静止時の補正値を計測
	for(i=0; i<20; i++){
		// デバイスからデータ取得
		regL = wiringPiI2CReadReg8(i2c_fd,0x2c);	// Lレジスタ読み出し
		regH = wiringPiI2CReadReg8(i2c_fd,0x2d);	// Hレジスタ読み出し
		out_z = (regH<<24|regL<<16)>>16;			// H,L合成
		adj = adj + out_z;
		usleep(10000);
	}
	adj = adj / 20;									// 平均化し補正値とする

	//左旋回動作
	if ( strcmp( argv[1], LEFT_STR ) == 0 ){
		digitalWrite(MOTOR1, 1);
		digitalWrite(MOTOR4, 1);
	}
	//右旋回動作
	if ( strcmp( argv[1], RIGHT_STR ) == 0 ){
		digitalWrite(MOTOR2, 1);
		digitalWrite(MOTOR3, 1);
	}

	// ジャイロデータを取得
	for(i=0; i<10000; i++){
		// デバイスからデータ取得
		regL = wiringPiI2CReadReg8(i2c_fd,0x2c);
		regH = wiringPiI2CReadReg8(i2c_fd,0x2d);
		out_z = (regH<<24|regL<<16)>>16;

		// データを補正し角度算出(台形法)
		data += ((out_z - adj) + backup) / 2;
		backup = out_z - adj;
		deg = abs(data / 9700);					// 9700は角度変換用係数
		//printf("  測定値 : %5.2f  角度 : %5.2f\n",data,deg);

		//角度をチェックし、指定値に達していたら停止
		if (deg > kai ){
			//ブレーキ
			digitalWrite(MOTOR1, 1);
			digitalWrite(MOTOR4, 1);
			digitalWrite(MOTOR2, 1);
			digitalWrite(MOTOR3, 1);
			usleep(100000);
			//停止
			digitalWrite(MOTOR1, 0);
			digitalWrite(MOTOR4, 0);
			digitalWrite(MOTOR2, 0);
			digitalWrite(MOTOR3, 0);
			return;
		}

		usleep(10000);
	}

	return;
}

上記コードをコンパイル

gcc gyro_turn.c -o gyro_turn -I/usr/local/include -L/usr/local/lib -lwiringPi

実行は

sudo ./gyro_turn r 90

と、することにより右に90度旋回します。(実際には停止距離を考慮し角度を決定)

若干、動作にばらつきもありますが、ほぼ納得できる結果が得られました。

 

Webコントロールの画面構成

webコントロール画面は、MJPG-Streamerのストリーミング映像を中心に構成しています。

機能面では、Motionの方が充実しているのですが、動作の軽快なMJPG-Streamerを利用しています。

Webコントロール画面

カメラ映像にはjavascriptにより、情報を重ね合わせ表示しています。

web_cnt_sta_v0

起動時刻、CPU負荷、CPU温度、無線品質、距離センサーのデータを表示しています。

今後、バッテリー電圧や進行方向・走行距離などの表示を加えたいと思います。

試作レベルの為、操作性やデザインなどを考慮していなかったので、現在、新バージョンを製作中です。