目次

RTOS利用

 複数のマイコン、デジタル回路を載せた制御基板の中で
 最上位にあるプロセッサは、各マイコン、デジタル回路
 の処理を整理して協調動作させなければなりません。

 複数の機能を単独と協調させるには、RTOSを使った方が楽になるので
 自作のRTOSであるUSO(Unvoiced Shadow Operating system)を使います。

 次の条件で、RTOSを使えるかを検討しました。

 USOは最大16個のタスクの面倒をみることができるので
 各タスクに機能を割り当てて動かします。

 USOを使ったMCR_VCマシンの制御基板に、ATmega644(40ピンのAVR)を
 選んで、実現できるかを次のコードで確認してみます。

#include <avr/io.h>
#include <avr/interrupt.h>

#define OFF 0
#define ON  OFF+1

#define LED_OFF 1
#define LED_ON  0

#define FOSC   16000000
#define BAUD   9600
#define MYUBRR (FOSC/16/BAUD)-1

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

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

#define TSK_ID_MAX 8

#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 NORMAL TSK_ID1
#define CRANK  TSK_ID2
#define ROTATE TSK_ID3
#define LANE   TSK_ID4
#define CHANGE TSK_ID5
#define BLIND  TSK_ID6

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

TCBP tcb[TSK_ID_MAX];

#define BUFSIZE 8

#define CLOSED 0
#define OPENED CLOSED+1

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

volatile UBYTE lcnt ;

volatile ULONG tickcount ;
volatile UBYTE tmp ;
volatile UBYTE sensor ;
volatile UBYTE sgate ;
volatile UBYTE timeout ;

volatile UBYTE dir ;
volatile UBYTE left  ;
volatile UBYTE right ;

volatile UBYTE state  ;
volatile UBYTE mstate ;

volatile UBYTE wflag ;

volatile UBYTE run_tsk ;
volatile UBYTE ready   ;
volatile UBYTE suspend ;
volatile UBYTE waitq   ;

volatile UBYTE spat[13];

#define NO  0
#define YES NO+1

#define MASKFF 0xff
#define MASK0F 0x0f
#define MASKF0 0xf0
#define MASK80 0x80
#define MASK20 0x20

#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 IDLE 0
#define RUN  IDLE+1

#define TOUT_BIT  5
#define SGATE_BIT 4

#define SS_TRG    5
#define SRS_TRG   4
#define TRG       3
#define SRV_TRG   2
#define LEFT_TRG  1
#define RIGHT_TRG 0

/*--------------------------------*/
/* Insert user functions protoype */
/*--------------------------------*/
void  user_initialize(void);

UBYTE get_hex(UBYTE x);
void  delay_ms(UWORD x);
void  send_dc(UBYTE lx,UBYTE rx);
UBYTE get_sensor(void);
UBYTE get_start(void);
void  init_spat(void);
void  show_road_state(UBYTE x);
void  show_state(UBYTE x);

UWORD conv_value(UBYTE x);

/*-------------*/
/* system call */
/*-------------*/
void  cre_tsk(UBYTE tid,void (*tsk)(void));
void  sta_tsk(UBYTE tid,UBYTE sta);
void  rsm_tsk(UBYTE tid);
void  sus_tsk(UBYTE tid);
void  slp_tsk(void);
void  wai_tsk(UWORD x);
void  init_os(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);

/*------*/
/* main */
/*------*/
int main(void)
{
  TCBP  pcur_tsk ;
  UBYTE tmp ;
  UBYTE i ;
  /* disable interrupt */
  cli();
  /* initialize port and variables */
  user_initialize();
  /* initialize monitor */
  init_os();
  /* 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);
  /* start task task */
  sta_tsk(TSK_ID0,TTS_READY);
  sta_tsk(NORMAL ,TTS_SUSPEND);
  sta_tsk(CRANK  ,TTS_SUSPEND);
  sta_tsk(ROTATE ,TTS_SUSPEND);
  sta_tsk(LANE   ,TTS_SUSPEND);
  sta_tsk(CHANGE ,TTS_SUSPEND);
  sta_tsk(BLIND  ,TTS_SUSPEND);
  sta_tsk(TSK_ID7,TTS_READY);
  /* enable interrupt */
  sei();
  /* endless loop */
  run_tsk = TSK_ID0 ;
  while ( ON ) {
    /* RTOS */
    pcur_tsk = tcb[run_tsk] ;
    if ( is_tsk_ready( run_tsk ) == ON ) { (*(pcur_tsk.tsk))(); }
    run_tsk++;
    if ( run_tsk == TSK_ID_MAX ) { run_tsk = TSK_ID0 ; }
    /* cyclic handling */
    if ( wflag ) {
      wflag = OFF ;
      tmp = waitq ;
      for ( i = 0 ; i < TSK_ID_MAX ; i++ ) {
        if ( tmp & ON ) {
          tcb[i].wcount-- ;
          if ( tcb[i].wcount == 0 ) { rsm_tsk(i); }
        }
        tmp >>= 1 ;
      }
    }
  }
  /* dummy */
  return 0 ;
}

/*-----------------------*/
/* Insert user functions */
/*-----------------------*/
void user_initialize(void)
{
  /* PORT A */
  PORTA = 0b11111111 ; /* 11111111 */
  DDRA  = 0b00000000 ; /* iiiiiiii */ 
  /* PORT B */		
  PORTB = 0b00000000 ; /* 00000000 */
  DDRB  = 0b11111111 ; /* oooooooo */ 
  /* PORT C */		
  PORTC = 0b00000000 ; /* 00000000 */
  DDRC  = 0b11111111 ; /* oooooooo */
  /* PORT D */		
  PORTD = 0b11111100 ; /* 00001100 */
  DDRD  = 0b00000010 ; /* iiiiiioi */
  /* initialize timer1 */
  {
    tickcount = 0 ;
    /* select CTC , (presclae /8) 2MHz */
    TCCR1B = (1 << WGM12) | (1 << CS11) ;
    /* clear timer/counter */
  	TCNT1 = 0 ;
    OCR1A = 1999 ;
    OCR1B = 2100 ;
    /* Enable Compare match interruption */
    TIMSK1 = (1 << OCIE1A) ;
  }
  /* others */
  left   = 0 ;
  right  = 0 ;
  state = IDLE ;
  mstate = 0 ;
  init_spat();
}

/* TIMER1 interrupt */
ISR(TIMER1_COMPA_vect)
{
  /* increment */
  tickcount++;
  /* judge */
  if ( (tickcount & 0xf) == 10 ) {
    wflag = ON ;
  }
}

UBYTE get_hex(UBYTE x)
{
  volatile UBYTE result ;
  /* default */
  result = 0 ;
  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 ;
  /* set counter */
  target = tickcount + (ULONG)x ;
  /* wait */
  while ( target > tickcount ) ;
}

void  send_dc(UBYTE lx,UBYTE rx)
{
  /* left */
  PORTC = lx ;
  /* left trigger */
  PORTB &= ~(1 << LEFT_TRG);
  PORTB |=  (1 << LEFT_TRG);
  /* right */
  PORTC = rx ;
  /* right trigger */
  PORTB &= ~(1 << RIGHT_TRG);
  PORTB |=  (1 << RIGHT_TRG);
  /* send */
  PORTB |=  (1 << TRG);
  PORTB &= ~(1 << TRG);
}

UBYTE get_sensor(void)
{
  return sensor ;
}

UBYTE get_start(void)
{
  return sgate;
}

void  init_spat(void)
{
  *(spat+ 0) = 0x00 ; /* ALL_BLACK   */
  *(spat+ 1) = 0xff ; /* ALL_WHITE   */
  *(spat+ 2) = 0xf0 ; /* LEFT_WHITE  */
  *(spat+ 3) = 0x0f ; /* RIGHT_WHITE */
  *(spat+ 4) = 0x18 ; /* CENTER      */
  *(spat+ 5) = 0x0c ; /* TINY_RIGHT  */
  *(spat+ 6) = 0x06 ; /* RIGHT       */
  *(spat+ 7) = 0x03 ; /* BIG_RIGHT   */
  *(spat+ 8) = 0x30 ; /* TINY_LEFT   */
  *(spat+ 9) = 0x60 ; /* LEFT        */
  *(spat+10) = 0xc0 ; /* BIG_LEFT    */
  *(spat+11) = 0xc3 ; /* BOTH_WHITE  */
  *(spat+12) = 0x5a ; /* ILLEAGAL    */
}

void  show_road_state(UBYTE x)
{
  UBYTE result;
  /* get code */
  result = *(spat+x);
  /* impress */
  PORTC = result ^ MASKFF ;
  /* send trigger */
  PORTB |=  (1 << SRS_TRG);
  PORTB &= ~(1 << SS_TRG);
}

void  show_state(UBYTE x)
{
  UBYTE result;
  /* generate code */
  result = (1 << x);
  /* impress */
  PORTC = result ^ MASKFF ;
  /* send trigger */
  PORTB |=  (1 << SS_TRG);
  PORTB &= ~(1 << SS_TRG);
}

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

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

void rsm_tsk(UBYTE tid)
{
  UWORD tmp ;
  tmp = (1 << tid);
  ready   |=  tmp;
  suspend &= ~tmp;
  waitq   &= ~tmp;
}

void sus_tsk(UBYTE tid)
{
  UWORD tmp ;
  tmp = (1 << tid);
  ready   &= ~tmp;
  suspend |=  tmp;
  waitq   &= ~tmp;
}

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

void wai_tsk(UWORD x)
{
  UWORD tmp ;
  tmp = (1 << run_tsk);
  ready   &= ~tmp;
  suspend &= ~tmp;
  waitq   |=  tmp;
  tcb[run_tsk].wcount = x ;
}

void init_os(void)
{
  ready   = 0;
  suspend = 0;
  waitq   = 0;
}

UBYTE is_tsk_ready(UBYTE tid)
{
  return( (ready >> tid) & 1 ) ;
}

#define TIME_TRG_BIT 4

/* system control */
void tsk0_proc(void)
{
  /* start trigger */
  if ( state == IDLE ) {
    if ( get_start() == OPENED ) {
      state = RUN ;
      rsm_tsk(NORMAL);
      /* timer trigger */
      PORTB |=  (1 << TIME_TRG_BIT);
      PORTB &= ~(1 << TIME_TRG_BIT);
    }
  }
  /* timeout */
  if ( state == RUN ) {
    if ( timeout == 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 ;
    }
  }
  /* show state */
  show_state( mstate );
}

/* NORMAL */
void tsk1_proc(void)
{
  /* machine state */
  mstate = 1 ;
  /* default */
  dir = DIR_CENTER ;
  /* get sensor */
  if ( sensor == CENTER ) {
    left = 50 ; right = 50 ;
  }
  if ( sensor == TINY_RIGHT ) {
    left = 50 ; right = 55 ;
  }
  if ( sensor == RIGHT ) {
    left = 50 ; right = 60 ;
  }
  if ( sensor == BIG_RIGHT ) {
    left = 50 ; right = 70 ;
  }
  if ( sensor == TINY_LEFT ) {
    left = 55 ; right = 50 ;
  }
  if ( sensor == LEFT ) {
    left = 60 ; right = 50 ;
  }
  if ( sensor == BIG_LEFT ) {
    left = 70 ; right = 50 ;
  }
  if ( sensor == ALL_BLACK ) {
    left = 10 ; right = 10 ;
  }
  if ( sensor == BOTH_WHITE ) {
    left = 20 ; right = 20 ;
  }
  if ( sensor == ILLEAGAL ) {
    left = 10 ; right = 10 ;
  }
  /* update */
  send_dc(left,right);
  /* judge */
  if ( sensor == ALL_WHITE ) {
    rsm_tsk(CRANK);
    slp_tsk();
  }
  if ( sensor == LEFT_WHITE ) {
    dir = DIR_LEFT ;
    rsm_tsk(LANE);
    slp_tsk();
  }
  if ( sensor == RIGHT_WHITE ) {
    dir = DIR_RIGHT ;
    rsm_tsk(LANE);
    slp_tsk();
  }
}

/* CRANK */
void tsk2_proc(void)
{
  /* machine state */
  mstate = 2 ;
  /* get sensor */
  if ( sensor == CENTER ) {
    left = 30 ; right = 30 ;
  }
  if ( sensor == TINY_LEFT ) {
    left = 35 ; right = 30 ;
  }
  if ( sensor == LEFT ) {
    left = 40 ; right = 30 ;
  }
  if ( sensor == BIG_LEFT ) {
    left = 45 ; right = 30 ;
  }
  if ( sensor == TINY_RIGHT ) {
    left = 30 ; right = 35 ;
  }
  if ( sensor == RIGHT ) {
    left = 30 ; right = 40 ;
  }
  if ( sensor == BIG_RIGHT ) {
    left = 30 ; right = 45 ;
  }
  if ( sensor == BOTH_WHITE ) {
    left = 20 ; right = 20 ;
  }
  if ( sensor == ALL_BLACK ) {
    left = 10 ; right = 10 ;
  }
  if ( sensor == ILLEAGAL ) {
    left = 10 ; right = 10 ;
  }
  /* update */
  send_dc(left,right);
  /* judge */
  if ( sensor == LEFT_WHITE ) {
    dir = DIR_LEFT ;
    rsm_tsk(ROTATE);
    slp_tsk();
  }
  if ( sensor == RIGHT_WHITE ) {
    dir = DIR_RIGHT ;
    rsm_tsk(ROTATE);
    slp_tsk();
  }
}

/* ROTATE */
void tsk3_proc(void)
{
  /* machine state */
  mstate = 3 ;
  /* judge */
  if ( dir == DIR_RIGHT ) {
    left = 10 ; right = 50 ;
  }
  if ( dir == DIR_LEFT ) {
    left = 50 ; right = 10 ;
  }
  /* update */
  send_dc(left,right);
  /* judge */
  if ( sensor == LEFT_WHITE ) {
    dir = DIR_LEFT ;
    rsm_tsk(ROTATE);
    slp_tsk();
  }
  /* judge */
  if ( sensor == CENTER ) {
    send_dc(50,50);
    dir = DIR_CENTER ;
    rsm_tsk(NORMAL);
    slp_tsk();
  }  
}

/* LANE */
void tsk4_proc(void)
{
  UBYTE tdir ;
  /* machine state */
  mstate = 4 ;
  /* direction */
  tdir = dir ;
  /* get sensor */
  if ( sensor == CENTER ) {
    left = 30 ; right = 30 ;
  }
  if ( sensor == TINY_LEFT ) {
    left = 35 ; right = 30 ;
  }
  if ( sensor == LEFT ) {
    left = 40 ; right = 30 ;
  }
  if ( sensor == BIG_LEFT ) {
    left = 45 ; right = 30 ;
  }
  if ( sensor == TINY_RIGHT ) {
    left = 30 ; right = 35 ;
  }
  if ( sensor == RIGHT ) {
    left = 30 ; right = 40 ;
  }
  if ( sensor == BIG_RIGHT ) {
    left = 30 ; right = 45 ;
  }
  if ( sensor == BOTH_WHITE ) {
    left = 20 ; right = 20 ;
  }
  if ( sensor == ALL_BLACK ) {
    left = 10 ; right = 10 ;
  }
  if ( sensor == ILLEAGAL ) {
    left = 10 ; right = 10 ;
  }
  /* update */
  send_dc(left,right);
  /* judge */
  if ( sensor == ALL_BLACK ) {
    dir <<= 2 ;
    if ( tdir == DIR_LEFT ) {
      dir = DIR_RIGHT ;
    }
    if ( tdir == DIR_RIGHT ) {
      dir = DIR_LEFT ;
    }
    dir <<= 2 ;
    dir |= tdir ;
    rsm_tsk(CHANGE);
    slp_tsk();
  }
}

/* CHANGE */
void tsk5_proc(void)
{
  UBYTE tdir ;
  /* machine state */
  mstate = 5 ;
  /* default */
  tdir = dir & 3 ;
  dir >>= 2 ;
  /* rotate */
  if ( tdir == DIR_LEFT ) {
    left = 30 ; right = 50 ;
  }
  if ( tdir == DIR_RIGHT ) {
    left = 50 ; right = 30 ;
  }
  /* update */
  send_dc(left,right);
  /* delay */
  delay_ms(100);
  /* */
  left = 30 ; right = 30 ;
  /* exit */
  rsm_tsk(BLIND);
  slp_tsk();
}

/* BLIND */
void tsk6_proc(void)
{
  /* machine state */
  mstate = 6 ;
  /* rotate */
  if ( dir == DIR_LEFT ) {
    left = 10 ; right = 30 ;
  }
  if ( dir == DIR_RIGHT ) {
    left = 30 ; right = 10 ;
  }
  /* update */
  send_dc(left,right);
  /* delay */
  delay_ms(100);
  /* */
  left = 50 ; right = 50 ;
  /* update */
  send_dc(left,right);
  /* delay */
  delay_ms(50);
  /* sensor */
  if ( sensor == CENTER ) {
    rsm_tsk(NORMAL);
    slp_tsk();
  }
}

/* sensing */
void tsk7_proc(void)
{
  /* get sensor data */
  sensor = PINA ;
  /* start gate */
  sgate = CLOSED ;
  if ( !(sensor & (1 << SGATE_BIT)) ) { sgate = OPENED ; }
  /* timeout */
  timeout = OFF ;
  if ( sensor & (1 << TOUT_BIT) ) { timeout = ON ; }
  /* sensor */
  sensor &= MASK0F ;
  /* show road state */
  show_road_state( sensor );
  /* wait 50ms */
  wai_tsk(50);
}

 ATmega644は、FlashROMが64kバイトですが、そのうちの
 4kバイト弱にRTOSとランダムロジックの関数が入りました。

 使ったATmega644基板は、以下。



 シリアルインタフェース処理、デバッグ用タスクを入れても
 10タスクなので、マルチプロセッサ構成の制御基板ならば
 ATmega164でも充分だと考えられます。

 H8/3052の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 x_flags ;

ULONG timcnt ;

#define SFLAG x_flags.BIT.B0
#define UFLAG x_flags.BIT.B1
#define WFLAG x_flags.BIT.B2
#define TFLAG x_flags.BIT.B3

#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 OFF      0
#define ON       OFF+1

#define CLOSED 0
#define OPENED CLOSED+1

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

UBYTE sensor ;
UBYTE sgate ;
UBYTE timeoutcnt ;

UBYTE dir ;
UBYTE left  ;
UBYTE right ;

UBYTE state  ;
UBYTE mstate ;

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

UBYTE run_tsk ;
UBYTE ready   ;
UBYTE suspend ;
UBYTE 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  user_initialize(void);
void  init_timer1(void);
UBYTE get_hex(UBYTE x);
void  delay_ms(UWORD x);
void  binary_display(UBYTE x);
void  init_spat(void);
UWORD conv_value(UBYTE x);

/*-------------*/
/* system call */
/*-------------*/
void  cre_tsk(UBYTE tid,void (*tsk)(void));
void  sta_tsk(UBYTE tid,UBYTE sta);
void  rsm_tsk(UBYTE tid);
void  sus_tsk(UBYTE tid);
void  slp_tsk(void);
void  wai_tsk(UWORD x);
void  init_os(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 ;
  UBYTE tmp ;
  UBYTE i ;
  /* disable interrupt */
  DI ;
  /* initialize */
  user_initialize();
  /* 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 ; }
    /* cyclic handling */
    if ( WFLAG == ON ) {
      WFLAG = OFF ;
      for ( i = 0 ; i < TSK_ID_MAX ; i++ ) {
        if ( waitq & *(bpat+i) ) {
          tcb[i].wcount-- ;
          if ( tcb[i].wcount == 0 ) { rsm_tsk(i); }
        }
      }
    }
  }
  return 0 ;
}

/*-----------------------*/
/* Insert user functions */
/*-----------------------*/
void user_initialize(void)
{
  /* PORT 4 */
  P4DR  = 0x00 ;
  P4DDR = MASKFF ; /* all outputs */
  /* 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 */
  x_flags.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 ;
  /* SCI */
  init_sci_1(br9600);
  /* others */
  left   = 0 ;
  right  = 0 ;
  state = IDLE ;
  mstate = 0 ;
  init_spat();
  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("W set data")                    ; rs1_crlf();
  rs1_puts(" column(15) line(64) data(16)") ; rs1_crlf();
  rs1_puts("F show frame buffer context")   ; rs1_crlf();
  rs1_puts("C clear frame buffer context")  ; rs1_crlf();
  rs1_puts("L display on LCD")              ; rs1_crlf();
  rs1_puts("G GBC handling")                ; rs1_crlf();
  rs1_puts("  G0 initialize  GBC")          ; rs1_crlf();
  rs1_puts("  G1 take picture")             ; rs1_crlf();
  rs1_puts("  G2 convert and show on LCD")  ; rs1_crlf();
  rs1_puts("  G3 clear gbc buffer")         ; rs1_crlf();
  rs1_puts("  G4 show gbc buffer")          ; rs1_crlf();
  rs1_puts("B set line block number")       ; rs1_crlf();
  rs1_puts("b show line block number")      ; rs1_crlf();
  rs1_puts("S set servo motor count")       ; rs1_crlf();
  rs1_puts("s show servo motor count")      ; rs1_crlf();
  rs1_puts("D set DC motor duty ratio")     ; rs1_crlf();
  rs1_puts("d show DC motor duty ratio")    ; rs1_crlf();
  rs1_puts("E enable or disable motor")     ; rs1_crlf();
  rs1_puts("e show enable or disable")      ; rs1_crlf();
*/
}

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)
{
  return sensor ;
}

UBYTE get_start(void)
{
  return sgate;
}

void  init_spat(void)
{
  *(spat+ 0) = 0x00 ; /* ALL_BLACK   */
  *(spat+ 1) = 0xff ; /* ALL_WHITE   */
  *(spat+ 2) = 0xf0 ; /* LEFT_WHITE  */
  *(spat+ 3) = 0x0f ; /* RIGHT_WHITE */
  *(spat+ 4) = 0x18 ; /* CENTER      */
  *(spat+ 5) = 0x0c ; /* TINY_RIGHT  */
  *(spat+ 6) = 0x06 ; /* RIGHT       */
  *(spat+ 7) = 0x03 ; /* BIG_RIGHT   */
  *(spat+ 8) = 0x30 ; /* TINY_LEFT   */
  *(spat+ 9) = 0x60 ; /* LEFT        */
  *(spat+10) = 0xc0 ; /* BIG_LEFT    */
  *(spat+11) = 0xc3 ; /* BOTH_WHITE  */
  *(spat+12) = 0x5a ; /* ILLEAGAL    */
}

void  show_road_state(UBYTE x)
{
  UBYTE result;
  /* get code */
  result = *(spat+x);
  /* impress */
  PADR = result ^ MASKFF ;
}

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

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

void sta_tsk(UBYTE tid,UBYTE sta)
{
  UWORD tmp ;
  /* get bit pattern */
  tmp  = *(bpat+tid);
  /* set bit */
  if ( sta == TTS_READY   ) { ready   |= tmp; }
  if ( sta == TTS_SUSPEND ) { suspend |= tmp; }
  if ( sta == TTS_WAIT    ) { waitq   |= tmp; }
}

void rsm_tsk(UBYTE tid)
{
  UWORD tmp ;
  UWORD tmpx ;
  /* get bit pattern */
  tmp  = *(bpat+tid);
  tmpx = tmp ^ 0xffff;
  /* 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 ^ 0xffff;
  /* 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 ^ 0xffff;
  /* 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 task */
  sta_tsk(TSK_ID0,TTS_READY);
  sta_tsk(NORMAL ,TTS_SUSPEND);
  sta_tsk(CRANK  ,TTS_SUSPEND);
  sta_tsk(ROTATE ,TTS_SUSPEND);
  sta_tsk(LANE   ,TTS_SUSPEND);
  sta_tsk(CHANGE ,TTS_SUSPEND);
  sta_tsk(BLIND  ,TTS_SUSPEND);
  sta_tsk(TSK_ID7,TTS_READY);
  sta_tsk(XDEBUG ,TTS_READY);
}

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

/* system control */
void tsk0_proc(void)
{
  /* start trigger */
  if ( state == IDLE ) {
    if ( get_start() == OPENED ) {
      state = RUN ;
      rsm_tsk(NORMAL);
      /* enable motor power */
      PBDR |= (1 << EDC_BIT);
      /* 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 */
      PBDR &= ~(1 << EDC_BIT);
    }
  }
  /* show state */
  show_state( mstate );
}

/* NORMAL */
void tsk1_proc(void)
{
  UBYTE xmul ;
  /* machine state */
  mstate = 0 ;
  /* show */
  show_state((1 << mstate));
  /* default */
  dir = DIR_CENTER ;
  /* get sensor */
  if ( sensor == CENTER ) {
    left = 50 ; right = 50 ;
    /* climb hill or down hill */
    if ( sloap ) {
      xmul = *(sloapt+sloap) ;
      left  = (UBYTE)( xmul * left * 1.0 / 100 );
      right = (UBYTE)( xmul * right * 1.0 / 100 );
    }
  }
  if ( sensor == TINY_RIGHT ) {
    left = 50 ; right = 55 ;
  }
  if ( sensor == RIGHT ) {
    left = 50 ; right = 60 ;
  }
  if ( sensor == BIG_RIGHT ) {
    left = 50 ; right = 70 ;
  }
  if ( sensor == TINY_LEFT ) {
    left = 55 ; right = 50 ;
  }
  if ( sensor == LEFT ) {
    left = 60 ; right = 50 ;
  }
  if ( sensor == BIG_LEFT ) {
    left = 70 ; right = 50 ;
  }
  if ( sensor == ALL_BLACK ) {
    left = 10 ; right = 10 ;
  }
  if ( sensor == BOTH_WHITE ) {
    left = 20 ; right = 20 ;
  }
  if ( sensor == ILLEAGAL ) {
    left = 10 ; right = 10 ;
  }
  /* update */
  send_dc(left,right);
  /* judge */
  if ( sensor == ALL_WHITE ) {
    rsm_tsk(CRANK);
    slp_tsk();
  }
  if ( sensor == LEFT_WHITE ) {
    dir = DIR_LEFT ;
    rsm_tsk(LANE);
    slp_tsk();
  }
  if ( sensor == RIGHT_WHITE ) {
    dir = DIR_RIGHT ;
    rsm_tsk(LANE);
    slp_tsk();
  }
}

/* CRANK */
void tsk2_proc(void)
{
  /* machine state */
  mstate = 1 ;
  /* show */
  show_state((1 << mstate));
  /* get sensor */
  if ( sensor == CENTER ) {
    left = 30 ; right = 30 ;
  }
  if ( sensor == TINY_LEFT ) {
    left = 35 ; right = 30 ;
  }
  if ( sensor == LEFT ) {
    left = 40 ; right = 30 ;
  }
  if ( sensor == BIG_LEFT ) {
    left = 45 ; right = 30 ;
  }
  if ( sensor == TINY_RIGHT ) {
    left = 30 ; right = 35 ;
  }
  if ( sensor == RIGHT ) {
    left = 30 ; right = 40 ;
  }
  if ( sensor == BIG_RIGHT ) {
    left = 30 ; right = 45 ;
  }
  if ( sensor == BOTH_WHITE ) {
    left = 20 ; right = 20 ;
  }
  if ( sensor == ALL_BLACK ) {
    left = 10 ; right = 10 ;
  }
  if ( sensor == ILLEAGAL ) {
    left = 10 ; right = 10 ;
  }
  /* update */
  send_dc(left,right);
  /* judge */
  if ( sensor == LEFT_WHITE ) {
    dir = DIR_LEFT ;
    rsm_tsk(ROTATE);
    slp_tsk();
  }
  if ( sensor == RIGHT_WHITE ) {
    dir = DIR_RIGHT ;
    rsm_tsk(ROTATE);
    slp_tsk();
  }
}

/* ROTATE */
void tsk3_proc(void)
{
  /* machine state */
  mstate = 2 ;
  /* show */
  show_state((1 << mstate));
  /* judge */
  if ( dir == DIR_RIGHT ) {
    left = 10 ; right = 50 ;
  }
  if ( dir == DIR_LEFT ) {
    left = 50 ; right = 10 ;
  }
  /* update */
  send_dc(left,right);
  /* judge */
  if ( sensor == LEFT_WHITE ) {
    dir = DIR_LEFT ;
    rsm_tsk(ROTATE);
    slp_tsk();
  }
  /* judge */
  if ( sensor == CENTER ) {
    send_dc(50,50);
    dir = DIR_CENTER ;
    rsm_tsk(NORMAL);
    slp_tsk();
  }  
}

/* LANE */
void tsk4_proc(void)
{
  UBYTE tdir ;
  /* machine state */
  mstate = 3 ;
  /* show */
  show_state((1 << mstate));
  /* direction */
  tdir = dir ;
  /* get sensor */
  if ( sensor == CENTER ) {
    left = 30 ; right = 30 ;
  }
  if ( sensor == TINY_LEFT ) {
    left = 35 ; right = 30 ;
  }
  if ( sensor == LEFT ) {
    left = 40 ; right = 30 ;
  }
  if ( sensor == BIG_LEFT ) {
    left = 45 ; right = 30 ;
  }
  if ( sensor == TINY_RIGHT ) {
    left = 30 ; right = 35 ;
  }
  if ( sensor == RIGHT ) {
    left = 30 ; right = 40 ;
  }
  if ( sensor == BIG_RIGHT ) {
    left = 30 ; right = 45 ;
  }
  if ( sensor == BOTH_WHITE ) {
    left = 20 ; right = 20 ;
  }
  if ( sensor == ALL_BLACK ) {
    left = 10 ; right = 10 ;
  }
  if ( sensor == ILLEAGAL ) {
    left = 10 ; right = 10 ;
  }
  /* update */
  send_dc(left,right);
  /* judge */
  if ( sensor == ALL_BLACK ) {
    dir <<= 2 ;
    if ( tdir == DIR_LEFT ) {
      dir = DIR_RIGHT ;
    }
    if ( tdir == DIR_RIGHT ) {
      dir = DIR_LEFT ;
    }
    dir <<= 2 ;
    dir |= tdir ;
    rsm_tsk(CHANGE);
    slp_tsk();
  }
}

/* CHANGE */
void tsk5_proc(void)
{
  UBYTE tdir ;
  /* machine state */
  mstate = 4 ;
  /* show */
  show_state((1 << mstate));
  /* default */
  tdir = dir & 3 ;
  dir >>= 2 ;
  /* rotate */
  if ( tdir == DIR_LEFT ) {
    left = 30 ; right = 50 ;
  }
  if ( tdir == DIR_RIGHT ) {
    left = 50 ; right = 30 ;
  }
  /* update */
  send_dc(left,right);
  /* delay */
  delay_ms(100);
  /* */
  left = 30 ; right = 30 ;
  /* exit */
  rsm_tsk(BLIND);
  slp_tsk();
}

/* BLIND */
void tsk6_proc(void)
{
  /* machine state */
  mstate = 5 ;
  /* show */
  show_state((1 << mstate));
  /* rotate */
  if ( dir == DIR_LEFT ) {
    left = 10 ; right = 30 ;
  }
  if ( dir == DIR_RIGHT ) {
    left = 30 ; right = 10 ;
  }
  /* update */
  send_dc(left,right);
  /* delay */
  delay_ms(100);
  /* */
  left = 50 ; right = 50 ;
  /* update */
  send_dc(left,right);
  /* delay */
  delay_ms(50);
  /* sensor */
  if ( sensor == CENTER ) {
    rsm_tsk(NORMAL);
    slp_tsk();
  }
}

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

/* 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' ) {
      PBDR |= (1 << EDC_BIT) ;
    }
    /* disable DCDC conveter */
    if ( cmd == 'e' ) {
      PBDR &= ~(1 << EDC_BIT) ;
    }
  }
}


目次

inserted by FC2 system