組み込みアプリケーションの構成例

今回紹介するのは「組み込み系のプログラムを作るときにそのプログラム構成をどんな風に分けたらいいの？」ってあたりの話。基本的には、CQ出版の 「リアルタイムシステム実現のための自律オブジェクト指向」(岩橋正実) って本に詳しいあたりの話です。

作るアプリケーションの定義

じゃぁ、何(制御対象)のプログラムを作るかについて定義します。今回は、

くらいの定義にしておきます。
ただし、このままだとあまりに抽象的すぎるので、制御タスクを 「通信で与えられた目標速度にモータの回転速度を追従させる」 ということにしましょう。

以下が、今回のプログラム内容についてのイメージ図です。

em01_abst.png

レイヤー定義

では全体構成を確認するために、プログラムで行うことを文章に書き出してみる。

で、
次は、制御内容をレイヤー毎に分けてみる。レイヤーに分けることで、情報の流れを明確にすることができる。 またしてもイメージ図。なんか、３層だとレイヤーっぽくないな...。

em01_layers.png

つか、ここまで書き出したらプログラム構成なんて、終わったようなものです。


プログラム例

と言うわけで、ここまでの内容を C で実現すると以下のような感じ。
プロジェクト名は、embedded01 と命名する。

以下のコードを Makefile 付きでパッケージ化したものはこちら -> embedded01.tar.gz

メインプログラム 

/*
  ・「・ラ・ア。シ・キ・逾ホシツチ  Satofumi KAMIMURA
  $Id$

  トフソョ・ヌ・ミ・、・ケ、ホウ荀゜ヘ・タ霰ル、ャーヨケ筅、、ネ、ケ、  ショ、キ、ソ・ヌ。シ・ソ、ホ・ム・ア・テ・ネ、ネ、キ、ニ、ホノセイチ、マフオクツ・シ・ラテ讀ヌケヤ、ヲ
*/

#include "interrupt.h"
#include "connectCtrl.h"
#include "motorCtrl.h"


static void execCtrlLoop(void) {
  // !!! タゥク讌筍シ・ノ、ャハ」ソ「、遉ヒ、マ。「、ウ、ウ、ヌセ醋ャ、アナホス靉ヤ、ヲ
  // !!! コ」イマ。「タゥク讌ソ・ケ・ッ、ホシ爨マ・筍シ・ソ、ホツョナルタゥク讀タ、ア、ハ、ホ、ヌ。「・キ・ラ・
  // クスコ゜、ホフワノクイセツョナル、筍シ・ソ、ヒサリシィ
  setMotorVel(getRecvedRefVel());
}


int main(void) {

  // ウ荀゜タ゜ト遙「オレ、モ・ヌ・ミ・、・ケ、ホス魘ス
  initInterrupt();
  initConnectionDevice();
  initMotorDevice();

  // タゥク讌シ・ラ、ホオッニー
  setInterruptHandler(execCtrlLoop, 1);
  start_interrupt();

  // トフソョ・ム・ア・テ・ネ、ホエニサ  while (1) {
    evaluatePacketData();
  }
}


割り込み管理 

/*
  サヨヒ隍ホウ荀゜エノヘ  Satofumi KAMIMURA
  $Id$
*/

#include "interrupt.h"
#ifndef NULL
#define NULL ((void*)0x0)
#endif

static task_callback_t func_ptr = NULL;


// ウ荀゜、ヌサリト熙キ、ソシ隍ヒクニ、ミ、リソタ、ネイセト熙キ、ニシツチケ、void interrupt_callback(void) {
  if (!func_ptr) {
    return;
  }
  // !!! ノャヘラ、ヒアク、ニサリトツクス、ケ、ソ、皃ホス靉  
  func_ptr();
}


// ウ荀゜タ゜ト熙ホス魘ス
void initInterrupt(void) {

  // !!! シツチヘツク
}


// サリトmsec ヒ隍ヒクニ、モスミ、オ、荀゜・マ・ノ・鬢ホナミマソ
void setInterruptHandler(task_callback_t callback, int usec) {

  func_ptr = callback;
  
  // !!! usec 、ホネソアヌハ。、マシツチヘツク
}


// ウ荀゜ス靉ホウォサマ
void start_interrupt(void) {

  // !!! シツチヘツク
}


// ウ荀゜ス靉ホト莉゜
void stop_interrupt(void) {

  // !!! シツチヘツク
}


通信まわり 

/*
  トフソョタゥク  Satofumi KAMIMURA
  $Id$
*/

#include "connectCtrl.h"

enum { RECV_BUFFER_SIZE = 32 };
static char RecvBuffer[RECV_BUFFER_SIZE];
static int Filled = 0;
static int RefVel = 0;          // フワノクイセツョナル


// トフソョ・ヌ・ミ・、・ケ、ャ・ヌ。シ・ソ、ョ、キ、ソ、ネ、ュ、ヒクニ、モスミ、オ、リソネ、ケ、void recvData(char ch) {
  // !!! ヒワナマ。「・ミ・テ・ユ・。・オ・、・コ、ホタゥクツ、ネ、ォ。「・ー・ミ・テ・ユ・。、ヌ、ホシツチネ、ォソァ。ケ...
  RecvBuffer[Filled++] = ch;
}


// トフソョ・ヌ・ミ・、・ケ、ホス魘ス
void initConnectionDevice(void) {

  // !!! シツチヘツク
}


// ショ・ヌ。シ・ソ、ム・ア・テ・ネ、ネ、キ、ニノセイチ
void evaluatePacketData(void) {
  
  // ショ・ヌ。シ・ソ、ホニ簣ニ、ォ、鯲ワノクウムツョナル、霹タ、ケ、  if (Filled > 0) {
    RefVel = RecvBuffer[0]; // !!! ヒワナマ。「・ム・ア・テ・ネ、マ、キ、ソキフ、ハ、ノ
    Filled = 0;
  }
}


// ショ、キ、ソフワノクイセツョナル、ホテヘ、ヨ、ケ
int getRecvedRefVel(void) {
  return RefVel;
}


モータ制御 

#include "motorCtrl.h"
#include "encCtrl.h"
#ifdef SIMULATOR
enum { SMP_USEC = 1000 };
void set_mode(const unsigned char id, const int mode) {}
void set_pwm(const unsigned char id, const int duty) {}
#else
#include <7040S.H>

#include "tRunCtrl.h"
#ifndef SMP_USEC
#define SMP_USEC 1000
#endif


void set_pwm(const unsigned char id, const int duty) {

  if (id == 0) {
    if (duty <= 0 || duty >= 255) {
      PFC.PECR1.WORD &= ~0x0001; /* PE8 */
      PE.DR.WORD &= ~0x0100;
      PE.DR.WORD |= (duty <= 0) ? 0 : 0x0100;
    } else {
      PFC.PECR1.WORD |= 0x0001; /* TIOC3A */
      MTU3.TGRB = duty;
    }
  } else if (id == 1) {
    if (duty <= 0 || duty >= 255) {
      PFC.PECR1.WORD &= ~0x0010; /* PE10 */
      PE.DR.WORD &= ~0x0400;
      PE.DR.WORD |= (duty <= 0) ? 0 : 0x0400;
    } else {
      PFC.PECR1.WORD |= 0x0010; /* TIOC3C */
      MTU3.TGRD = duty;
    }
  }
}


void set_mode(const unsigned char id, const int mode) {
  if (id == 0) {
    /* DIR(PE5), EN(PE7) */
    if (mode == MTR_MODE_CCW_BREAK) {
      PE.DR.WORD |= 0x0020;     /* DIR(PE5)=1 */
      PE.DR.WORD &= ~0x0080;    /* EN(PE7)=0 */
    } else if (mode == MTR_MODE_CW_BREAK) {
      PE.DR.WORD &= ~0x0020;    /* DIR(PE5)=0 */
      PE.DR.WORD &= ~0x0080;    /* EN(PE7)=0 */
    } else {                    /* MTR_MODE_FREE */
      PE.DR.WORD |= 0x0080;     /* EN(PE7)=1 */
    }

  } else if (id == 1) {
    /* DIR(PE9), EN(PE11) */
    if (mode == MTR_MODE_CCW_BREAK) {
      PE.DR.WORD |= 0x0200;     /* DIR(PE9)=1 */
      PE.DR.WORD &= ~0x0800;    /* EN(PE11)=1 */
    } else if (mode == MTR_MODE_CW_BREAK) {
      PE.DR.WORD &= ~0x0200;    /* DIR(PE9)=0 */
      PE.DR.WORD &= ~0x0800;    /* EN(PE11)=1 */
    } else {                    /* MTR_MODE_FREE */
      PE.DR.WORD |= 0x0800;     /* EN(PE11)=1 */
    }
  }
}


// 直接制御用
void setDirectMotorMode(unsigned char id, int mode) {
  set_mode(id, mode);
}

void setDirectMotorPwm(unsigned char id, int duty) {
  set_pwm(id, duty);
}


void initMotor(void) {
  int i;

  PFC.PEIOR.WORD |= 0x0500;     /* use TIOC3A, TIOC3C port */
  MTU3.TCR.BYTE = 0x20 | 0x01;  /* cycle=TGRA, duty=TGRB, cycle/4 */
  MTU3.TMDR.BYTE = 0xc2;        /* pwm mode 1, buffer disable */
  MTU3.TIOR.BYTE.H = 0x12;      /* CMA=0, CMB=1 */
  MTU3.TIOR.BYTE.L = 0x12;      /* CMC=0, CMD=1 */
  MTU3.TGRA = MTU3.TGRC = 255;  /* cycle */

  for (i = 0; i <= 1; ++i) {
    set_pwm(i, 0);
    set_mode(i, MTR_MODE_FREE);
  }

  PFC.PECR1.WORD |= 0x0011;     /* use TGRA, TGRC port */

  MTU.TSTR.BYTE |= 0xc0;        /* pwm active */

  PFC.PECR1.WORD &= ~0x0044;    /* TIOC3B, TIOC3D */
  PFC.PECR2.WORD &= ~0x4400;    /* TIOC1B, TIOC2B */
  PFC.PEIOR.WORD |= 0x0aa0;
}
#endif


void initMotorInfo(const unsigned char id, motorInfo_t *mtr) {
  mtr->id = id;
  mtr->gain_p = MTR_GAIN_P;
  mtr->gain_i = MTR_GAIN_I;
  mtr->i_value = 0;

  mtr->r_per_v = (int)((1<<MTR_I_BIT_WIDTH) * MTR_OHM / MTR_POWER);
  mtr->ke_per_v = (int)(1.0 / MTR_RPM_PER_V
                        * 60 * 1000 / (SMP_USEC / 1000)
                        * (1 << MTR_I_BIT_WIDTH) / (MTR_POWER * ENC_PULSE));
}


static int calcGainOutput(const int ref_cnt, const int cnt_diff,
                          motorInfo_t *mtr) {
  int diff;
  int output;

  diff = ref_cnt - cnt_diff;
  mtr->i_value += diff;
  if (mtr->i_value > I_VALUE_MAX) {
    mtr->i_value = I_VALUE_MAX;
  } else if (mtr->i_value < -I_VALUE_MAX) {
    mtr->i_value = -I_VALUE_MAX;
  }
  output = (mtr->gain_p * diff) + (mtr->gain_i * mtr->i_value);
  output <<= (MTR_I_BIT_WIDTH - 10);

  return output;
}


// モータに対して、MTR_I_BIT_SHIF シフトされた電流[A] を流すように設定する
static int calcInvertedCtrlPwm(const int output, const int cnt_diff,
                               motorInfo_t *mtr) {

  int sign;
  int pwm;

  pwm = (mtr->r_per_v * output) + (mtr->ke_per_v * cnt_diff);
  sign = (pwm < 0) ? -1 : +1;
  pwm = (sign * pwm) >> ((MTR_I_BIT_WIDTH << 1) - 8);
  if (pwm > 0xff) {
    pwm = 0xff;
  }
  return (sign * pwm);
}


void setMotorFree(motorInfo_t *mtr) {
  set_mode(mtr->id, MTR_MODE_FREE);
}


int setMotorRevolution(const int ref_cnt, const int cnt_diff,
                       motorInfo_t *mtr) {
  int ref_pwm;
  int mode;
  int pwm;
  int output;

  output = calcGainOutput(ref_cnt, cnt_diff, mtr);
  ref_pwm = calcInvertedCtrlPwm(output, cnt_diff, mtr);
  mode = (ref_pwm >= 0) ? MTR_MODE_CW_BREAK : MTR_MODE_CCW_BREAK;
  pwm = (ref_pwm >= 0) ? ref_pwm : -ref_pwm;

  set_pwm(mtr->id, pwm);
  set_mode(mtr->id, mode);

  return ref_pwm;
}

設計の指針としては、

って感じです。



何か質問等がありましたら、お気軽にどぞ〜♪

Generated on Mon Apr 13 22:52:06 2009 by  doxygen 1.5.7.1