LoginSignup
2
3

More than 5 years have passed since last update.

@TetsuroAkagawain株式会社アフレル

エリア分割走行の応用（CSVファイルの活用） with EV3

Last updated at Posted at 2017-07-11

概要

　今回の内容は、以前書いたエリアについての記事内容※の続きになります。まだ読んでいらっしゃらない場合は、先にそちらをご覧ください。
※ETロボコンにおける自己位置の応用（エリア分割） with EV3
URL:http://qiita.com/TetsuroAkagawa/items/1798c12e9517e6f1cbe9

　前回のエリア分割走行技術の紹介では、エリア情報を配列で管理していました。この方法でエリア情報を管理してしまうと、あまりよろしくないというお話もしたことかと思います。
　具体的には、以下の点で不便です。

・エリア情報の内容を変えるだけで、プログラムのコンパイルからやり直さなければならないので、調整の際の作業に時間がかかる
・エリア情報を管理しているファイルの記述内容において、そのエリア情報の記述のために大きくスペースを使うことになってしまい、本来のプログラム自体の記述内容が読みにくくなる

　二つ目の項目はプログラムの記述方法の工夫次第でなんとかなるのですが、一つ目の項目はとても重要です。
　特にRTロボコン大会当日に行われる試走会は、制限時間が設定された状態でPID係数なり、エリアの長さの微調整などの作業を行うので、時間との勝負になります。そこで、いちいち変更の度にコンパイルをしていたら、どうあっても時間が足りません。

　よって、こういった問題を解決するのが、「csvファイル」での管理です。

利用方法

利用方法を図に示します。csvファイルというプログラムとは別にファイルを用意して、そのファイルの記述内容をプログラムの方から読み込むことで利用します。
手順は以下の通りになります。

  1. mallocを使って、格納先の構造体の領域を確保する
  2. ファイルポインタを宣言する
    • ファイルポインタとは、ファイル構造体のポインタ変数です（ファイル構造体はstdio.hにて定義されています）。直接この構造体のメンバーを指定して参照することはないですが、後述のfopen()関数内では、各メンバーを参照しながら処理が行われます。
  3. fopen()関数を使ってファイルをオープンする
    • 戻り値にファイルポインタが返されるので、それを(1)のファイルポインタに代入します。
  4. fscanf()関数でファイル情報を一行ずつ読み込む
  5. (4)の処理をEOFが返されるまで、繰り返して実行する
  6. fcloseを使って、オープンしたファイルをクローズする

簡単にまとめて説明すると以上になります。この箇所についての詳しい説明は、様々なサイトで分かりやすく説明されていますので、まだよく分からない場合は、そちらを参考にしてください。

サンプルプログラム

以下がサンプルプログラムになります。手順としては、上記の説明通りですが、所々とりあげて説明していきます。
　まず、最初にmallocに使って領域を確保しているのですが、「エリアも80個もあれば充分だろ」と安易な考えで余分に領域を取っています。
　whileループを使って、CSVからエリア情報を順に構造体に格納していきます。このときに用いる構造体についての定義は area.h 内で定義されています。あとは、前回のエリアについての紹介記事と同じで、その構造体から情報を取り出すことで、実際の走行に活用していきます。

Area.c
#include "Area.h" // 自身のヘッダファイル

/*** ファイル名はここを変えてください ***/
#define L_FILE "sample_data_l.csv"
#define R_FILE "sample_data_r.csv"

//コース情報
course_data courseData;
//現在のエリアID（エリア情報取り出しの参照先）
static int currentArea = 0;

// 非公開操作　
/*********************************************
一時的なエリア構造体を生成する
**********************************************/
_areaStruct createAreaStrut(float dist, float sp, float kp, float ki, float kd, int target) {
    _areaStruct as;
    as.distance = dist;
    as.speed = sp;
    as.p = kp;
    as.i = ki;
    as.d = kd;
    as.target = target;
    return as;
}

//公開操作
/*********************************************
CSVデータからエリア情報を取り出し、格納する
エリア情報を取り出し完了    true;
エリア情報を取り出し失敗    false;
**********************************************/
int Area_readCSV(bool_t LR) {
    courseData.R_course.count = 0;
    courseData.L_course.count = 0;

    FILE *fp;
    char *fname = R_FILE;
    int ret;
    float dist, sp, kp, ki, kd;
    int target = 0;
    int count = 0;

    switch(LR) {
    case L_COURSE:
        // メモリ確保（エリアの数が80個以上にならない想定で、80個分確保）
        courseData.L_course.area = (_areaStruct *)malloc(80 * sizeof(_areaStruct));
        // 読み込むファイル名を指定(上部で定義済み)
        fname = L_FILE;
        // もしもファイルが開けなかったらfalseを返す
        if ((fp = fopen(fname, "r")) == NULL) {
            return false;
        }
        // CSVから読み込んだ情報をエリア構造体に順に格納していく
        while((ret = fscanf(fp, "%f, %f, %f, %f, %f, %d", &dist, &sp, &kp, &ki, &kd, &target)) != EOF) {
            courseData.L_course.area[count] = createAreaStrut(dist, sp, kp, ki, kd, target);
            count++;
        }
        //コース上のエリア個数を格納する
        courseData.L_course.count = count;
        break;
    case R_COURSE:
        /* 以下のRコースの処理は、上記Lコースの処理と同様 */
        courseData.R_course.area = (_areaStruct *)malloc(50 * sizeof(_areaStruct));
        fname = R_FILE;
        if ((fp = fopen(fname, "r")) == NULL) {
            return false;
        }
        while((ret = fscanf(fp, "%f, %f, %f, %f, %f, %d", &dist, &sp, &kp, &ki, &kd, &target)) != EOF) {
            courseData.R_course.area[count] = createAreaStrut(dist, sp, kp, ki, kd, target);
            count++;
        }
        courseData.R_course.count = count;
        break;
    }

    fclose(fp);

    return true;
}

/*********************************************
エリア情報を更新するか判定する(更新したいコースデータ L or R)
エリア情報を更新する  true;
エリア情報を更新しない   false;
**********************************************/
bool_t Area_update(bool_t LR) {
    float Area_curDistance = 0;

    Area_curDistance = Distance_getDistance();

    switch(LR) {
    case L_COURSE:
        //エリアを走破したら次のエリアへ移動(最後のエリアだった場合はそのまま)
        if((Area_curDistance >= courseData.L_course.area[currentArea].distance)
            && (currentArea < (courseData.L_course.count-1)) ){
            Distance_init();
            ev3_speaker_play_tone(NOTE_DS6, 100);   /* 音が鳴る */
            currentArea++;
            return true;
        }
        break;
    case R_COURSE:
        //エリアを走破したら次のエリアへ移動(最後のエリアだった場合はそのまま)
        if((Area_curDistance >= courseData.R_course.area[currentArea].distance)
            && (currentArea < (courseData.R_course.count-1)) ){
            Distance_init();
            ev3_speaker_play_tone(NOTE_DS6, 100);   /* 音が鳴る */
            currentArea++;
            return true;
        }
        break;
    }

    return false;
}

/****************************************************
エリア情報を取得する(取得したいコースデータ L or R)
****************************************************/
_areaStruct Area_getAreaData(bool_t LR) {
    Area_update(LR);
    switch(LR) {
    case L_COURSE:
        return courseData.L_course.area[currentArea];
        break;
    case R_COURSE:
        return courseData.R_course.area[currentArea];
        break;
    }
}
//エリアを次に進める
void Area_change() {
    currentArea++; //次のエリアへ進める
}
//現在のエリアIDを設定する
void Area_setCurrentAreaID(int area_id) {
    currentArea = area_id;
}
//現在のエリアIDを取得する
int Area_getCurrentAreaID() {
    return currentArea;
}
Area.h
#ifndef _AREA_H_
#define _AREA_H_

#include <stdio.h>
#include <stdlib.h>
#include "ev3api.h"
#include "Distance.h"

//各コースの識別用定義
#define L_COURSE 0
#define R_COURSE 1

/* エリア情報構造体 */
typedef struct areaStruct {
    float distance;// 距離
    float speed;   // 走行速度
    float p;       // P係数
    float i;       // I係数
    float d;       // D係数
    int target;    // 輝度PIDの目標値
} _areaStruct;

/* 各コース情報構造体 */
typedef struct areaData {
    _areaStruct *area; //コースのエリア情報
    int count;         //コースのエリア数
} _areaData;

/* ゲーム全体情報構造体 */
typedef struct courseData {
    _areaData R_course;
    _areaData L_course;
} course_data;

/**************************************************
CSVデータからエリア情報を取り出し、格納する
エリア情報を取り出し完了    true;
エリア情報を取り出し失敗    false;
***************************************************/
int Area_readCSV(bool_t LR);
/************************************************************
エリア情報を更新するか判定する(更新したいコースデータ L or R)
エリア情報を更新する  true;
エリア情報を更新しない   false;
*************************************************************/
bool_t Area_update(bool_t LR);
/**************************************************
エリア情報を取得する(取得したいコースデータ L or R)
***************************************************/
_areaStruct Area_getAreaData(bool_t LR);

//エリアを次に進める
void Area_change();
//エリアIDを設定する
void Area_setCurrentAreaID(int area_id);
//エリアIDを取得する
int Area_getCurrentAreaID();

#endif

　app.c 内では、デモとしてエリア情報が順にLCDに表示させるようにしています。タイヤを回転させて、その走行距離に応じて、エリアを遷移していく様子が、LCDの出力から見ることができます。

app.c
#include "ev3api.h"
#include "app.h"
#include "Distance.h"
#include "Area.h"

#include <stdio.h>
#include <stdlib.h>

#if defined(BUILD_MODULE)
#include "module_cfg.h"
#else
#include "kernel_cfg.h"
#endif

#define DEBUG

#ifdef DEBUG
#define _debug(x) (x)
#else
#define _debug(x)
#endif

/* LCD周辺定義 */
#define CALIB_FONT (EV3_FONT_SMALL)
#define STR 6
#define CALIB_FONT_WIDTH (6/*TODO: magic number*/)
#define CALIB_FONT_HEIGHT (8/*TODO: magic number*/)

_areaStruct sample_data;

void main_task(intptr_t unused) {
    /* モーター出力ポートの設定 */
    ev3_motor_config(EV3_PORT_C, LARGE_MOTOR);
    ev3_motor_config(EV3_PORT_B, LARGE_MOTOR);

    /* csvファイルからエリア情報を取り出し、格納する */
    Area_readCSV(L_COURSE);
    /* 格納された現在のエリア情報を取り出し、LCD出力用構造体に格納 */
    sample_data =  Area_getAreaData(L_COURSE);
    /* LCDに現在のエリア情報を出力させる */
     main_printLCD();

    while(1) {
        /* 計測器更新 */
        Distance_update();

        /* エリア切り替えが発生したら、LCDに出力 */
        if(Area_update(L_COURSE) == true) {
            /* LCD出力用構造体に格納 */
            sample_data =  Area_getAreaData(L_COURSE);
            /* LCDに出力させる */
            main_printLCD();
        }

        //エリア切り替えをさせるためタイヤを回す
        ev3_motor_set_power(EV3_PORT_C, 30);
        ev3_motor_set_power(EV3_PORT_B, 30);

        /* タスクを4[ms]動作周期にするために、4[ms]のタスクスリープを入れる */
        tslp_tsk(4);
    }
}


/******************
LCD出力用関数
*******************/
void main_printLCD(void) {
    static char str[STR][30];   // LCD出力用文字列変数

    ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
    sprintf(str[0], "distance = %d", (int)(sample_data.distance));
    sprintf(str[1], "speed = %d", (int)(sample_data.speed));
    sprintf(str[2], "p = %d", (int)(sample_data.p * 1000));
    sprintf(str[3], "i = %d", (int)(sample_data.i * 1000));
    sprintf(str[4], "d = %d", (int)(sample_data.d * 1000));
    sprintf(str[5], "target = %d", (int)(sample_data.target));
    ev3_lcd_draw_string(str[0], 0, CALIB_FONT_HEIGHT*1);
    ev3_lcd_draw_string(str[1], 0, CALIB_FONT_HEIGHT*2);
    ev3_lcd_draw_string(str[2], 0, CALIB_FONT_HEIGHT*3);
    ev3_lcd_draw_string(str[3], 0, CALIB_FONT_HEIGHT*4);
    ev3_lcd_draw_string(str[4], 0, CALIB_FONT_HEIGHT*5);
    ev3_lcd_draw_string(str[5], 0, CALIB_FONT_HEIGHT*6);
}
2
3
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
2
3