NXTway-GS(2輪型倒立振子ロボット) C API

 

2輪型倒立振子ロボット(通称: NXTway)はNXTを使用した制御アプリケーションのうち、最も難易度の高いもの(?)の一つです。2輪型倒立振子を上手くバランスさせながら走行させる技術は、米国のSegwayを筆頭に日立製作所のEMIEWシリーズや、最近ではトヨタ自動車のWingletなどにも応用されています。通常、この技術には「現代制御」などの制御理論が用いられており、複雑な運動方程式の導出や最適な制御パラメータの算出等が必要になります。今回紹介するNXTway-GS(HiTechnic社製Gyro Sensor搭載)では制御の専門知識が無くても、手順書に沿って組み立てたNXTway-GS筐体に対して、NXTway-GS C APIを適切に使用するだけでロボットのバランス走行を実現することができます。 また、NXTway-GSの筐体およびサンプルプログラムを2輪型倒立振子ロボット拡張アプリケーションのベースとしても使用することができます。

 

組み立て手順書: NXTway-GS_Building_Instructions.pdf

最新のNXTway-GS筐体では以前のバージョンと比較して、組み立ての容易化、使用部品の削減、組み付け剛性のアップ、そして次のバリエーションに対応しています:
- NXT標準タイヤおよびRCX用モータサイクルタイヤ
- ナロートレッドおよびワイドトレッド
上記のバリエーションのうち、RCX用モータサイクルタイヤ+ワイドトレッドの組み合わせで最も安定度が高くなります。なお、RCX用モータサイクルタイヤはNXTの標準キット/教育キットには含まれていないため、拡張キットが別途必要になります。

 

NXTway-GS C API: NXTway_GS_C_API.pdf

NXTway-GS C APIは、バランス制御関数(balance_control)と初期化関数(balance_init)という2つの関数を提供しています。バランス制御関数は4msec周期で実行されることを前提に設計されています。また初期化関数では内部状態変数の初期化をおこなっています。これら2つのC関数は、ハンドコーディングによって実装されたのではなく、The MathWorks社のMATLAB&Simulinkというブロック線図ベースのモデリング/シミュレーションソフトウェア上で設計し、Real-Time Workshop Embedded CoderというCコード自動生成ツールによって実装されています。こちらにNXTway-GSのMATLAB&Simulinkモデルベース開発環境および制御方法(サーボ制御:状態+積分フィードバック)の詳細解説が含まれた日本語PDF資料が公開されています。 NXTway-GS C APIはCライブラリの形で配布していますが、自動生成されたCソースファイル一式はnxtOSEK\ecrobot\nxtway_gs_balancerディレクトリに格納されています。また、MATLAB&SimulinkがなくてもNXTway-GS C APIコード生成用Simulinkモデルをみれるように、上記ディレクトリ内にモデルがHTMLファイル形式で格納されています(モデルを表示するにはSVG対応のWebブラウザが必要になります)。なお、NXTway-GS C APIを使用する際にはbalancer.h ファイルをCソースコードにインクルードする必要があります。

NXTway-GS C API
説明

void
balance_control(
    F32 args_cmd_forward,
    F32 args_cmd_turn,
    F32 args_gyro,
    F32 args_gyro_offset,
    F32 args_theta_m_l,
    F32 args_theta_m_r,
    F32 args_battery,
    S8 *ret_pwm_l,
    S8 *ret_pwm_r)

NXTway-GS用バランス制御関数。 この関数は4msec周期で起動されることを前提に設計されています。なお、ジャイロセンサ オフセット値はセンサ個体および通電によるドリフトを伴いますので、適宜補正する必要があります。また、左右の車輪駆動 モータは個体差により、同じ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出力値

void
balance_init(void)

NXTway-GS用バランス制御初期化関数。内部状態量変数を初期化します。 この関数によりバランス制御機能を初期化する場合は、併せて左右の車輪駆動モーターのエンコーダ値を0にリセットしてください。

引数:
    無し
戻り値:
    無し

 

サンプルプログラム:

NXTway-GS C APIを使用したサンプルプログラムはBluetooth通信を介したR/Cロボットアプリケーションで、PC用ゲームパッドコントローラによってラジコン操作がおこなえます。ロボットのラジコン操作を実現するには、PC側にNXT GamePadというソフトウェアを別途インストールする必要があります。サンプルプログラムでは更に超音波センサを使用した障害物回避機能も実装されており、ロボットの前方約25cmの範囲内に障害物がある場合、ロボットは自動的に後進して障害物を回避します。サンプルプログラムはRCXモータサイクルタイヤ装着用に設定されているため、NXT標準タイヤを使用する場合は、下記のコンパイラマクロ定義(USER_DEF = NXT_STD_TIRE)をサンプルMakefileに追加する必要があります。NXTway-GS C APIを使用したバランス制御性能は制御パラメータの値によって変わります。大抵の場合はデフォルトの制御パラメータ値で上手く制御できるはずですが、NXTの個体差補償等のためのチューニング用に制御パラメータはNXTway-GS C APIライブラリの一部ではなく、balancer_param.c というサンプルプログラムとして提供しています。

samples\nxtway_gs\Makefile

# Target specific macros
TARGET = NXTway_GS

NXTOSEK_ROOT = ../..

# nxtway_gs_balancer library desiged for NXTway-GS two wheeled self-balancing robot
USER_INC_PATH = $(NXTOSEK_ROOT)/ecrobot/nxtway_gs_balancer
USER_LIB = nxtway_gs_balancer

# using NXT standard tires (not Motorcycle tires)
#USER_DEF = NXT_STD_TIRE <-NXT標準タイヤを使用する場合はこのマクロ定義を有効にする

# User application source
TARGET_SOURCES := \
    balancer_param.c \      
    nxtway_gs_main.c

# OSEK OIL file
TOPPERS_OSEK_OIL_SOURCE := ./nxtway_gs.oil

# below part should not be modified
O_PATH ?= build
include $(NXTOSEK_ROOT)/ecrobot/lejos_osek.tmf

samples\nxtway_gs\nxt_config.h

/**
 *******************************************************************************
 ** FILE NAME : nxt_config.h
 **
 ** ABSTRUCT : NXT device configration
 *******************************************************************************
 **/

#ifndef _NXT_CONFIG_H_
#define _NXT_CONFIG_H_

#include "ecrobot_interface.h"

/* NXT motor port configuration */
#define PORT_MOTOR_R NXT_PORT_B
#define PORT_MOTOR_L NXT_PORT_C
/* NXT sensor port configuration */
#define PORT_SONAR   NXT_PORT_S2
#define PORT_GYRO    NXT_PORT_S4
/* NXT Bluetooth configuration */
#define BT_PASS_KEY  "1234" <-Bluetooth通信接続用パスキーの定義
#endif

samples\nxtway_gs\nxtway_gs_main.c

/**
 ******************************************************************************
 ** FILE NAME : nxtway_gs_main.c
 **
 ** ABSTRUCT  : NXTway-GS (with a HiTechnic Gyro Sensor)
 **             Two wheeled self-balancing R/C robot controlled via Bluetooth.
 ******************************************************************************
 **/

#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"

#include "balancer.h" /* NXTway-GS C API header file */ <-NXTway-GS C API用ヘッダファイル
#include "nxt_config.h"

/*============================================================================
 * MACRO DEFINITIONS
 *===========================================================================*/
typedef enum {
  INIT_MODE,   /* system initialize mode */
  CAL_MODE,    /* gyro sensor offset calibration mode */
  CONTROL_MODE /* balance and RC control mode */
} MODE_ENUM;

#define MIN_DISTANCE (25)    /* minimum distance in cm for obstacle avoidance */
#define BT_RCV_BUF_SIZE (32) /* it must be 32bytes with NXT GamePad */

/*============================================================================
 * DATA DEFINITIONS
 *===========================================================================*/

MODE_ENUM nxtway_gs_mode = INIT_MODE; /* NXTway-GS mode */
volatile U8 obstacle_flag;       /* 1(obstacle detected)/0(no obstacle) */

/*============================================================================
 * FUNCTIONS
 *===========================================================================*/
/*============================================================================
 * Embedded Coder Robot hook functions
 *===========================================================================*/
//*****************************************************************************
// FUNCTION    : ecrobot_device_initialize
// ARGUMENT    : none
// RETURN      : none
// DESCRIPTION : ECRobot device init hook function
//*****************************************************************************

void ecrobot_device_initialize(void)
{
  ecrobot_init_sonar_sensor(PORT_SONAR);
  ecrobot_init_bt_slave(BT_PASS_KEY);
}

//*****************************************************************************
// FUNCTION    : ecrobot_device_terminate
// ARGUMENT    : none
// RETURN      : none
// DESCRIPTION : ECRobot device term hook function
//*****************************************************************************

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

/*============================================================================
 * TOPPERS ATK specific Function/Tasks
 *===========================================================================*/

DeclareCounter(SysTimerCnt);

//*****************************************************************************
// FUNCTION    : user_1ms_isr_type2
// ARGUMENT    : none
// RETURN      : none
// DESCRIPTION : 1msec periodical OSEK type 2 ISR
//*****************************************************************************

void user_1ms_isr_type2(void)
{
  /* Increment System Timer Count to activate periodical Tasks */
  (void)SignalCounter(SysTimerCnt);
}

//*****************************************************************************
// TASK        : OSEK_Task_ts1
// ARGUMENT    : none
// RETURN      : none
// DESCRIPTION : 4msec periodical Task and controls NXTway-GS
//               INIT_MODE
//               ↓
//               CAL_MODE
//               ↓
//               CONTROL_MODE
//*****************************************************************************

TASK(OSEK_Task_ts1)
{
  static U32 gyro_offset;    /* gyro sensor offset value */
  static U32 avg_cnt;        /* average counter to calc gyro offset */
  static U32 cal_start_time; /* calibration start time */
  static U8 bt_receive_buf[BT_RCV_BUF_SIZE]; /* Bluetooth receive buffer(32bytes) */
  SINT i;
  S8 cmd_forward,cmd_turn;
  S8 pwm_l,pwm_r;

  switch(nxtway_gs_mode){
    case(INIT_MODE):
      gyro_offset = 0;
      avg_cnt = 0;
      for (i = 0; i < BT_RCV_BUF_SIZE; i++){
        bt_receive_buf[i] = 0;
      }
      balance_init(); /* NXTway-GS C API initialize function */ <-バランス制御初期化関数の呼び出し
      nxt_motor_set_count(PORT_MOTOR_L, 0); /* reset left motor count */
      nxt_motor_set_count(PORT_MOTOR_R, 0); /* reset right motor count */
      cal_start_time = ecrobot_get_systick_ms();
      nxtway_gs_mode = CAL_MODE;
      break;

    case(CAL_MODE):
      gyro_offset += (U32)ecrobot_get_gyro_sensor(PORT_GYRO);
      avg_cnt++;
      /* 1000msec later, start balancing */
      if ((ecrobot_get_systick_ms() - cal_start_time) >= 1000U){
        gyro_offset /= avg_cnt;
        ecrobot_sound_tone(440U, 500U, 30); /* beep a tone */
        nxtway_gs_mode = CONTROL_MODE;
      }
      break;

    case(CONTROL_MODE):
      /*
       * R/C command from NXT GamePad
       * buf[0]: -100(forward max.) to 100(backward max.)
       * buf[1]: -100(turn left max.) to 100(turn right max.)
       */
      (void)ecrobot_read_bt_packet(bt_receive_buf, BT_RCV_BUF_SIZE);
      cmd_forward = -(S8)bt_receive_buf[0]; /* reverse the direction */
      cmd_turn = (S8)bt_receive_buf[1];
      if (obstacle_flag == 1){
        /* make NXTway-GS move backward to avoid obstacle */
        cmd_forward = -100;
        cmd_turn = 0;
      }

      /* NXTway-GS C API balance control function (has to be invoked in 4msec period) */
      
balance_control( <-バランス制御関数の呼び出し
        (F32)cmd_forward,
        (F32)cmd_turn,
        (F32)ecrobot_get_gyro_sensor(PORT_GYRO),
        (F32)gyro_offset,
        (F32)nxt_motor_get_count(PORT_MOTOR_L),
        (F32)nxt_motor_get_count(PORT_MOTOR_R),
        (F32)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);

      ecrobot_bt_data_logger(cmd_forward, cmd_turn); /* Bluetooth data logger */
      break;

    default:
      /* unexpected mode */
      nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
      nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
      break;
  }

  TerminateTask(); /* terminates this task and executed by System Timer Count */
}

//*****************************************************************************
// TASK        : OSEK_Task_ts2
// ARGUMENT    : none
// RETURN      : none
// DESCRIPTION : 40msec periodical Task and detects obstacle in front of
//               NXTway-GS by using a sonar sensor
//*****************************************************************************

TASK(OSEK_Task_ts2)
{
  obstacle_flag = 0; /* no obstacles */
  if ((nxtway_gs_mode == CONTROL_MODE) &&
    (ecrobot_get_sonar_sensor(PORT_SONAR) <= MIN_DISTANCE)){
     obstacle_flag = 1; /* obstacle detected */
  }

  TerminateTask(); /* terminates this task and executed by System Timer Count */
}

//*****************************************************************************
// TASK        : OSEK_Task_Background
// ARGUMENT    : none
// RETURN      : none
// DESCRIPTION : Background(never terminated) Task
//*****************************************************************************

TASK(OSEK_Task_Background)
{
  while(1){
    ecrobot_status_monitor("NXTway-GS"); /* LCD display */
    systick_wait_ms(500); /* 500msec wait */
  }
}

/******************************** END OF FILE ********************************/

samples\nxtway_gs\balancer_param.c

/**
 *******************************************************************************
 ** FILE NAME : balancer_param.c
 **
 ** ABSTRUCT  : NXTway-GS balance control parameters
 **
 ** NOTE: These parameters definitely affect the balance control of NXTway-GS.
 *******************************************************************************
 **/
#include "ecrobot_interface.h"

/*============================================================================
 * DATA DEFINITIONS
 *===========================================================================*/

F32 A_D = 0.8F;   /* low pass filter gain for motors average count */
F32 A_R = 0.996F; /* low pass filter gain for motors target count */

/*
 * NOTE: When NXT standard tires are used for NXTway-GS, a compiler macro (NXT_STD_TIRE)
 * has to be added to USER_DEF macro definition in user Makefile
 * E.g. USER_DEF = NXT_STD_TIRE
 */

#ifdef NXT_STD_TIRE
  /* state feedback gain for NXT standard tire */
  F32 K_F[4] = {-0.834434F, -38.1749F, -1.0985F, -3.55477F};
#else
  /* state feedback gain for RCX Motorcycle tire */
  F32 K_F[4] = {-0.870303F, -31.9978F, -1.1566F, -2.78873F};
#endif

F32 K_I = -0.44721F;   /* servo control integral gain */
F32 K_PHIDOT = 25.0F;  /* turn target speed gain */
F32 K_THETADOT = 7.5F; /* forward target speed gain */

const
F32 BATTERY_GAIN = 0.001089F; /* battery voltage gain for motor PWM outputs */
const
F32 BATTERY_OFFSET = 0.625F;  /* battery voltage offset for motor PWM outputs */

/******************************** END OF FILE ********************************/

samples\nxtway_gs\nxtway_gs.oil

/**
 ******************************************************************************
 ** FILE NAME : nxtway_gs.oil
 **
 ** ABSTRUCT : OSEK OIL(OSEK Implementation Language) file for NXTway-GS
 ******************************************************************************
 **/
#include "implementation.oil"

CPU ATMEL_AT91SAM7S256
{
  OS LEJOS_OSEK
  {
    STATUS = EXTENDED;
    STARTUPHOOK = FALSE;
    ERRORHOOK = FALSE;
    SHUTDOWNHOOK = FALSE;
    PRETASKHOOK = FALSE;
    POSTTASKHOOK = FALSE;
    USEGETSERVICEID = FALSE;
    USEPARAMETERACCESS = FALSE;
    USERESSCHEDULER = FALSE;
  };

  /* Definition of application mode */
  APPMODE appmode1{};

  /* Definitions of a periodical task: OSEK_Task_ts1 */
  TASK OSEK_Task_ts1
  {
    AUTOSTART = FALSE;
    PRIORITY = 3;
    ACTIVATION = 1;
    SCHEDULE = FULL;
    STACKSIZE = 512; /* bytes */
  };
  ALARM OSEK_Alarm_task_ts1
  {
    COUNTER = SysTimerCnt;
    ACTION = ACTIVATETASK
    {
      TASK = OSEK_Task_ts1;
    };
    AUTOSTART = TRUE
    {
      APPMODE = appmode1;
      ALARMTIME = 1;
      CYCLETIME = 4; /* executed every 4msec */ <-4msec周期起動タスク
    };
  };

  /* Definitions of a periodical task: OSEK_Task_ts2 */
  TASK OSEK_Task_ts2
  {
    AUTOSTART = FALSE;
    PRIORITY = 2;
    ACTIVATION = 1;
    SCHEDULE = FULL;
    STACKSIZE = 512; /* bytes */
  };
  ALARM OSEK_Alarm_task_ts2
  {
    COUNTER = SysTimerCnt;
    ACTION = ACTIVATETASK
    {
      TASK = OSEK_Task_ts2;
    };
    AUTOSTART = TRUE
    {
      APPMODE = appmode1;
      ALARMTIME = 1;
      CYCLETIME = 40; /* executed every 40msec */
    };
  };

  /* Definition of background task: OSEK_Task_Background */
  TASK OSEK_Task_Background
  {
    AUTOSTART = TRUE
    {
      APPMODE = appmode1;
    };
    PRIORITY = 1; /* lowest priority */
    ACTIVATION = 1;
    SCHEDULE = FULL;
    STACKSIZE = 512; /* bytes */
  };

  /* Definition of OSEK Alarm Counter: SysTimerCnt */
  COUNTER SysTimerCnt
  {
    MINCYCLE = 1;
    MAXALLOWEDVALUE = 10000;
    TICKSPERBASE = 1;
  };

 

 

 

Back to Samples