TOPへ 2.リモコンロボットの作成

1.セグウェイ風ロボットの作成


応用編では、高度なプログラムの紹介と、簡単な解説をしていきます。
高度なプログラムに触れることで、NXTの可能性を感じてください。さらに、自由な発想で、プログラムを改良、応用しましょう。
この章では、NXTway-GS C APIを紹介します。

1.ロボットの組み立て

新たなロボットを組み立てます。
ここに載っているBuilding Instructions(組み立て手順書)を参考に、ロボットを組み立てます。
このページでは、Motorcycle tiresを用いた、narrow treadのロボットを例として解説します。
 ※Motorcycle tiresを持っていない人は、standard tiresを用いましょう。

このようなロボットを2輪型倒立振子ロボットと呼びます。




2.プログラム

ロボットがその場で倒立するプログラムです。
動作させるには以下のファイルが必要です。
Makefile
balancer_param.c
nxt_config.h
nxtway1.c
nxtway1.oil

ソースファイルはこのようになっています。
nxtway1.c
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"
#include "balancer.h"
#include "nxt_config.h"

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を宣言 */

void ecrobot_device_initialize(){
	nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
	nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
}

void ecrobot_device_terminate(){
	nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
	nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
}

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_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);
	
	TerminateTask();
}


このプログラムには、NXTway-GS C APIを用いています。
このAPIを実装することで、容易に倒立制御を実現できます。
解説を以下に示します。

balance_control(…)
倒立制御をする関数です。戻り値のPWM値をモータに送ると、ロボットが倒立します。
引数は、
args_cmd_forward: 前進/後進命令。100(前進最大値)〜-100(後進最大値)
args_cmd_turn: 旋回命令。100(右旋回最大値)〜-100(左旋回最大値)
args_gyro: ジャイロセンサ値
args_gyro_offset: ジャイロセンサオフセット値
args_theta_m_l: 左モータエンコーダ値
args_theta_m_r: 右モータエンコーダ値
args_battery: バッテリ電圧値(mV)
です。
戻り値は、
ret_pwm_l: 左モータPWM出力値
ret_pwm_r: 右モータPWM出力値
です。

※この関数は4msec毎に呼び出すことを前提に作られています。


balance_init()
NXTway-GS用バランス制御初期化関数です。内部状態量変数を初期化します。
使用するときは、併せてロータリーエンコーダも初期化します。


前進指令値(forward)や、旋回指令値(turn)を変えることで、自由にロボットを操作できます。
前進指令値、旋回指令値ともに-100〜100までです。
機能の付加などにも挑戦してみましょう。



TOPへ 2.リモコンロボットの作成