で、立たないんだけど。。。

で、立たないんだけど。。。

1. [舟元]これこれ、なんだか分かる？

1. [吉田]えーっと。ジャイロセンサだっけ。
2. [舟元]そうそう。このセンサで、走行体が一秒間に何度傾いたのかを検出するんだ。この値をもとにして、走行体のモーターを回すことでバランスを取るんだよ。

→　参照：マインドストームに使用するジャイロセンサのページ

1. [吉田]ふふ。if文でしょ。

if (前に倒れそうになったら) {
    前進する();
} else if (後ろに倒れそうになったら) {
    後退する();
}

1. [舟元]そんなに簡単にはいかないんだなー。前傾しているか、後傾しているか、だけだったらif文だけで良いんだけどね。実際には、どの程度傾いているとか、自立させるのと同時に倒れないように前進したいとか、曲がりたいとか、いろいろな要因を考慮して、モーターをどのくらい、どの方向に回せば良いかを決めないといけないんだ。

走行体をソフトウェアを使って自立させる場合を考えると、図のように「物理現象（傾き）をセンサで読み取り、制御量（どの程度モーターを回すか）を決定し、実際にモーターを動かす」ということを繰り返します。「実際にモーターを動かすと、その影響で物理現象（傾き）が変わる」ということも分かります。このように、「何かした（モーターを回した）結果を、次の入力（センサで読み取る傾き）とする」制御を、フィードバック制御と言います。

1. [吉田]え？計算？
2. [舟元]そうそう。その計算をやってくれるのが balance_control() っていう関数なんだよ。

例えばその場で静止させようとするだけでも、図のように、大きく前傾したら大きく前進させ、小さく前傾したら小さく前進させないと自立できません。倒立振子ライブラリの balance_control() 関数は、ジャイロセンサでの測定値などを引数に受け、左右のモーターをどれだけ動かせば良いかを計算してくれます。

倒立振子制御に関する詳細は、nxtOSEKの公式サイト内に NXTway-GS 倒立振子制御 C API 解説書 として公開されています。

1. [舟元]倒立振子ライブラリには、いろいろと調整できるパラメータもあるけど、まずは走行体を立たせて、ライントレースできるようになるほうを優先しよっか。
2. [吉田]うん。タッチセンサを押したら自立して、もう一度タッチセンサを押すと終わるのを作ってから徐々にレベルアップしていこう。

いざ倒立っ！！

1. [吉田]今回は、standupっていう名前にしておくね。必要なファイルは、Makefileとstandup.oil、standup.cかな。
2. [舟元]そうだね。あと、さっき話した倒立振子ライブラリのパラメータを設定しないといけないんだけど、これは nxtOSEK のサンプルの中に使い回せるファイルがあるから、それを使おう。
3. [吉田]よーし。書くぞー。

Makefile:

# 生成するファイル名
TARGET = standup

# コンパイルするソースファイル名
TARGET_SOURCES = standup.c balancer_param.c

# OILのファイル名
TOPPERS_OSEK_OIL_SOURCE = ./standup.oil

# 倒立振子ライブラリの利用
USER_INC_PATH= ../../../../nxtOSEK/ecrobot/nxtway_gs_balancer
USER_LIB = nxtway_gs_balancer

O_PATH ?= build
include ../../../../nxtOSEK/ecrobot/ecrobot.mak

standup.oil:

#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;
  };

  APPMODE appmode1{};

  TASK Main {
    AUTOSTART = TRUE {
      APPMODE = appmode1;
    };
    PRIORITY = 1;
    ACTIVATION = 1;
    SCHEDULE = FULL;
    STACKSIZE = 512;
  };
};

standup.c:

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

#define PORT_MOTOR_L NXT_PORT_C
#define PORT_MOTOR_R NXT_PORT_B

#define PORT_TOUCH NXT_PORT_S4
#define PORT_GYRO NXT_PORT_S1

#define GYRO_OFFSET 592

void ecrobot_device_initialize() { }
void ecrobot_device_terminate() { }
void user_1ms_isr_type2(){ }

TASK(Main) {
    S8 pwm_l, pwm_r;
    S8 forward = 0, turn = 0;

    nxt_motor_set_count(PORT_MOTOR_L, 0);
    nxt_motor_set_count(PORT_MOTOR_R, 0);
    balance_init();

    while (1) { // 開始チェック
        if (ecrobot_get_touch_sensor(PORT_TOUCH) == 1) {
            break;
        }
    }

    while (1) {
        balance_control(
                forward, turn,
                ecrobot_get_gyro_sensor(PORT_GYRO),
                GYRO_OFFSET,
                nxt_motor_get_count(PORT_MOTOR_L),
                nxt_motor_get_count(PORT_MOTOR_R),
                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);

        systick_wait_ms(4);

        if (ecrobot_get_touch_sensor(PORT_TOUCH) == 1) { // 終了チェック
            break;
        }
    }
    nxt_motor_set_speed(PORT_MOTOR_L, 0, 0);
    nxt_motor_set_speed(PORT_MOTOR_R, 0, 0);
    while (1) {
        systick_wait_ms(100);
    }
}

balancer_param.c:

（nxtOSEKに含まれる samples_c/nxtway_gs/balancer_param.c をコピーしてください。）

1. [吉田]うん。こんな感じかな。動かしてみよぉっと。転送して、起動して、よし。タッチセンサを押すと。。。あれ、、、ん？動かない。
2. [舟元]んーっと、、、これ、ちゃんとプログラムしたとおり動いているよ。
3. [吉田]えっ！？だって、モーターも回らないし動いてないよ。
4. [舟元]タッチセンサを一回押したら自立開始ってしたかったんだけど、一回押しただけで終了しちゃっているんだね。
5. [吉田]？？

上記のプログラムでは、タッチセンサが押されている状態を確認しています。図のように人間が一度タッチセンサを押している間に、走行体の中では、開始チェックと終了チェックの二度タッチセンサが押されているかを確認してしまうため、自立する前に終了してしまっています。人間が操作するために必要な時間と、走行体（コンピュータ）が処理する時間のスケールが違うために発生してしまう現象です。

1. [舟元]タッチセンサのチェックする関数を作ったから、これに置き換えてみよう。

タッチセンサをチェックする関数:

/**
 * タッチセンサが放された瞬間に1を返します。
 * それ以外では0を返します。
 */
U8 isPushed() {
   static U8 oldStatus = 0;
   U8 currentStatus = ecrobot_get_touch_sensor(PORT_TOUCH);

   if (oldStatus == 1 && currentStatus == 0) {
       // 前回押されていて、今回放されていたら、1を返す
       oldStatus = currentStatus;
       return 1;
   } else {
       oldStatus = currentStatus;
       return 0;
   }
}

開始チェック部分:

while (1) {
    if (isPushed() == 1) {
        break;
    }
    systick_wait_ms(100);
}

終了チェック部分:

if (isPushed() == 1) {
    break;
}

1. [吉田]そうかー。押されている最中をif文の条件にしちゃうと、押されている間はずーっと条件が成り立っちゃうもんね。押された瞬間、放された瞬間にするとうまくいくのか。組み込みは奥が深いなー。