ここでは NXT と PC 間の Bluetooth 通信の方法を示しますが、NXT同士の Bluetooth 通信については、基礎編の10．Bluetooth通信で説明しています。
1．Bluetooth 通信プログラム
以下に前々回の説明したPD制御によるライントレースのプログラムに、Bluetooth通信機能を加えたプログラム示します。
datalogging.c
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"
#define PORT_LIGHT NXT_PORT_S3 /* 入出力ポートの定義 */
#define PORT_TOUCH NXT_PORT_S2
#define L_MOTOR NXT_PORT_B
#define R_MOTOR NXT_PORT_C
#define BLACK 700
#define WHITE 500
#define BT_PASS_KEY "1234" /* bluetoothパスキー */
DeclareTask(Task1); /* Task1を宣言 */
void ecrobot_device_initialize(){ /* OSEK起動時の処理 */
nxt_motor_set_speed(L_MOTOR,0,0);
nxt_motor_set_speed(R_MOTOR,0,0);
ecrobot_set_light_sensor_active(PORT_LIGHT);
//Bluetoothデバイス設定 デバイス名を新たに設定したい場合のみ、以下のコメントアウトをはずす。
//ecrobot_set_bt_device_name("NXT31");
ecrobot_init_bt_slave(BT_PASS_KEY); /* スレーブとして初期化 */
}
void ecrobot_device_terminate(){ /* OSEK終了時の処理 */
nxt_motor_set_speed(L_MOTOR,0,0);
nxt_motor_set_speed(R_MOTOR,0,0);
ecrobot_set_light_sensor_inactive(PORT_LIGHT);
}
void user_1ms_isr_type2(void){}
void sound_beep(){ /* ビープ音を鳴らすユーザ関数 */
ecrobot_sound_tone(600, 2, 50);
systick_wait_ms(20);
ecrobot_sound_tone(500, 5, 50);
systick_wait_ms(50);
}
TASK(Task1)
{
int speed=70;
float Kp, Kd;
int black,white,gray,light;
int err, err_prev ;
float turn;
float turn_p, turn_d;
//PIDゲイン
//限界感度を求める場合は Kp の値を少しづつ大きくしていく
Kp = 0.4;
Kd = 1.6;
black = BLACK;
white = WHITE;
gray = (black + white) / 2;
display_clear(0);
display_goto_xy(0, 1);
display_string("PUSH START");
display_update();
while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){ /* TSが押されたらスタート */
systick_wait_ms(10);
}
systick_wait_ms(500); /* 500msec待つ */
err_prev = ecrobot_get_light_sensor(PORT_LIGHT) - gray;
while(ecrobot_get_touch_sensor(PORT_TOUCH) == 0){ // タッチセンサが押されたらストップ
light = ecrobot_get_light_sensor(PORT_LIGHT);
err = light - gray;
//err_sum += err;
turn_p = Kp * err ;
turn_d = Kd * (err - err_prev);
turn = turn_p + turn_d ;
err_prev = err;
nxt_motor_set_speed( L_MOTOR,speed - turn, 1 );
nxt_motor_set_speed( R_MOTOR,speed + turn, 1 );
ecrobot_bt_data_logger((int)turn_p , (int)turn_d ); /* turn 値の P成分とD成分を送信*/
systick_wait_ms(10); /* 10msec待つ */
}
nxt_motor_set_speed(L_MOTOR,0,1);
nxt_motor_set_speed(R_MOTOR,0,1);
systick_wait_ms(1000); /* 1秒待つ */
TerminateTask(); /* 処理終了 */
}
|
上記のプログラムでは、デバイスの初期化 ecrobot_device_initialize で、スレーブとしての初期化を行い、Main Task 中の走行制御の無限ループの中で 10msec 周期で、ターン値とその微分成分を送信しています。
ecrobot_bt_data_logger関数でユーザが明示的に送信できる変数は２つだけですが、送信時間、バッテリ電圧、モータ回転速度、各ポートのセンサデータが送信されます。