【第4回】レゴマインドストームNXTでライントレーサを作成しよう

レゴマインドストームNXTでロボットプログラミング

コーディングして、動かしてみよう

　今回の設計に基づいたコーディング例は、以下の通りです。また、実際に動作している姿もお楽しみください。なお、今回は、黒のしきい値500、wait時間100ミリ秒という調整を行い、動作させています。

#include <time.h>
#include <stdio.h>
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"
#include "systick.h"

#define TOUCHED 0

/** 光センサのポート */
#define LIGHT_SENSOR_PORT NXT_PORT_S3
/** タッチセンサのポート */
#define BUMPER_SENSOR_PORT NXT_PORT_S1

/** 右モータ */
#define RIGHT_MOTOR_PORT NXT_PORT_A
/** 左モータ */
#define LEFT_MOTOR_PORT NXT_PORT_B
/** ライントレースかどうか */
#define LINE_TRACE 1
/** 停止する */
#define STOP 0

/** 黒のしきい値 */
#define IS_BLACK 500
/** Wait時間 */
#define WAIT_TIME 100

/*--------------------------------------------------------------------------*/
/* OSEK declarations                                                        */
/*--------------------------------------------------------------------------*/
DeclareCounter(SysTimerCnt);
DeclareResource(lcd);
DeclareTask(MainTask);

/*--------------------------------------------------------------------------*/
/* Globals                                                                  */
/*--------------------------------------------------------------------------*/
int pushedPls = 0;

/*--------------------------------------------------------------------------*/
/* ECRobot hooks                                                            */
/*--------------------------------------------------------------------------*/
const U8 target_subsystem_name[] = "DesignTest";

void ecrobot_device_initialize()
{
    /* write NXT device initializations */
    /** 光センサをONにする */
    ecrobot_set_light_sensor_active(LIGHT_SENSOR_PORT);
}

void ecrobot_device_terminate()
{
    /* write NXT device terminations */
}

/*--------------------------------------------------------------------------*/
/* OSEK hooks                                                               */
/*--------------------------------------------------------------------------*/
void StartupHook(void){}
void ShutdownHook(StatusType ercd){}
void PreTaskHook(void){}
void PostTaskHook(void){}
void ErrorHook(StatusType ercd){}

/*--------------------------------------------------------------------------*/
/* Function to be invoked from a category 2 interrupt                       */
/*--------------------------------------------------------------------------*/
void user_1ms_isr_type2(void)
{
    StatusType ercd;
    /*
     *  Increment OSEK Alarm System Timer Count
     */
    ercd = SignalCounter( SysTimerCnt );
    if( ercd != E_OK ){
        ShutdownOS( ercd );
    }
}

/**
 * LeftTire_front
 * 左のタイヤを正転させます．
 *
 */
void LeftTire_front(int speed_percent)
{
    nxt_motor_set_speed(LEFT_MOTOR_PORT, speed_percent, 1);
}

/**
 * LeftTire_stop
 * 左のタイヤを停止させます．
 *
 */
void LeftTire_stop()
{
    nxt_motor_set_speed(LEFT_MOTOR_PORT, 0, 1);
}

/**
 * RightTire_front
 * 右のタイヤを正転させます．
 *
 */
void RightTire_front(int speed_percent)
{
    nxt_motor_set_speed(RIGHT_MOTOR_PORT, speed_percent, 1);
}

/**
 * RightTire_stop
 * 右のタイヤを停止させます．
 *
 */
void RightTire_stop()
{
    nxt_motor_set_speed(RIGHT_MOTOR_PORT, 0, 1);
}

/**
 * rightTurn
 * 右に曲がります．
 *
 */
void Handle_rightTurn(void)
{
    LeftTire_front(50);
    RightTire_stop();
    systick_wait_ms(WAIT_TIME);
}

/**
 * leftTurn
 * 左に曲がります．
 *
 */
void Handle_leftTurn(void)
{
    RightTire_front(50);
    LeftTire_stop();
    systick_wait_ms(WAIT_TIME);
}

/**
 * TouchSensor_isTouched 
 * タッチセンサがタッチされたかを返します．
 *
 */
int TouchSensor_isTouched(void)
{
    int touch = FALSE;

    if (TOUCHED == ecrobot_get_touch_sensor(BUMPER_SENSOR_PORT)) {
        pushedPls = 1;
    }
    
    /** 信号の立下りを見てボタンが押されたかを判断する */
    if (1 == pushedPls && TOUCHED != ecrobot_get_touch_sensor(BUMPER_SENSOR_PORT)) {
        touch = TRUE;
        pushedPls = 0;
    }    

    return touch;
}

/**
 * LineSensor_initWork
 * センサの準備をします． 
 *
 */
void LineSensor_initWork(void)
{

}

/**
 * LineSensor_inOnline
 * 線上かどうかを返します．
 *
 * @return 線上の場合 ON_LINE, それ以外 OUT_LINE
 *
 */
int LineSensor_isOnline(void)
{
    int value = ecrobot_get_light_sensor(LIGHT_SENSOR_PORT);
    return value > IS_BLACK;
}

/**
 * Body_changeStatus
 * 運転状態を変更します．
 *
 */
int Body_changeStatus(int status)
{
    /** 現在は停止，運転のみのステータスなので単に反転させるだけ */
    return (TRUE == TouchSensor_isTouched())? !status: status; 
}

/**
 * MainTask
 * Mainタスクです．
 *
 */
TASK(MainTask)
{
    /** ステータス */
    int status = 0;

    /** 初期処理を行います */
    LineSensor_initWork();

    while(1) {
        status = Body_changeStatus(status); 
        switch(status) {
            case LINE_TRACE:
                if (LineSensor_isOnline()) {
                    /** 線上の場合は右に曲がる */
                    Handle_rightTurn();
                } else {
                    /** 線外の場合は左に曲がる */
                    Handle_leftTurn();
                }
                break;
            case STOP:
                /** 両輪を停止する */
                LeftTire_stop();
                RightTire_stop();
                break;
        }
    } 

    /** タスクを終了する */
    TerminateTask(); 

}