// ファイル名　LineTrace.c
// ライントレースをする
// 明るさ設定は電源投入時直後に白レベルを読み取る。
#include <htc.h>
__CONFIG(PWRTEN&HS&WDTDIS&UNPROTECT&MCLRDIS&BORDIS&IESODIS&FCMDIS);

#define _XTAL_FREQ 20000000
#define PERIOD 100
#define false 0
#define true  1
#define AN4   0x91
#define AN5   0x95
#define AN6   0x99
#define AN7   0x9d
#define AN10  0xa9
#define AN11  0xad
#define ABS(x) ((x>=0) ? (x) : -(x))

typedef unsigned char byte;

void LED(byte n, byte times);
void Delay50();
void Delay100();
void initialize();
void setIRThreshold(byte th);
byte checkIR();
void LineTrace();

// グローバル変数の宣言
byte counter=0;
byte LeftCdS, CenterCdS, RightCdS,ExRCdS;
byte threshold[4];
byte ADch=0;
byte ConvEndFlag;
byte ADC[4]={AN4,AN5,AN6,AN11};
byte Leftspeed=208;
byte Centerspeed=208;
byte Rightspeed=208;

// 割り込み処理ルーチン
void interrupt Intr(void)
{
  // 約2ms毎に割り込む
  if(T0IF) {       // 割り込みがtimer0からか？
    counter++;
    TMR0 = PERIOD;
    T0IF = 0;                 // 割り込みフラグクリア
    return;
  }
  if(ADIF) {      // 割り込みがADコンバーターからか？
    if(ADch==0) LeftCdS = ADRESL;
    else if(ADch==1) CenterCdS = ADRESL;
    else if(ADch==2) RightCdS = ADRESL;
    else if(ADch==3) ExRCdS = ADRESL;
    ConvEndFlag=true;         // データの更新あり
    ADIF = 0;                 // 割り込みフラグクリア
  }
}

#define SLOW  30
#define  LOW  40
#define MIDDLE 80
#define MIDDLE1 100
#define MIDDLE2 140
#define HIGH  160

main()
{
  int i;
  byte state=0;

  initialize();
  // 値を大きくすると感度が高くなる。
  while(1){
    setIRThreshold(13);
    LED(2, 5);
    state=2;

  // Timer0 の設定
    T0IE=1;                     // Timer0 の割り込みON
    T0IF=0;                     // 割り込みフラグクリア

  // ADコンバーターの設定
    ADch = 3;
    ConvEndFlag=true;
    ADIE = 1;                   // ADコンバータの割り込み許可
    ADIF = 0;                   // 割り込みフラグクリア
    PEIE = 1;                   // 周辺装置の割り込み許可
    GIE  = 1;                   // CPUへの割り込み許可
    
    LineTrace();
  }  
}

void LineTrace()
{
  while(RA3)
  {
    switch(checkIR()){     
      case 15:  // センサーがライン上にある。
              Rightspeed=Leftspeed=HIGH;  // 直進
              RC5=0; RC7=0;
              break;
      case 14:  // 右に一つずれた
              Rightspeed = MIDDLE2; Leftspeed = MIDDLE1;//左に行く 
              RC5=0; RC7=0;
              break; 
      case 7:   // 左に一つずれた
              Rightspeed = MIDDLE1; Leftspeed = MIDDLE2;// 右に行く
              RC5=0; RC7=0;
              break;

      case 12:  // 右に2つずれた
              Rightspeed = MIDDLE2; Leftspeed = LOW;    //左に行く 
              RC5=0; RC7=0;
              break;          
      case 3:   // 左に2つずれた
              Rightspeed = LOW;  Leftspeed = MIDDLE2;   // 右に行く
              RC5=0; RC7=0;
              break;

      case 1:   // 右に3つずれた
              Rightspeed = SLOW;  Leftspeed = MIDDLE2;  // 右に行く
              RC5=0; RC7=0;
              break;
      case 8:   // 左に3つずれた
              Rightspeed = MIDDLE2; Leftspeed = SLOW;   //左に行く 
              RC5=0; RC7=0;
              break; 
    } // switch
    RC4 = counter<Rightspeed;
    RC6 = counter<Leftspeed;

  }  // while
}

void initialize()
{
  PORTA = 0;                  // PORTAを0にする
  TRISA = 0;                  // PORTAを出力に設定する
  TRISB = 0x30;               // 4,5input
  PORTB = 0;
  PORTC = 0;                  // PORTCを0にする
  TRISC = 0x0f;               // PORTC下4bit入力、上4bit出力
  ANSEL = 0xF0;               // AN4,5,6,7 ON
  ANSELH = 0x0C;              // AN10,11 ON
  ADCON0 = AN4;               // ADFM=1(右詰)  AN4 adOn
                              // 1 = Right justified
  OPTION = 0;
                              // 1:8 prescale
  T1CON = 0x31;               // Timer1 settings
  TMR1IF = 0;                 // clear TMR1IF
  TMR1H = 0xf8;               // Initialize Timer1 register
  TMR1L = 0x00;

  ADC[0]=AN4;
  ADC[1]=AN5;
  ADC[2]=AN6;
  ADC[3]=AN11;
}

void LED(byte n, byte times)
{
  byte i;
  for(i=0; i<times; i++) {
    PORTA =  n;
    Delay50();                // 50msディレイ
    PORTA =  0;
    Delay50();                // 50msディレイ
  }
}

void Delay50()                // 50m秒ディレイ
{
  int i;
  for(i=0; i<5; i++)
    __delay_ms(10);
}

void Delay100()               // 100m秒ディレイ
{
  int i;
  for(i=0; i<10; i++)
    __delay_ms(10);
}

void setIRThreshold(byte th)
{
  byte i, j, x;
  int average[4]={0,0,0,0};
  GIE=0;
 // 明るいときの値 明るいと大きい値
  ADC[0]=AN4;
  ADC[1]=AN5;
  ADC[2]=AN6;
  ADC[3]=AN11;
  for(j=0; j<40; j++) {
    for(i=0; i<4; i++) {
      ADCON0 = ADC[i];
      GODONE = 1;
      while(GODONE);
      x = ADRESL;
      average[i] = (average[i]*9+x)/10;
    }
  }
  for(i=0; i<4; i++)
    threshold[i]=average[i]*th/20;
  GIE=1;
}

byte checkIR()
{
  byte status;

  if(ConvEndFlag) { // AD変換が終わったらすぐに次のチャンネルの変換を行う
    ConvEndFlag = false;
    if(++ADch==4) ADch=0;
    ADCON0 = ADC[ADch];  // 変換スタート
    GODONE = 1;
  }
  PORTA=0;
  status = 0;
  RA2 = ExRCdS<threshold[3];
  RA1 = RightCdS<threshold[2];
  RA0 = CenterCdS<threshold[1];

  status = RA2;
  status += RA1 ? 2 : 0;
  status += RA0 ? 4 : 0;
  status += (LeftCdS<threshold[0]) ? 8 : 0;
  return status;
}