移動ロボットの行動プログラミング
第3回 測域センサの使い方

$ sudo chmod 777 /dev/ttyACM0
$ cat /dev/ttyACM0
	

$ cat - > /dev/ttyACM0

SCIP2.0
VV
PP
II
		

MD0384038400001

MD0384038400001 コマンドエコーバック
00P ステータスコード(00:OK)とチェックサム
MD0384038400000 データ送信開始行(後ろ2桁は残りスキャン回数)
99b ステータスコード(99:OK)とチェックサム
3=CCf タイムスタンプ(4桁)とチェックサム
1DhM 距離データ(3桁)とチェックサム
		

#include <stdio.h>
#include <unistd.h>
#include <sys/time.h>
#include <signal.h>
#include <math.h>
#include <scip2awd.h>

int escape;

void ctrlc( int notused )
{
	// MDコマンドを発行したままプログラムを終了すると、
	// 次回起動時に少し余分に時間がかかる
  	escape = 1;
  	signal( SIGINT, NULL );
}

int main( int argc, char *argv[] )
{
	S2Port *port;    // ポート
	S2Sdd_t buf;     // データ取得用ダブルバッファ
	S2Scan_t *scan;  // データ読み出し用構造体
	S2Param_t param; // センサのパラメータ構造体
	int ret;

	if( argc != 2 ){
    	fprintf( stderr, "USAGE: %s device\n", argv[0] );
    	return 0;
  	}

	// ポートを開く
  	port = Scip2_Open( argv[1], B0 );
  	if( port == 0 ){
    	fprintf( stderr, "ERROR: Failed to open device.\n" );
    	return 0;
  	}
	printf( "Port opened\n" );

  	// 初期化
  	escape = 0;
  	signal( SIGINT, ctrlc );
  	S2Sdd_Init( &buf );
  	printf( "Buffer initialized\n" );

  	// URGのパラメータ取得
  	Scip2CMD_PP( port, &param );

  	// URG-04LXの全方向のデータを取得開始
  	Scip2CMD_StartMS( port, param.step_min, param.step_max,
                    	1, 0, 0, &buf, SCIP2_ENC_3BYTE );

  	while( !escape ){
    	ret = S2Sdd_Begin( &buf, &scan );
    	if( ret > 0 ){
      		int j;
      		// 新しいデータがあった時の処理をここで行う
      		printf( "Front distance: %lu mm\n", 
               		scan->data[ param.step_front - param.step_min ] );

      		// 処理例:スキャンしたデータをxy座標(m単位)に変換
      		for( j = 0; j < scan->size; j ++ ){
        		float x, y;
        		float scan_theta;

        		// scan->data[j]はmm単位の距離を表し、
        		// 20mm以下の距離は測距エラーを意味する
        		if( scan->data[j] < 20 ) continue;

       			scan_theta = M_PI * 2.0 * ( j - ( param.step_front - param.step_min ) ) / 
                        param.step_resolution;
        		// URGが上下逆についている場合は、scan_theta = -scan_theta;
        		x = scan->data[j] * 0.001 * cos( scan_theta );
        		y = scan->data[j] * 0.001 * sin( scan_theta );

        		// このx,yやscan->data[]の値を上手く使って処理を行う
      		}
      		// S2Sdd_BeginとS2Sdd_Endの間でのみ、構造体scanの中身にアクセス可能
      		S2Sdd_End( &buf );
    	}
    	else if( ret == -1 ){
      		// 致命的なエラー時(URGのケーブルが外れたときなど)
      		fprintf( stderr, "ERROR: Fatal error occurred.\n" );
      		break;
    	}
    	else{
      		// 新しいデータはまだ無い
      		usleep( 10000 );
    	}
  	}
  	printf( "\nStopping\n" );

  	ret = Scip2CMD_StopMS( port, &buf );
  	if( ret == 0 ){
    	fprintf( stderr, "ERROR: StopMS failed.\n" );
    	return 0;
  	}

  	printf( "Stopped\n" );
  	S2Sdd_Dest( &buf );
  	printf( "Buffer destructed\n" );
  	Scip2_Close( port );
  	printf( "Port closed\n" );

  	return 1;
}
		

$ gcc test.c -o test -lscip2awd -lypspur -lpthread -lm
$ sudo chmod 777 /dev/ttyACM0
$ ./test /dev/ttyACM0
　　　　

$gcc test.c -o test -Wl,--no-as-needed -lscip2awd -lypspur -lpthread -lm
        

$ sudo su -
# echo "/usr/local/lib" > /etc/ld.so.conf.d/lib64.conf
# echo "/usr/local/lib64" >> /etc/ld.so.conf.d/lib64.conf
# ldconfig
# exit