目次 前 次

---

ファームウエア検討

　2015年まで、ファームウエアは外国人研修担当時に作成した
　H8/3048用のものを加筆、修正したコードを使っていました。

　マルチプロセッサ構成で、新シャーシを使ったメカには
　相応しくない内容なので、一新します。

　研修担当時に作成したファームウエアでは、密着タイプセンサー
　を使っていました。




　GBC(Game Boy Camera)に合わせた処理に変更し、最上位のデバイスは
　GBCの出力情報を使って移動方法を決定後、DCモータにPWM波形で使う
　DUTY比を出力すると機能限定します。

　入力、出力に必要なビット数を見積もります。

　入力

　　GBCを担当するプロセッサはArduino互換で、４ビットで構成した
　　情報を利用。

#define ALL_BLACK   0
#define ALL_WHITE   1
#define LEFT_WHITE  2
#define RIGHT_WHITE 3
#define CENTER      4
#define TINY_RIGHT  5
#define RIGHT       6
#define BIG_RIGHT   7
#define TINY_LEFT   8
#define LEFT        9
#define BIG_LEFT    10
#define BOTH_WHITE  11
#define ILLEAGAL    12

　　この４ビットの他に、スタートゲートの開閉状態とスタートスイッチ
　　の状態を受取る３ビットを用意すれば、入力はおしまい。

　　入力は合計７ビットですが、１ビットの予備を考え８ビット。

　出力

　　DCモータを動かすために、２ビット必要。

　　センサーと内部状態を表示するには、各８ビットあれば充分。

　　合算すると、２＋８＋８＝１８になります。

　　センサーと内部状態を表示する、各８ビットは74HC164のような
　　シフトレジスタを間に入れると、転送クロック、データ２ビット
　　の合計３ビットで処理可能。

　　必要なビット数は、２＋３＝５になります。

　　回路は、次のように単純にできます。



　　マイコンからは、３ピンで２ブロックのLEDを制御。

DCLK
sensor_data
state_data

　　FPGAでは、内部に専用レジスタがあればよいだけなので
　　８ビット出力を２つ用意します。

　ここまでの検討から、Arduino＋PICのマルチプロセッサ構成
　基板ではセンサーと内部状態表示をシリアル通信で対応すると
　新しい基板の半田付けは不要と判断。

　Arduinoをピン数の多いSanguinoに交換すれば、他にGameBoyCameraを
　担当するユニットとの接続だけにできます。

　利用メカの特性に最適なファームウエアを用意したいので
　タイヤの回転と線方向の移動距離を求めておきます。

　タイヤ関連諸元からの計算

　　DCモータシャフトに取付けたタイヤ直径は、57mm。



　　タイヤ円周は、2πr（ｒは半径）より、57πmm。
　　πを３と近似すると、１回転で171mm移動可能に。

　　１回転を正の整数で除算すると、次の移動距離に。

1/9 => 19 mm
1/8 => 21 mm
1/7 => 24 mm
1/6 => 28 mm
1/5 => 34 mm
1/4 => 42 mm
1/3 => 57 mm
1/2 => 85 mm

　　左右のモータの回転数に差がある場合、内周と外周の
　　中間にある円弧で、移動する距離を換算してマシンの
　　挙動を考えます。




　　左右の回転数の差を求め、移動距離の差に換算し
　　arc tangentを取れば、回転角度を計算できます。

　　瞬時値から、その時点のベクトルを判断できます。




　　角度を計算するには、縦、横の値が必要。縦はタイヤの
　　回転数の差で求められ、横はモーターのシャフト長で
　　固定値になります。

　　シャフト長は、固定で154mm。




　　左右の回転数の差が0.5であれば、回転角度は45度程度。

　　マシンが走行している状態で、45度という回転角度は
　　スピンするくらいの大きな力をかけることになります。

　　スピンしないでラインを追従するには、回転数の差が
　　0.01から0.2程度にするとわかります。
　　車速感応制御で、滑らかなに走行させることも考えて
　　おかないと。

　　回転数の差を求めるには、ロータリーエンコーダを使うことが
　　知られていますが、モータに流れる電流値で回転数を算出する
　　ことも可能。メカに制約がある場合、電流値による回転数算出
　　を利用することに。

　ここまでで大まかな処理を決めたので、APIを利用してトップレベル
　のコードを作成します。

int main(void)
{
  /* initialize */
  init_usr();
  /* detect start gate state */
  sgate = get_start();
  while ( sgate == CLOSED ) {
    /* delay 100ms */
    delay_ms(100);
    /* get start gate state */
    sgate = get_start();
  }
  /* endless loop */
  while ( ON ) {
    /* get sensor state */
    sensor = get_sensor();
    /* judge mode */
    switch ( mode ) {
      case MD_NORMAL : mode = execute_normal(sensor); break ;
      case MD_CRANK  : mode = execute_crank(sensor) ; break ;
      case MD_ROTATE : mode = execute_rotate(sensor); break ;
      case MD_LANE   : mode = execute_lane(sensor);   break ;
      case MD_CHANGE : mode = execute_change(sensor); break ;
      case MD_BLIND  : mode = execute_blind(sensor);  break ;
      default        : break ;
    }
    /* delay 100ms */
    delay_ms(100);
  }
  /* dummy */
  return 0 ;
}

　トップレベルの処理から展開していくと以下。

#include <ADuC7026.h>

#define OFF 0
#define ON  OFF+1

#define NO  0
#define YES NO+1

/* data definitions */
typedef unsigned char  UBYTE ;
typedef   signed char  SBYTE ;
typedef unsigned short UWORD ;
typedef   signed short SWORD ;
typedef unsigned long  ULONG ;
typedef   signed long  SLONG ;

void  IRQ_Handler(void) __irq;
void  init_usr(void);

/*--------------------*/
/* function prototype */
/*--------------------*/
void  delay_100us(UWORD x);
void  delay_ms(UWORD x);

/*---------------*/
/* FPGA handling */
/*---------------*/
UBYTE execute_normal(UBYTE x);
UBYTE execute_crank(UBYTE x);
UBYTE execute_rotate(UBYTE x);
UBYTE execute_lane(UBYTE x);
UBYTE execute_change(UBYTE x);
UBYTE execute_blind(UBYTE x);

UBYTE get_start(void);
UBYTE get_sensor(void);
void  send_duty(UBYTE which,UBYTE x);
void  update_motor(void);

/* global variables */
volatile ULONG timcnt ;

volatile UBYTE lduty ;
volatile UBYTE rduty ;

volatile UBYTE dir[2] ;

#define MD_NONE   0
#define MD_NORMAL MD_NONE+1
#define MD_CRANK  MD_NONE+2
#define MD_ROTATE MD_NONE+3
#define MD_LANE   MD_NONE+4
#define MD_CHANGE MD_NONE+5
#define MD_BLIND  MD_NONE+6

#define ALL_BLACK   0
#define ALL_WHITE   1
#define LEFT_WHITE  2
#define RIGHT_WHITE 3
#define CENTER      4
#define TINY_RIGHT  5
#define RIGHT       6
#define BIG_RIGHT   7
#define TINY_LEFT   8
#define LEFT        9
#define BIG_LEFT    10
#define BOTH_WHITE  11
#define ILLEAGAL    12

#define TRG 18
#define BLD 17
#define ALD 16

#define MASK0F 0x0f

#define DIR_CENTER 0
#define DIR_RIGHT  DIR_CENTER+1
#define DIR_LEFT   DIR_CENTER+2

#define CLOSED 0
#define OPENED CLOSED+1

volatile UBYTE sgate ;
volatile UBYTE sensor ;
volatile UBYTE mode  ;

void main(void)
{
  /* initialize */
  init_usr();
  /* detect start gate state */
  while ( sgate == CLOSED ) {
    /* delay 100ms */
    delay_ms(100);
    /* get start gate state */
    sgate = get_start();
  }
  /* endless loop */
  while ( ON ) {
    /* get sensor state */
    sensor = get_sensor();
    /* judge mode */
    switch ( mode ) {
      case MD_NORMAL : mode = execute_normal(sensor); break ;
      case MD_CRANK  : mode = execute_crank(sensor) ; break ;
      case MD_ROTATE : mode = execute_rotate(sensor); break ;
      case MD_LANE   : mode = execute_lane(sensor);   break ;
      case MD_CHANGE : mode = execute_change(sensor); break ;
      case MD_BLIND  : mode = execute_blind(sensor);  break ;
      default        : break ;
    }
    /* delay 100ms */
    delay_ms(100);
  }
}

void IRQ_Handler(void) __irq
{
  /* judge timer0 interruption (100us) */
  if ( (IRQSTA & RTOS_TIMER_BIT) == RTOS_TIMER_BIT ) {
    /* clear timer0 interrupt flag */
    T0CLRI = 0xff ;
    /* increment */
    timcnt++ ;
    /* judge */
    if ( (timcnt & 0xfff) == 0 ) { GP4DAT ^= (1 << 23); }
  }
}

void init_usr(void)
{
  /* select clock 10.44MHz
     initialized in start up routine
  */
  PLLKEY1 = 0xaa ;
  PLLCON  = 0x01 ;
  PLLKEY2 = 0x55 ;
  /* power control
     initialized in start up routine 
  */
  /* initialize UART */
  {
    /* set baud rate 19200 bps CD = 2 */
    COMCON0 = 0x80 ; /* select COMDIV1 and COMDIV0 */
    COMDIV0 = 0x11 ;
    COMDIV1 = 0x00 ;
    /* set conditions */
    COMCON0 = 0x03 ; /* select COMRX and COMTX , 8bit data , 1 stop bit , no parity */
    /* enable interrupt */
    COMIEN0 = 0x01 ; /* ERBFI */
  }
  /* P0 */
  {
    /* use GPIO */
    GP0CON = 0x00000000 ;
    /* */
    GP0DAT = 0xDF1F0000 ;
  }
  /* P1 */
  {
    /* use UART */
    GP1CON = 0x00000011 ;
    /* */
    GP1DAT = 0xfef00000 ;
  }
  /* P2 */
  {
    /* all bits inputs */
    GP2DAT = 0x00000000 ;
  }
  /* P3 */
  {
    /* all bits outputs */
    GP3DAT = 0xff000000 ;
  }
  /* P4 */
  {
    /* all bits outputs */
    GP4DAT = 0xff030000 ;
  }
  /* initialize timer 0 (10kHz) */
  {
    T0LD  = 1044 ; /* (10.44MHz / 1) / 10kHz */
    T0CON = 0xc0  ; /* enable , cyclic , 1/1  */ 
  }
  timcnt = 0 ;
  lduty  = 0 ;
  rduty  = 0 ;
  update_motor();
  mode = MD_NONE ;
  sgate = CLOSED ;
  /* enable timer 0 interrupt */
  IRQEN = RTOS_TIMER_BIT ;
}

void delay_100us(UWORD x)
{
  ULONG last ;
  /* calculate */
  last = timcnt + x ;
  /* wait */
  while ( timcnt < last ) ;
}

void delay_ms(UWORD x)
{
  ULONG last ;
  /* calculate */
  last = timcnt + 10 * x ;
  /* wait */
  while ( timcnt < last ) ;
}

UBYTE execute_normal(UBYTE x)
{
  UBYTE state ;
  UBYTE dtime ;

  /* default */
  lduty = 30 ;
  rduty = 30 ;
  dtime = 30 ;
  state = MD_NORMAL ;

  if ( x == ALL_BLACK )
  {
    lduty = 10 ; rduty = 10 ; dtime = 10 ;
  }
  if ( x == ALL_WHITE )
  {
    lduty = 30 ; rduty = 30 ; dtime = 30 ;
  }
  if ( x == LEFT_WHITE || x == RIGHT_WHITE || x == CENTER )
  {
    lduty = 50 ; rduty = 50 ; dtime = 30 ;
  }
  if ( x == TINY_RIGHT )
  {
    lduty = 50 ; rduty = 55 ; dtime = 25 ;
  }
  if ( x == RIGHT )
  {
    lduty = 50 ; rduty = 60 ; dtime = 25 ;
  }
  if ( x == BIG_RIGHT )
  {
    lduty = 50 ; rduty = 70 ; dtime = 25 ;
  }
  if ( x == TINY_LEFT )
  {
    lduty = 55 ; rduty = 50 ; dtime = 25 ;
  }
  if ( x == LEFT )
  {
    lduty = 60 ; rduty = 50 ; dtime = 25 ;
  }
  if ( x == BIG_LEFT )
  {
    lduty = 70 ; rduty = 50 ; dtime = 25 ;
  }
  if ( x == BOTH_WHITE )
  {
    lduty = 30 ; rduty = 30 ; dtime = 10 ;
  }
  /* update motor parameters */
  update_motor();
  delay_ms( dtime );

  *(dir+0) = DIR_CENTER;
  if ( x == ALL_WHITE )
  {
    state = MD_CRANK ;
  }
  if ( x == LEFT_WHITE )
  {
    state = MD_LANE ;
    *(dir+0) = DIR_LEFT ;
  }
  if ( x == RIGHT_WHITE )
  {
    state = MD_LANE ;
    *(dir+0) = DIR_RIGHT ;
  }

  return state;
}

UBYTE execute_crank(UBYTE x)
{
  UBYTE state ;
  UBYTE dtime ;

  /* default */
  lduty = 30 ;
  rduty = 30 ;
  dtime = 30 ;

  if ( x == ALL_BLACK )
  {
    lduty = 10 ; rduty = 10 ; dtime = 10 ;
  }
  if ( x == ALL_WHITE )
  {
    lduty = 30 ; rduty = 30 ; dtime = 10 ;
  }
  if ( x == LEFT_WHITE )
  {
    lduty = 30 ; rduty = 30 ; dtime = 20 ;
  }
  if ( x == RIGHT_WHITE )
  {
    lduty = 30 ; rduty = 30 ; dtime = 20 ;
  }
  if ( x == CENTER )
  {
    lduty = 40 ; rduty = 40 ; dtime = 30 ;
  }
  if ( x == TINY_RIGHT )
  {
    lduty = 40 ; rduty = 45 ; dtime = 25 ;
  }
  if ( x == RIGHT ) 
  {
    lduty = 40 ; rduty = 50 ; dtime = 25 ;
  }
  if ( x == BIG_RIGHT )
  {
    lduty = 40 ; rduty = 55 ; dtime = 25 ;
  }
  if ( x == TINY_LEFT )
  {
    lduty = 45 ; rduty = 40 ; dtime = 25 ;
  }
  if ( x == LEFT )
  {
    lduty = 50 ; rduty = 40 ; dtime = 25 ;
  }
  if ( x == BIG_LEFT )
  {
    lduty = 55 ; rduty = 40 ; dtime = 25 ;
  }
  if ( x == BOTH_WHITE )
  {
    lduty = 30 ; rduty = 30 ; dtime = 20 ;
  }

  /* update motor parameters */
  update_motor();
  delay_ms( dtime );

  state = MD_CRANK ;
  *(dir+0) = DIR_CENTER;
  if ( x == ALL_WHITE )
  {
    state = MD_CRANK ;
  }
  if ( x == LEFT_WHITE )
  {
    state = MD_LANE ;
    *(dir+0) = DIR_LEFT ;
  }
  if ( x == RIGHT_WHITE )
  {
    state = MD_LANE ;
    *(dir+0) = DIR_RIGHT ;
  }

  return state;
}

UBYTE execute_rotate(UBYTE x)
{
  UBYTE state ;
  UBYTE dtime ;

  dtime = 30 ;
  state = MD_ROTATE ;
  if ( dir == DIR_LEFT )
  {
    lduty = 30 ; rduty = 50 ;
  }
  if ( dir == DIR_RIGHT )
  {
    lduty = 50 ; rduty = 30 ;
  }
  /* update motor parameters */
  update_motor();
  delay_ms( dtime );

  if ( x == CENTER )
  {
    lduty = 30 ; rduty = 30 ; *(dir+0) = DIR_CENTER ; state = MD_NORMAL ;
  }

  return state;
}

UBYTE execute_lane(UBYTE x)
{
  UBYTE state ;
  UBYTE dtime ;

  /* default */
  lduty = 30 ;
  rduty = 30 ;
  dtime = 30 ;
  state = MD_LANE ;

  if ( x == ALL_BLACK )
  {
    lduty = 10 ; rduty = 10 ; dtime = 10 ;
  }
  if ( x == ALL_WHITE ) 
  {
    lduty = 30 ; rduty = 30 ; dtime = 10 ;
  }
  if ( x == LEFT_WHITE || x == RIGHT_WHITE )
  {
    lduty = 30 ; rduty = 30 ; dtime = 30 ;
  }
  if ( x == CENTER )
  {
    lduty = 40 ; rduty = 40 ; dtime = 30 ;
  }
  if ( x == TINY_RIGHT )
  {
    lduty = 40 ; rduty = 45 ; dtime = 25 ;
  }
  if ( x == RIGHT )
  {
    lduty = 40 ; rduty = 50 ; dtime = 25 ;
  }
  if ( x == BIG_RIGHT )
  {
    lduty = 40 ; rduty = 55 ; dtime = 25 ;
  }
  if ( x == TINY_LEFT )
  {
    lduty = 45 ; rduty = 40 ; dtime = 25 ;
  }
  if ( x == LEFT )
  {
    lduty = 50 ; rduty = 40 ; dtime = 25 ;
  }
  if ( x == BIG_LEFT )
  {
    lduty = 55 ; rduty = 40 ; dtime = 25 ;
  }

  /* update motor parameters */
  update_motor();
  delay_ms( dtime );

  if ( x == ALL_BLACK )
  {
    state = MD_CHANGE ; *(dir+0) = DIR_CENTER ;
  }

  return state ;
}

UBYTE execute_change(UBYTE x)
{
  UBYTE state ;
  UBYTE dtime ;

  /* default */
  dtime = 50 ;
  state = MD_CHANGE ;

  /* turn */
  if ( *(dir+0) == DIR_LEFT )
  {
    lduty = 35 ; rduty = 55 ;
  }
  if ( *(dir+0) == DIR_RIGHT )
  {
    lduty = 55 ; rduty = 35 ;
  }

  /* update motor parameters */
  update_motor();
  delay_ms( dtime );

  /* straight */
  lduty = 30 ;
  rduty = 30 ;

  /* update motor parameters */
  update_motor();
  delay_ms( dtime );

  /* turn */
  if ( x == ALL_BLACK ) {
    if ( *(dir+0) == DIR_LEFT )
    {
	    *(dir+1) = DIR_RIGHT ;
    }
    if ( *(dir+0) == DIR_RIGHT )
    {
      *(dir+1) = DIR_LEFT ;
    }
  }
  return 0;
}

UBYTE execute_blind(UBYTE x)
{
  UBYTE state ;
  UBYTE dtime ;

  /* default */
  dtime = 50 ;
	state = MD_BLIND ;

  /* judge */
  if ( x == CENTER ) {
    if ( *(dir+1) == DIR_LEFT )
    {
      lduty = 25 ; rduty = 35 ;
    }
    if ( *(dir+1) == DIR_RIGHT )
    {
      lduty = 35 ; rduty = 25 ;
    }
    /* update motor parameters */
    update_motor();
    delay_ms( dtime );
    state = MD_NORMAL ;
  } else {
    lduty = 30 ; rduty = 30 ;
    /* update motor parameters */
    update_motor();
    delay_ms( dtime );
  }

  return state ;
}

void send_duty(UBYTE which,UBYTE x)
{
  /* clear */
  GP3DAT = 0xff000000 ;
  /* impress duty */
  GP3DAT |= (x << 16);
  /* send trigger */
  if ( which ) { GP4DAT &= ~(1 << BLD); }
  else         { GP4DAT &= ~(1 << ALD); }
  /* wait */
  delay_100us(1);
  /* send trigger */
  if ( which ) { GP4DAT |= (1 << BLD); }
  else         { GP4DAT |= (1 << ALD); }
}

void update_motor(void)
{
  /* send left duty */
  send_duty(ON, lduty);
  send_duty(OFF,rduty);
  /* send trigger */
  GP4DAT |= (1 << TRG);
  /* wait */
  delay_100us(1);
  /* send trigger */
  GP4DAT &= ~(1 << TRG);
}

UBYTE get_start(void)
{
  return 0;
}

UBYTE get_sensor(void)
{
  UBYTE result ;

  result = GP2DAT & MASK0F ;

  return result ;
}

　RTOSを使う場合、以下とします。

#include "3052.h"

typedef unsigned char  UBYTE ;
typedef unsigned short UWORD ;
typedef unsigned long  ULONG ;
typedef   signed char  SBYTE ;
typedef   signed short SWORD ;

#define NO  0
#define YES NO+1

typedef struct {
  void  (*tsk)(void);
  UWORD wcount ;
} TCBP ;

#define TSK_ID_MAX 9

#define TSK_ID0 0
#define TSK_ID1 1
#define TSK_ID2 2
#define TSK_ID3 3
#define TSK_ID4 4
#define TSK_ID5 5
#define TSK_ID6 6
#define TSK_ID7 7
#define TSK_ID8 8

#define NORMAL TSK_ID1
#define CRANK  TSK_ID2
#define ROTATE TSK_ID3
#define LANE   TSK_ID4
#define CHANGE TSK_ID5
#define BLIND  TSK_ID6

#define XDEBUG TSK_ID8

#define TTS_SUSPEND 0
#define TTS_WAIT    TTS_SUSPEND+1
#define TTS_READY   TTS_SUSPEND+2

TCBP tcb[TSK_ID_MAX];

/*----------------*/
/* user variables */
/*----------------*/
#define ITU1_AREG 24999

typedef union {
  struct {
    unsigned char B7:1;
    unsigned char B6:1;
    unsigned char B5:1;
    unsigned char B4:1;
    unsigned char B3:1;
    unsigned char B2:1;
    unsigned char B1:1;
    unsigned char B0:1;
  } BIT ;
  unsigned char DR ;
} FLAGSP ;

FLAGSP xflags ;

ULONG timcnt ;

#define SFLAG xflags.BIT.B0
#define UFLAG xflags.BIT.B1
#define WFLAG xflags.BIT.B2
#define TFLAG xflags.BIT.B3
#define MFLAG xflags.BIT.B4

#define P1DDR P1.DDR
#define P1DR  P1.DR.BYTE
#define P2DDR P2.DDR
#define P2DR  P2.DR.BYTE
#define P3DDR P3.DDR
#define P3DR  P3.DR.BYTE
#define P4DDR P4.DDR
#define P4DR  P4.DR.BYTE
#define P5DDR P5.DDR
#define P5DR  P5.DR.BYTE
#define P6DDR P6.DDR
#define P6DR  P6.DR.BYTE
#define P8DDR P8.DDR
#define P8DR  P8.DR.BYTE
#define P7DR  P7.DR.BYTE
#define P9DDR P9.DDR
#define P9DR  P9.DR.BYTE
#define PADDR PA.DDR
#define PADR  PA.DR.BYTE
#define PBDDR PB.DDR
#define PBDR  PB.DR.BYTE

#define MASKFFFF 0xffff
#define MASKFF   0xff
#define MASKCF   0xcf
#define MASK0F   0x0f
#define MASK80   0x80
#define MASK03   0x03

#define OFF      0
#define ON       OFF+1

#define MASKF0 0xf0

#define CLOSED 0
#define OPENED CLOSED+1

#define DIR_CENTER 0
#define DIR_RIGHT  1
#define DIR_LEFT   2

UBYTE sgate ;
UBYTE timeoutcnt ;

UBYTE dir[2] ;
UBYTE left  ;
UBYTE right ;

UBYTE state  ;
UBYTE mstate ;

UBYTE mode ;

UBYTE sloapf ;
UBYTE sloapr ;
UBYTE sloap  ;
UBYTE sloapt[3] ;

UBYTE xcnt3 ;
UBYTE xcnt5 ;
UBYTE xcnt6 ;

UBYTE normal_l[13] ;
UBYTE normal_r[13] ;

UBYTE crank_l[13] ;
UBYTE crank_r[13] ;

UBYTE lane_l[13] ;
UBYTE lane_r[13] ;

UBYTE run_tsk ;
UWORD ready   ;
UWORD suspend ;
UWORD waitq   ;
UWORD bpat[16] ;

#define ALL_BLACK   0
#define ALL_WHITE   1
#define LEFT_WHITE  2
#define RIGHT_WHITE 3
#define CENTER      4
#define TINY_RIGHT  5
#define RIGHT       6
#define BIG_RIGHT   7
#define TINY_LEFT   8
#define LEFT        9
#define BIG_LEFT    10
#define BOTH_WHITE  11
#define ILLEAGAL    12

#define GEAR0     10
#define GEAR5    150
#define GEAR10   300
#define GEAR15   450
#define GEAR20   600
#define GEAR25   750
#define GEAR30   900
#define GEAR35  1050
#define GEAR40  1200
#define GEAR45  1350
#define GEAR50  1500
#define GEAR55  1650
#define GEAR60  1800
#define GEAR65  1950
#define GEAR70  2100
#define GEAR75  2250
#define GEAR80  2400
#define GEAR85  2550
#define GEAR90  2700
#define GEAR95  2850
#define GEAR100 2999

#define IDLE 0
#define RUN  IDLE+1

#define SGATE_BIT 4
#define EDC_BIT   1

void init_sci_1(TBaudRate x);
void rs1_putchar(UBYTE x);
void rs1_crlf(void);
void rs1_puts(UBYTE *x);
void show_help(void);

UBYTE sindex ;
UBYTE sbuf[16];
UBYTE cmd ;
UBYTE asc_hex[16] = {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'} ;

/*--------------------------------*/
/* Insert user functions protoype */
/*--------------------------------*/
void  init_usr(void);
void  init_timer1(void);
void  init_table(void);
UBYTE get_hex(UBYTE x);
void  delay_ms(UWORD x);
void  binary_display(UBYTE x);
UWORD conv_value(UBYTE x);
UBYTE get_swx(void);
void  send_motor_power(UBYTE x);

/*-------------*/
/* system call */
/*-------------*/
void  cre_tsk(UBYTE tid,void (*tsk)(void));
void  rsm_tsk(UBYTE tid);
void  sus_tsk(UBYTE tid);
void  slp_tsk(void);
void  wai_tsk(UWORD x);
void  init_os(void);
void  timer_handler(void);
UBYTE is_tsk_ready(UBYTE tid);

/*------*/
/* task */
/*------*/
void  tsk0_proc(void);
void  tsk1_proc(void);
void  tsk2_proc(void);
void  tsk3_proc(void);
void  tsk4_proc(void);
void  tsk5_proc(void);
void  tsk6_proc(void);
void  tsk7_proc(void);
void  tsk8_proc(void);

/*------*/
/* main */
/*------*/
int main(void)
{
  TCBP pcur_tsk ;
  /* disable interrupt */
  DI ;
  /* initialize */
  init_usr();
  /* initialize monitor */
  init_os();
  /* enable interrupt */
  EI ;
  /* opening message */
  rs1_puts("Hello");
  rs1_crlf();
  /* endless loop */
  run_tsk = TSK_ID0 ;
  while ( ON ) {
    /* RTOS */
    pcur_tsk = tcb[run_tsk] ;
    if ( is_tsk_ready( run_tsk ) ) { (*(pcur_tsk.tsk))(); }
    run_tsk++;
    if ( run_tsk == TSK_ID_MAX ) { run_tsk = TSK_ID0 ; }
    /* 10ms handling */
    if ( WFLAG == ON ) {
      WFLAG = OFF ;
      timer_handler();
    }
  }
  return 0 ;
}

/*-----------------------*/
/* Insert user functions */
/*-----------------------*/
void init_usr(void)
{
  /* PORT 4 */
  P4DR  = 0x00 ;
  P4DDR = MASKFF ; /* all outputs */
  /* PORT 6 */
  P6DR  = 0x00 ;
  P6DDR = 0x00 ; /* all inputs */
  /* PORT A */
  PADR  = 0      ;
  PADDR = MASKFF ; /* all outputs */
  /* PORT B */
  PBDR  = 0xc0 ;
  PBDDR = 0xcf ; /* PB5,PB4 are inputs , others are outputs */
  /* initialize */
  init_timer1();
  /* clear flags */
  xflags.DR = 0 ;
  /* initialize PWM */
  {
    ITU.TSTR.BIT.STR3 = (UBYTE)0x00; /* Stop the counter */
    ITU.TOER.BYTE     = (UBYTE)0x00;
    ITU3.TCR.BYTE     = (UBYTE)0xa0;
    ITU3.TCNT         = (UWORD)0x00;
    ITU.TFCR.BYTE     = (UBYTE)0xf6;
    ITU3.GRA          = (UWORD)3000;
    ITU4.GRA          = (UWORD)GEAR0;
    ITU3.GRB          = (UWORD)GEAR0;
  }
  /* start PWM */
  {
    ITU.TOER.BYTE     = (UBYTE)0xff;
    ITU.TSTR.BIT.STR3 = (UBYTE)0x01;
    ITU3.BRB          = (UWORD)GEAR10;
    ITU4.BRA          = (UWORD)GEAR10;
  }
  /* clear SCI buffer */
  *(sbuf+0) = 0 ;
  sindex = 0 ;
  /* initialize */
  timcnt = 0 ;
  xcnt3 = 0 ;
  xcnt5 = 0 ;
  xcnt6 = 0 ;
  /* SCI */
  init_sci_1(br9600);
  /* others */
  init_table();
  left   = 0 ;
  right  = 0 ;
  state = IDLE ;
  mstate = 0 ;
  sloap = 0 ;
  sloapf = 0 ;
  sloapr = 0 ;
  *(sloapt+0) = 100 ;
  *(sloapt+1) = 120 ;
  *(sloapt+2) =  80 ;
}

void init_timer1(void)
{
  /* stop timer */
  ITU.TSTR.BIT.STR1 = OFF ;
  /* TOER : Timer Output Enable Register
        7 **** -> 0
        6 **** -> 0
        5 EXB4 -> 0
        4 EXA4 -> 0
        3 EB3  -> 0
        2 EB4  -> 0
        1 EA4  -> 0
        0 EA3  -> 0
  */
  ITU.TOER.BYTE = 0 ;
  /* TIOR : Timer I/O Control Register
        7 **** -> 0
        6 IOB2 -> 0 GRB is not output compare match register
        5 IOB1 -> 0
        4 IOB0 -> 0
        3 **** -> 0
        2 IOA2 -> 0 GRA is not output compare match register
        1 IOA1 -> 0
        0 IOA0 -> 0
  */
  ITU1.TIOR.BYTE = 0 ;
  /* TCR : Timer Control Register
        7 ****  -> 0
        6 CCLR1 -> 0 clear TCNT if GRA = TCNT
        5 CCLR0 -> 1
        4 CKEG1 -> 0 rising edge
        3 CKEG0 -> 0
        2 TPSC2 -> 0 φ利用
        1 TPSC1 -> 0
        0 TPSC0 -> 0
  */
  ITU1.TCR.BYTE = 0x20 ;
  /* TIER : Timer Interrupt Enable Register
        7 ****  -> 0
        6 ***   -> 0
        5 ***   -> 0
        4 ***   -> 0
        3 ***   -> 0
        2 OVIE  -> 0
        1 IMIEB -> 0
        0 IMIEA -> 1 select compare match interrupt
  */
  ITU1.TIER.BIT.IMIEA = ON ;
  /* reference */
  ITU1.GRA = ITU1_AREG ;
  ITU1.GRB = MASKFFFF ;
  /* counter */
  ITU1.TCNT = 0 ;
  /* start timer */
  ITU.TSTR.BIT.STR1 = ON ;
}

/*+++++++++++++++++++++++++++++++++++++*/
/* ITU1 interrupt with compare match A */
/*                     1ms interval */
/*+++++++++++++++++++++++++++++++++++++*/
void int_imia1(void)
{
  UBYTE dummy ;
  /* clear flag */
  dummy = ITU1.TSR.BIT.IMFA ;
  ITU1.TSR.BIT.IMFA = OFF ;
  /* increment */
  timcnt++ ;
  /* judge 10ms passed */
  if ( timcnt & 10 ) { WFLAG = ON ; }
  /* judge 1000ms passed */
  if ( (timcnt & 1023) == 100 ) {
    timeoutcnt++ ;
  }
}

/*+++++++++++++++++++++++++*/
/* SCI_1 receive interrupt */
/*+++++++++++++++++++++++++*/
void  init_sci_1(TBaudRate x)
{
  volatile UWORD i;
  /* SCR : Serial Control Register
    7 bit TIE  -> 0 Transmit Interrupt Enable(disable)
    6 bit RIE  -> 0 Receive  Interrupt Enable(disable)
    5 bit TE   -> 0 Transmit Enable(disable)
    4 bit RE   -> 0 Receive  Enable(disable)
    3 bit MPIE -> 0 Multi Processor Interrupt Enable(disable)
    2 bit TEIE -> 0 Transmit End Interrupt Enable(disable)
    1 bit CKE1 -> 0 Clock Source (Use Internal Baud Rate Generator)
    0 bit CKE0 -> 0 
  */
  SCI1.SCR.BYTE = 0 ;
  /* SMR : Serial Mode Register
    7 bit C/nA -> 0 Communication Mode(Asynchronous)
    6 bit CHR  -> 0 data Charactor (8 bits)
    5 bit PE   -> 0 Parity Enable(disable)
    4 bit O/nE -> 0 Parity Mode(even)
    3 bit STOP -> 0 Stop Bit(1 bit)
    2 bit MP   -> 0 Multi Processor(disable)
    1 bit CKS1 -> 0 Clock Source ( φ )
    0 bit CKS0 -> 0 
  */
  SCI1.SMR.BYTE = 0 ;
  /* data transfer speed */
  SCI1.BRR = x ;
  /* wait 1 frame */
  for (i = 0; i < 3000 ; i++) ;
  /* enable Transmmit and Receive with interrupt */
  SCI1.SCR.BYTE = 0x70 ;
}

/*+++++++++++++++++++++++++*/
/* SCI_1 receive interrupt */
/*+++++++++++++++++++++++++*/
void int_rxi1(void)
{
  volatile UBYTE ch,dummy ;
  /* clear flag */
  dummy = SCI1.SSR.BYTE ;
  SCI1.SSR.BIT.RDRF = OFF ;
  /* get a character */
  ch = SCI1.RDR ;
  /* store */
  *(sbuf+sindex) = ch ;
  sindex++ ;
  /* check */
  if ( ch == '\r' ) {
    *(sbuf+sindex) = 0 ;
    sindex = 0 ;
    UFLAG  = ON ;
  }
}

/*+++++++++++++++*/
/* SCI_1 putchar */
/*+++++++++++++++*/
void rs1_putchar(UBYTE x)
{
  /* wait data transfer */
  while ( SCI1.SSR.BIT.TDRE == OFF ) ;
  /* put */
  SCI1.TDR = x ;
  SCI1.SSR.BIT.TDRE = OFF ;
}

/*++++++++++++*/
/* SCI_1 puts */
/*++++++++++++*/
void rs1_puts(UBYTE *x)
{
  while ( *x ) {
    /* send 1 charactors */
    rs1_putchar(*x);
    x++ ;
  }
}

/*++++++++++++*/
/* SCI_1 crlf */
/*++++++++++++*/
void rs1_crlf(void)
{
  rs1_putchar('\r');
  rs1_putchar('\n');
}

/*++++++++++++++++++++*/
/* SCI_1 command help */
/*++++++++++++++++++++*/
void show_help(void)
{
  rs1_puts("? help")    ; rs1_crlf();
  rs1_puts("E enable")  ; rs1_crlf();
  rs1_puts("e disable") ; rs1_crlf();
}

void  init_table(void)
{
  /* NORMAL */
  *(normal_l+ALL_BLACK)   = 10 ; *(normal_r+ALL_BLACK)   = 10 ;
  *(normal_l+ALL_WHITE)   = 30 ; *(normal_r+ALL_WHITE)   = 30 ;
  *(normal_l+LEFT_WHITE)  = 30 ; *(normal_r+LEFT_WHITE)  = 30 ;
  *(normal_l+RIGHT_WHITE) = 30 ; *(normal_r+RIGHT_WHITE) = 30 ;
  *(normal_l+CENTER)      = 50 ; *(normal_r+CENTER)      = 50 ;
  *(normal_l+TINY_RIGHT)  = 50 ; *(normal_r+TINY_RIGHT)  = 55 ;
  *(normal_l+RIGHT)       = 50 ; *(normal_r+RIGHT)       = 60 ;
  *(normal_l+BIG_RIGHT)   = 50 ; *(normal_r+BIG_RIGHT)   = 70 ;
  *(normal_l+TINY_LEFT)   = 55 ; *(normal_r+TINY_LEFT)   = 50 ;
  *(normal_l+LEFT)        = 60 ; *(normal_r+LEFT)        = 50 ;
  *(normal_l+BIG_LEFT)    = 70 ; *(normal_r+BIG_LEFT)    = 50 ;
  *(normal_l+BOTH_WHITE)  = 20 ; *(normal_r+BOTH_WHITE)  = 20 ;
  *(normal_l+ILLEAGAL)    = 10 ; *(normal_r+ILLEAGAL)    = 10 ;
  /* CRANK */
  *(crank_l+ALL_BLACK)   = 10 ; *(crank_r+ALL_BLACK)   = 10 ;
  *(crank_l+ALL_WHITE)   = 25 ; *(crank_r+ALL_WHITE)   = 25 ;
  *(crank_l+LEFT_WHITE)  = 25 ; *(crank_r+LEFT_WHITE)  = 25 ;
  *(crank_l+RIGHT_WHITE) = 25 ; *(crank_r+RIGHT_WHITE) = 25 ;
  *(crank_l+CENTER)      = 30 ; *(crank_r+CENTER)      = 30 ;
  *(crank_l+TINY_RIGHT)  = 30 ; *(crank_r+TINY_RIGHT)  = 35 ;
  *(crank_l+RIGHT)       = 30 ; *(crank_r+RIGHT)       = 40 ;
  *(crank_l+BIG_RIGHT)   = 30 ; *(crank_r+BIG_RIGHT)   = 45 ;
  *(crank_l+TINY_LEFT)   = 35 ; *(crank_r+TINY_LEFT)   = 30 ;
  *(crank_l+LEFT)        = 40 ; *(crank_r+LEFT)        = 30 ;
  *(crank_l+BIG_LEFT)    = 45 ; *(crank_r+BIG_LEFT)    = 30 ;
  *(crank_l+BOTH_WHITE)  = 20 ; *(crank_r+BOTH_WHITE)  = 20 ;
  *(crank_l+ILLEAGAL)    = 10 ; *(crank_r+ILLEAGAL)    = 10 ;
  /* LANE */
  *(lane_l+ALL_BLACK)   = 10 ; *(lane_r+ALL_BLACK)   = 10 ;
  *(lane_l+ALL_WHITE)   = 25 ; *(lane_r+ALL_WHITE)   = 25 ;
  *(lane_l+LEFT_WHITE)  = 25 ; *(lane_r+LEFT_WHITE)  = 25 ;
  *(lane_l+RIGHT_WHITE) = 25 ; *(lane_r+RIGHT_WHITE) = 25 ;
  *(lane_l+CENTER)      = 30 ; *(lane_r+CENTER)      = 30 ;
  *(lane_l+TINY_RIGHT)  = 30 ; *(lane_r+TINY_RIGHT)  = 35 ;
  *(lane_l+RIGHT)       = 30 ; *(lane_r+RIGHT)       = 40 ;
  *(lane_l+BIG_RIGHT)   = 30 ; *(lane_r+BIG_RIGHT)   = 45 ;
  *(lane_l+TINY_LEFT)   = 35 ; *(lane_r+TINY_LEFT)   = 30 ;
  *(lane_l+LEFT)        = 40 ; *(lane_r+LEFT)        = 30 ;
  *(lane_l+BIG_LEFT)    = 45 ; *(lane_r+BIG_LEFT)    = 30 ;
  *(lane_l+BOTH_WHITE)  = 20 ; *(lane_r+BOTH_WHITE)  = 20 ;
  *(lane_l+ILLEAGAL)    = 10 ; *(lane_r+ILLEAGAL)    = 10 ;
}

UBYTE get_hex(UBYTE x)
{
  UBYTE result ;
  /* default */
  result = 0 ;
  /* convert */
  if ( '0' <= x && x <= '9' ) { result = x - '0' ; }
  if ( 'A' <= x && x <= 'F' ) { result = x - 'A' + 10 ; }
  if ( 'a' <= x && x <= 'f' ) { result = x - 'a' + 10 ; }

  return result ;
}

void delay_ms(UWORD x)
{
  ULONG target ;
  /* calculate last value */
  target = timcnt + x ;
  /* wait */
  while ( timcnt < target ) ;
}

void  binary_display(UBYTE x)
{
  UBYTE loop ;
  UBYTE bdat ;
  /* show */
  for ( loop = 0 ; loop < 8 ; loop++ ) {
    bdat = '0' ;
    if ( x & MASK80 ) { bdat++ ; }
    rs1_putchar( bdat );
    x <<= 1 ;
  }
}

UWORD conv_value(UBYTE x)
{
  UWORD result ;
  /* convert */
  switch ( x ) {
    case   5 : result =   GEAR5 ; break ;
    case  10 : result =  GEAR10 ; break ;
    case  15 : result =  GEAR15 ; break ;
    case  20 : result =  GEAR20 ; break ;
    case  25 : result =  GEAR25 ; break ;
    case  30 : result =  GEAR30 ; break ;
    case  35 : result =  GEAR35 ; break ;
    case  40 : result =  GEAR40 ; break ;
    case  45 : result =  GEAR45 ; break ;
    case  50 : result =  GEAR50 ; break ;
    case  55 : result =  GEAR55 ; break ;
    case  60 : result =  GEAR60 ; break ;
    case  65 : result =  GEAR65 ; break ;
    case  70 : result =  GEAR70 ; break ;
    case  75 : result =  GEAR75 ; break ;
    case  80 : result =  GEAR80 ; break ;
    case  85 : result =  GEAR85 ; break ;
    case  90 : result =  GEAR90 ; break ;
    case  95 : result =  GEAR95 ; break ;
    case 100 : result = GEAR100 ; break ;
    default  : result =   GEAR0 ; break ;
  }

  return result ;
}

void  send_dc(UBYTE lx,UBYTE rx)
{
  UWORD left ;
  UWORD right ;
  /* convert */
  left  = conv_value( lx );
  right = conv_value( rx );
  /* store */
  ITU3.BRB = (UWORD)left;
  ITU4.BRA = (UWORD)right;
}

UBYTE get_sensor(void)
{
  UBYTE result ;
  /* get information from PORT7 lower nibble */
  result = P7DR ;
  /* masking */
  result &= MASK0F ;

  return result ;
}

UBYTE get_start(void)
{
  return sgate;
}

void  show_state(UBYTE x)
{
  UBYTE result;
  /* generate code */
  result = (1 << x);
  /* impress */
  P4DR = result ^ MASKFF ;
}

UBYTE get_swx(void)
{
  UBYTE tmp ;
  /* get switch state */
  tmp = P6DR ^ MASKF0 ;
  /* shift */
  tmp >>= 4 ;

  return tmp ;
}

void  send_motor_power(UBYTE x)
{
  if ( x == ON ) { PBDR |=  (1 << EDC_BIT); }
  else           { PBDR &= ~(1 << EDC_BIT); }
}

/* system call */
void cre_tsk(UBYTE tid,void (*tsk)(void))
{
  tcb[tid].tsk = tsk;
  tcb[tid].wcount = 0;
}

void sta_tsk(UBYTE tid,UBYTE tst)
{
  UWORD xtmp ;
  xtmp = (1 << tid) ;
  if ( tst == TTS_READY   ) { ready   |= xtmp ; }
  if ( tst == TTS_SUSPEND ) { suspend |= xtmp ; }
  if ( tst == TTS_WAIT    ) { waitq   |= xtmp ; }
}

void rsm_tsk(UBYTE tid)
{
  UWORD tmp ;
  UWORD tmpx ;
  /* get bit pattern */
  tmp  = *(bpat+tid);
  tmpx = tmp ^ MASKFFFF ;
  /* bit operation */
  ready   |= tmp;
  suspend &= tmpx;
  waitq   &= tmpx;
}

void sus_tsk(UBYTE tid)
{
  UWORD tmp ;
  UWORD tmpx ;
  /* get bit pattern */
  tmp  = *(bpat+tid);
  tmpx = tmp ^ MASKFFFF ;
  /* bit operation */
  ready   &= tmpx;
  suspend |= tmp ;
  waitq   &= tmpx;
}

void slp_tsk(void)
{
  sus_tsk(run_tsk);
}

void wai_tsk(UWORD x)
{
  UWORD tmp ;
  UWORD tmpx ;
  /* get bit pattern */
  tmp  = *(bpat+run_tsk);
  tmpx = tmp ^ MASKFFFF ;
  /* bit operation */
  ready   &= tmpx;
  suspend &= tmpx;
  waitq   |= tmp ;
  /* update counter */
  tcb[run_tsk].wcount = x ;
}

void init_os(void)
{
  UBYTE i;
  /* initialize queue */
  ready   = 0;
  suspend = 0;
  waitq   = 0;
  /* initalize pattern */
  for ( i = 0 ; i < 16 ; i++ ) { *(bpat+i) = (1 << i) ; }
  /* create task */
  cre_tsk(TSK_ID0,tsk0_proc); cre_tsk(NORMAL ,tsk1_proc);
  cre_tsk(CRANK  ,tsk2_proc); cre_tsk(ROTATE ,tsk3_proc);
  cre_tsk(LANE   ,tsk4_proc); cre_tsk(CHANGE ,tsk5_proc);
  cre_tsk(BLIND  ,tsk6_proc); cre_tsk(TSK_ID7,tsk7_proc);
  cre_tsk(XDEBUG ,tsk8_proc);
  /* start task state */
  sta_tsk(TSK_ID0,TTS_READY)  ; sta_tsk(TSK_ID1,TTS_SUSPEND);
  sta_tsk(TSK_ID2,TTS_SUSPEND); sta_tsk(TSK_ID3,TTS_SUSPEND);
  sta_tsk(TSK_ID4,TTS_SUSPEND); sta_tsk(TSK_ID5,TTS_SUSPEND);
  sta_tsk(TSK_ID6,TTS_SUSPEND); sta_tsk(TSK_ID7,TTS_READY);
  sta_tsk(TSK_ID8,TTS_READY)  ;
}

void  timer_handler(void)
{
  UBYTE i ;

  for ( i = 0 ; i < TSK_ID_MAX ; i++ ) {
    /* judge WAIT state */
    if ( waitq & *(bpat+i) ) {
      tcb[i].wcount-- ;
      if ( tcb[i].wcount == 0 ) { rsm_tsk(i); }
    }
  }
}

UBYTE is_tsk_ready(UBYTE tid)
{
  return( ready & *(bpat+tid) ) ;
}

/* system control */
void tsk0_proc(void)
{
  /* debug run */
  mode = get_swx() ;
  if ( mode & 0x08 ) {
    /* set flag */
    MFLAG = ON ;
    /* get mode */
    mode &= 7 ;
    /* change state */
    state = RUN ;
    /* crank */
    if ( mode == 2 ) { rsm_tsk(CRANK); }
    /* rotate */
    if ( mode == 3 ) { rsm_tsk(ROTATE); }
    /* lane */
    if ( mode == 4 ) { rsm_tsk(LANE); }
    /* change */
    if ( mode == 5 ) { rsm_tsk(CHANGE); }
    /* blind */
    if ( mode == 6 ) {  rsm_tsk(BLIND); }
    /* enable motor power */
    send_motor_power(ON);
    /* timer trigger clear */
    timeoutcnt = 0 ;
    TFLAG = OFF ;
  }
  /* start trigger */
  if ( state == IDLE ) {
    if ( get_start() == OPENED ) {
      state = RUN ;
      rsm_tsk(NORMAL);
      /* enable motor power */
      send_motor_power(ON);
      /* timer trigger clear */
      timeoutcnt = 0 ;
      TFLAG = OFF ;
    }
  }
  /* timeout */
  if ( state == RUN ) {
    if ( TFLAG == ON ) {
      state = IDLE ;
      /* suspend move handling task */
      state = IDLE ;
      sus_tsk(NORMAL);
      sus_tsk(CRANK);
      sus_tsk(ROTATE);
      sus_tsk(LANE);
      sus_tsk(CHANGE);
      sus_tsk(BLIND);
      /* machine state */
      mstate = 0 ;
      /* disable motor power */
      send_motor_power(OFF);
    }
  }
  /* show state */
  show_state( mstate );
}

/* NORMAL */
void tsk1_proc(void)
{
  UBYTE xmul ;
  UBYTE flag ;
  UBYTE xsensor ;
  /* default */
  flag = OFF ;
  /* show state */
  mstate = 1 ;
  show_state( mstate );
  /* get sensor */
  xsensor = get_sensor();
  /* update motor duty ratio */
  left  = *(normal_l+xsensor);
  right = *(normal_r+xsensor);
  /* default */
  *(dir+0) = DIR_CENTER ;
  *(dir+1) = DIR_CENTER ;
  /* get sensor */
  if ( xsensor == CENTER ) {
    /* climb hill or down hill */
    if ( sloap ) {
      xmul = *(sloapt+sloap) ;
      left  = (UBYTE)( xmul * left * 1.0 / 100 );
      right = (UBYTE)( xmul * right * 1.0 / 100 );
    }
  }
  /* update */
  send_dc(left,right);
  /* judge */
  if ( xsensor == ALL_WHITE ) {
    rsm_tsk(CRANK);
    flag = ON ;
  }
  if ( xsensor == LEFT_WHITE ) {
    *(dir+0) = DIR_LEFT ;
    *(dir+1) = DIR_RIGHT ;
    rsm_tsk(LANE);
    flag = ON ;
  }
  if ( xsensor == RIGHT_WHITE ) {
    *(dir+0) = DIR_RIGHT ;
    *(dir+0) = DIR_LEFT ;
    rsm_tsk(LANE);
    flag = ON ;
  }
  /* cyclic */
  if ( flag == ON ) {
    flag = OFF ;
    slp_tsk();
  } else {
    wai_tsk( 10 );
  }
}

/* CRANK */
void tsk2_proc(void)
{
  UBYTE flag ;
  UBYTE xsensor ;
  /* default */
  flag = OFF ;
  /* show state */
  mstate = 2 ;
  show_state( mstate );
  /* get sensor */
  xsensor = get_sensor();
  /* update motor duty ratio */
  left  = *(crank_l+xsensor);
  right = *(crank_r+xsensor);
  /* update */
  send_dc(left,right);
  /* judge */
  if ( xsensor == LEFT_WHITE ) {
    *(dir+0) = DIR_LEFT ;
    *(dir+1) = DIR_RIGHT ;
  }
  if ( xsensor == RIGHT_WHITE ) {
    *(dir+0) = DIR_RIGHT ;
    *(dir+1) = DIR_LEFT ;
  }
  if ( xsensor == ALL_BLACK  ) {
    /* initialize sequence counter */
    xcnt3 = 0 ;
    /* update state */
    rsm_tsk(ROTATE);
    flag = ON ;
  }
  /* cyclic */
  if ( flag == ON ) {
    flag = OFF ;
    slp_tsk();
  } else {
    wai_tsk( 100 );
  }
}

/* ROTATE */
void tsk3_proc(void)
{
  UBYTE flag ;
  UBYTE xsensor ;
  /* default */
  flag = OFF ;
  /* show state */
  mstate = 3 ;
  show_state( mstate );
  /* get sensor */
  xsensor = get_sensor();
  /* sequence */
  switch ( xcnt3 ) {
    /* rotate */
    case 0 :
      xcnt3 = 1 ;
      left = 50 ; right = 10 ;
      if ( *(dir+0) == DIR_RIGHT ) {
        left = 10 ; right = 50 ;
      }
      break ;
    /* judge */
    case 1 :
      xcnt3 = 1 ;
      if ( xsensor == CENTER ) {
        left = 30 ; right = 30 ;
        xcnt3 = 2 ; 
      }
      break ;
    /* straight and move */
    case 2 :
      xcnt3 = 3 ;
      break ;
    /* exit */
    case 3 :
      rsm_tsk(NORMAL);
      flag = ON ;
      break ;
    /* others */
    default:
      break;
  }
  /* update */
  send_dc(left,right);
  /* cyclic */
  if ( flag == ON ) {
    xcnt3 = 0 ;
    flag = OFF ;
    slp_tsk();
  } else {
    wai_tsk( 100 );
  }
}

/* LANE */
void tsk4_proc(void)
{
  UBYTE flag ;
  UBYTE xsensor ;
  /* default */
  flag = OFF ;
  /* show state */
  mstate = 4 ;
  show_state( mstate );
  /* get sensor */
  xsensor = get_sensor();
  /* update motor duty ratio */
  left  = *(lane_l+xsensor) ;
  right = *(lane_r+xsensor) ;
  /* update */
  send_dc(left,right);
  /* judge */
  if ( xsensor == ALL_BLACK ) {
    /* initialize sequence counter */
    xcnt5 = 0 ;
    /* update state */
    rsm_tsk(CHANGE);
    flag = ON ;
  }
  if ( xsensor == LEFT_WHITE ) {
    *(dir+0) = DIR_LEFT  ;
    *(dir+1) = DIR_RIGHT ;
  }
  if ( xsensor == RIGHT_WHITE ) {
    *(dir+0) = DIR_RIGHT ;
    *(dir+1) = DIR_LEFT  ;
  }
  /* cyclic */
  if ( flag == ON ) {
    flag = OFF ;
    slp_tsk();
  } else {
    wai_tsk( 10 );
  }
}

/* CHANGE */
void tsk5_proc(void)
{
  UBYTE flag ;
  /* default */
  flag = OFF ;
  /* show state */
  mstate = 5 ;
  show_state( mstate );
  /* sequence */
  switch ( xcnt5 ) {
    /* rotate */
    case 0 :
      xcnt5 = 1 ;
      /* set rotate direction */
      left = 50 ; right = 30 ;
      if ( *(dir+0) == DIR_LEFT ) {
        left = 30 ; right = 50 ;
      }
      break ;
    /* straight */
    case 1 :
      xcnt5 = 2 ;
      left = 30 ; right = 30 ;
      break ;
    /* move */
    case 2 :
      xcnt5 = 3 ;
      break ;
    /* exit */
    case 3 :
      rsm_tsk(BLIND);
      flag = ON ;
      break ;
    /* others */
    default:
      break;
  }
  /* update */
  send_dc(left,right);
  /* cyclic */
  if ( flag == ON ) {
    flag = OFF ;
    xcnt5 = 0 ;
    xcnt6 = 0 ;
    slp_tsk();
  } else {
    wai_tsk( 10 );
  }
}

/* BLIND */
void tsk6_proc(void)
{
  UBYTE flag ;
  UBYTE xsensor ;
  /* default */
  flag = OFF ;
  /* show state */
  mstate = 6 ;
  show_state( mstate );
  /* get sensor */
  xsensor = get_sensor();
  /* sequence */
  switch ( xcnt6 ) {
    /* straight */
    case 0 :
      xcnt6 = 1 ;
      left = 30 ; right = 30 ;
      break ;
    /* judge and move */
    case 1 :
      xcnt6 = 1 ;
      if ( xsensor == CENTER     ) { xcnt6 = 2 ; }
      if ( xsensor == TINY_LEFT  ) { xcnt6 = 2 ; }
      if ( xsensor == TINY_RIGHT ) { xcnt6 = 2 ; }
      break ;
    /* rotate */
    case 2 :
      xcnt6 = 3 ;
      /* set rotate direction */
      left = 50 ; right = 30 ;
      if ( *(dir+1) == DIR_LEFT ) {
        left = 30 ; right = 50 ;
      }
      break ;
    /* move */
    case 3 :
      xcnt6 = 4 ;
      left = 30 ; right = 30 ;
      break ;
    /* exit */
    case 4 :
      rsm_tsk(NORMAL);
      flag = ON ;
      break ;
    /* others */
    default:
      break;
  }
  /* update */
  send_dc(left,right);
  /* cyclic */
  if ( flag == ON ) {
    flag = OFF ;
    xcnt6 = 0 ;
    slp_tsk();
  } else {
    wai_tsk( 10 );
  }
}

/* sensing */
void tsk7_proc(void)
{
  UBYTE stmp ;
  UBYTE xsensor ;
  /* get sensor data */
  xsensor = P7DR ;
  /* start gate */
  sgate = CLOSED ;
  if ( !(xsensor & (1 << SGATE_BIT)) ) { sgate = OPENED ; }
  /* sloap */
  sloapf <<= 1 ; sloapf &= MASK03 ;
  sloapr <<= 1 ; sloapr &= MASK03 ;
  if ( xsensor & 0x40 ) { sloapr |= ON ; }
  if ( xsensor & 0x80 ) { sloapf |= ON ; }
  sloap = 0 ;
  if ( sloapf == 3 ) { sloap |= 1 ; }
  if ( sloapr == 3 ) { sloap |= 2 ; }
  /* time out */
  if ( timeoutcnt > 119 ) { TFLAG = ON ; }
  /* wait 50ms */
  wai_tsk(5);
}

/* execute debug */
void tsk8_proc(void)
{
  if ( UFLAG == ON ) {
    /* clear */
    UFLAG = OFF ;
    /* new line */
    rs1_crlf();
    /* command interpreter */
    cmd = *(sbuf+0) ;
    /* help */
    if ( cmd == '?' ) { show_help() ; }
    /* enable DCDC conveter */
    if ( cmd == 'E' ) { send_motor_power(ON); }
    /* disable DCDC conveter */
    if ( cmd == 'e' ) { send_motor_power(OFF); }
  }
}

---

目次 前 次