送信用と、受信用の２台のNXTが必要です。
受信側には、基礎編で作ったロボットをそのまま使います。
送信側も、基礎編で作ったロボットをそのまま使います。また、前章の倒立ロボットでもかまいません。
1．受信側プログラム
受信側プログラムは、下の4つのファイルからなります。
makefile
nxt_config.h
remote_sla1.c
remote_sla1.oil
"remote_sla1.c"の記述は以下のようになっています。
makefile
nxt_config.h
remote_sla1.c
remote_sla1.oil
"remote_sla1.c"の記述は以下のようになっています。
remote_sla1.c
受信用プログラムです。
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"
#include "nxt_config.h"
/* NXT Bluetooth configuration */
#define BT_PASS_KEY "1234" /* bluetoothパスキー */
#define BT_RCV_BUF_SIZE 32 /* bluetooth受信バッファの大きさ */
U8 bt_receive_buf[BT_RCV_BUF_SIZE]; /* bluetooth受信バッファの宣言 */
int forward=0,turn=0; //前進指令値、旋回値変数宣言
int pwm_L=0, pwm_R=0; //PWM値格納変数宣言
DeclareCounter(SysTimerCnt);
DeclareTask(Task_bg); /* Task_bgを宣言 */
DeclareTask(Task_drive); /* Task_balanceを宣言 */
DeclareTask(Task_LCD); /* Task_balanceを宣言 */
void ecrobot_device_initialize(){
nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
ecrobot_init_bt_slave(BT_PASS_KEY); /* スレーブとして初期化 */
}
void ecrobot_device_terminate(){
nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
ecrobot_term_bt_connection(); /* bluetooth通信終了 */
}
void user_1ms_isr_type2(void) {
StatusType ercd;
ercd = SignalCounter(SysTimerCnt);
if (ercd != E_OK)
{
ShutdownOS(ercd);
}
}
TASK(Task_drive){ //走るタスク
pwm_L = forward + turn;
if(pwm_L > 100)
pwm_L = 100;
if(pwm_L < -100)
pwm_L = -100;
pwm_R = forward - turn;
if(pwm_R > 100)
pwm_R = 100;
if(pwm_R < -100)
pwm_R = -100;
nxt_motor_set_speed(PORT_MOTOR_L, pwm_L, 1); //モータに指令を出す
nxt_motor_set_speed(PORT_MOTOR_R, pwm_R, 1);
TerminateTask();
}
TASK(Task_LCD){
display_clear(0);
display_goto_xy(0,1);
display_string("fwd="); /* 変数の内容を表示 */
display_goto_xy(8,1);
display_int((S8)bt_receive_buf[0], 4);
display_update();
display_goto_xy(0,2);
display_string("trn="); /* 変数の内容を表示 */
display_goto_xy(8,2);
display_int((S8)bt_receive_buf[1], 4);
display_update();
TerminateTask();
}
TASK(Task_bg){
nxt_motor_set_count(PORT_MOTOR_L, 0);
nxt_motor_set_count(PORT_MOTOR_R, 0);
while(1){
ecrobot_read_bt_packet(bt_receive_buf, BT_RCV_BUF_SIZE); /* 受信したものを受信バッファに代入 */
forward=(S8)bt_receive_buf[0];
turn=(S8)bt_receive_buf[1];
systick_wait_ms(1);
}
TerminateTask();
}
|
2．送信側プログラム
送信側プログラムは、下の4つのファイルからなります。
makefile
nxt_config.h
remote_mas.c
remote_mas.oil
"remote_mas.c"の記述は以下のようになっています。
送信側ロボットのタイヤを手で回してみましょう。ロボットが走ったり、曲がったりします。
タイヤが自動的に中心に戻るようにしていますが、出来が微妙です。
是非、改良しましょう。
ロータリーエンコーダだけでなく、他のセンサ（加速度センサなど）を使って操縦できるようにしてみましょう。
makefile
nxt_config.h
remote_mas.c
remote_mas.oil
"remote_mas.c"の記述は以下のようになっています。
remote_mas.c
Bluetooth通信を用いているので、受信側（スレーブ）から先に起動し、画面に<BT>の表示が出てから実行してください。
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"
#include "nxt_config.h"
/* NXT Bluetooth configuration */
#define BT_PASS_KEY "1234" /* bluetoothパスキー */
#define BT_SNV_BUF_SIZE 32 /* bluetooth送信バッファの大きさ */
const U8 BD_ADDR[7] = {0x00,0x16,0x53,0x06,0x89,0xB5,0x00}; /* スレーブNXTのデバイスアドレス */
//const U8 BD_ADDR[7] = {0x00,0x16,0x53,0x01,0x85,0x83,0x00}; /* スレーブNXTのデバイスアドレス */
U8 bt_send_buf[BT_SNV_BUF_SIZE]; /* bluetooth送信バッファの宣言 */
int forward,turn,rev_l,rev_r;
DeclareCounter(SysTimerCnt); /* SysTimerCntを宣言 */
DeclareTask(Task_Feedback); /* Task_Feedbackを宣言 */
DeclareTask(Task_LCD); /* Task_LCDを宣言 */
DeclareTask(Task_Ctrl); /* Task_Ctrlを宣言 */
void ecrobot_device_initialize(){
ecrobot_init_bt_master(BD_ADDR, BT_PASS_KEY); /* マスターとして初期化 */
}
void ecrobot_device_terminate(){
ecrobot_term_bt_connection(); /* bluetooth通信終了 */
}
void user_1ms_isr_type2(void){
StatusType ercd;
ercd = SignalCounter(SysTimerCnt);
if (ercd != E_OK)
{
ShutdownOS(ercd);
}
}
TASK(Task_Feedback){ /* つまみが自動で中央に戻る */
rev_l=nxt_motor_get_count(PORT_MOTOR_L);
if(rev_l>=-5||rev_l<=5)
nxt_motor_set_speed(PORT_MOTOR_L, 0, 0);
if(rev_l>5)
nxt_motor_set_speed(PORT_MOTOR_L, -55, 0);
if(rev_l<-5)
nxt_motor_set_speed(PORT_MOTOR_L, 55, 0);
rev_r=nxt_motor_get_count(PORT_MOTOR_R);
if(rev_r>=-5||rev_r<=5)
nxt_motor_set_speed(PORT_MOTOR_R, 0, 0);
if(rev_r>5)
nxt_motor_set_speed(PORT_MOTOR_R, -55, 0);
if(rev_r<-5)
nxt_motor_set_speed(PORT_MOTOR_R, 55, 0);
TerminateTask();
}
TASK(Task_LCD){
display_clear(0);
display_goto_xy(0,1);
display_string("fwd= "); /* 変数の内容を表示 */
display_int((S8)bt_send_buf[0], 4);
display_goto_xy(0,2);
display_string("trn= "); /* 変数の内容を表示 */
display_int((S8)bt_send_buf[1], 4);
display_update();
TerminateTask(); /* 処理終了 */
}
TASK(Task_Ctrl)
{
nxt_motor_set_count(PORT_MOTOR_L, 0);
nxt_motor_set_count(PORT_MOTOR_R, 0);
while(1){
forward = nxt_motor_get_count(PORT_MOTOR_L); /* 入力倍率 = 1 */
if(forward>100){
forward=100;
nxt_motor_set_count(PORT_MOTOR_L, 100); /* 100〜-100におさめる */
}
if(forward<-100){
forward=-100;
nxt_motor_set_count(PORT_MOTOR_L, -100);
}
turn = nxt_motor_get_count(PORT_MOTOR_R)/2; /* 入力倍率 = 1/2 */
if(turn>100){
turn=100;
nxt_motor_set_count(PORT_MOTOR_R, 200); /* 100〜-100におさめる */
}
if(turn<-100){
turn=-100;
nxt_motor_set_count(PORT_MOTOR_R, -200);
}
bt_send_buf[0]=forward; /* 送信バッファに代入 */
bt_send_buf[1]=turn;
ecrobot_send_bt_packet(bt_send_buf, BT_SNV_BUF_SIZE); /* 送信バッファの中身を送信 */
systick_wait_ms(5);
}
TerminateTask(); /* 処理終了 */
}
|
送信側ロボットのタイヤを手で回してみましょう。ロボットが走ったり、曲がったりします。
タイヤが自動的に中心に戻るようにしていますが、出来が微妙です。
是非、改良しましょう。
ロータリーエンコーダだけでなく、他のセンサ（加速度センサなど）を使って操縦できるようにしてみましょう。
※おまけ．倒立ロボットの操縦
倒立ロボットを操縦できるプログラムを以下に示します。
送信側プログラムは、上のものがそのまま使えますが、入力倍率などを変更したほうが良いかもしれません。
動作させるには以下のファイルが必要です。
makefile
balancer_param.c
nxt_config.h
remote_sla2.c
remote_sla2.oil
ソースファイルはこのようになっています。
送信側プログラムは、上のものがそのまま使えますが、入力倍率などを変更したほうが良いかもしれません。
動作させるには以下のファイルが必要です。
makefile
balancer_param.c
nxt_config.h
remote_sla2.c
remote_sla2.oil
ソースファイルはこのようになっています。
remote_sla2.c
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"
#include "balancer.h"
#include "nxt_config.h"
/* NXT Bluetooth configuration */
#define BT_PASS_KEY "1234" /* bluetoothパスキー */
#define BT_RCV_BUF_SIZE 32 /* bluetooth受信バッファの大きさ */
U8 bt_receive_buf[BT_RCV_BUF_SIZE]; /* bluetooth受信バッファの宣言 */
int forward=0,turn=0; //前進指令値、旋回値変数宣言
signed char pwm_L=0, pwm_R=0; //PWM値格納変数宣言
int gyro_offset=0; //ジャイロオフセット値格納変数宣言
DeclareCounter(SysTimerCnt);
DeclareTask(Task_bg); /* Task_bgを宣言 */
DeclareTask(Task_balance); /* Task_balanceを宣言 */
DeclareTask(Task_LCD); /* Task_balanceを宣言 */
void ecrobot_device_initialize(){
nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
ecrobot_init_bt_slave(BT_PASS_KEY); /* スレーブとして初期化 */
}
void ecrobot_device_terminate(){
nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
ecrobot_term_bt_connection(); /* bluetooth通信終了 */
}
void user_1ms_isr_type2(void) {
StatusType ercd;
ercd = SignalCounter(SysTimerCnt);
if (ercd != E_OK)
{
ShutdownOS(ercd);
}
}
TASK(Task_blance){ //バランスタスク
balance_control( //バランサAPIの呼び出し
(float)forward,
(float)turn,
(float)ecrobot_get_gyro_sensor(PORT_GYRO),
(float)gyro_offset,
(float)nxt_motor_get_count(PORT_MOTOR_L),
(float)nxt_motor_get_count(PORT_MOTOR_R),
(float)ecrobot_get_battery_voltage(),
&pwm_L,
&pwm_R
);
nxt_motor_set_speed(PORT_MOTOR_L, pwm_L, 1); //モータに指令を出す
nxt_motor_set_speed(PORT_MOTOR_R, pwm_R, 1);
TerminateTask();
}
TASK(Task_LCD){
display_clear(0);
display_goto_xy(0,1);
display_string("fwd="); /* 変数の内容を表示 */
display_goto_xy(8,1);
display_int((S8)bt_receive_buf[0], 4);
display_update();
display_goto_xy(0,2);
display_string("trn="); /* 変数の内容を表示 */
display_goto_xy(8,2);
display_int((S8)bt_receive_buf[1], 4);
display_update();
TerminateTask();
}
TASK(Task_bg){
gyro_offset = ecrobot_get_gyro_sensor(PORT_GYRO); //ジャイロの初期値補正
balance_init(); //バランサAPIの初期化
nxt_motor_set_count(PORT_MOTOR_L, 0);
nxt_motor_set_count(PORT_MOTOR_R, 0);
while(1){
ecrobot_read_bt_packet(bt_receive_buf, BT_RCV_BUF_SIZE); /* 受信したものを受信バッファに代入 */
forward=(S8)bt_receive_buf[0];
turn=(S8)bt_receive_buf[1];
systick_wait_ms(1);
}
TerminateTask();
}
|