// ファイル名　CdSPWM.c
// CdSに手をかざすとそれに対応してモーターが動く
// 左前のCdSだけに手をかざすと押される感じに車体が右に旋回する(赤のLED点灯)
// 右前のCdSだけに手をかざすと押される感じに車体が左に旋回する(緑のLED点灯)
// 後ろのCdS場合はバックする(青のLED点灯)
// 全部かざすと前進する。(白のLED点灯)
// これをタイマー0割り込みを用いて実現する
// 関数にして機能分担化させる
// 自動閾値設定(初期値95%)を電源投入直後に行うのでCdSの上には障害物
// (手など)はあってはいけない。

#include <htc.h>
__CONFIG(PWRTEN&HS&WDTDIS&UNPROTECT&MCLRDIS&BORDIS&IESODIS&FCMDIS);

// マクロ宣言
#define true  1
#define false 0
#define _XTAL_FREQ 20000000
#define ANS4 0x10
#define ANS5 0x20
#define ANS6 0x40
#define AN4  0x11
#define AN5  0x15
#define AN6  0x19
#define PERIOD 100

/*********************************************/
/* モーターをPWMで動かす時に利用するグローバル変数の
　　　counter変数のupdate間隔の計算

20MHz/4 = 5MHz
TMR0 * prescale*counter/5MHZ
= (256-PERIOD) * 2*256/5MHZ 
= (256-100) * 2*256/5MHZ = 16m秒
*/
//***************************
#define SLOW     35
#define LOW      40
#define MIDDLE   80
#define MIDDLE2 120
#define HIGH    160
//**************************
typedef unsigned char byte;

// プロトタイプ宣言
void initialize();
byte checkCdS();
void setThreshold(byte th);
void overHand();
//******************

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

//************************
byte Leftspeed=208;
byte Centerspeed=208;
byte Rightspeed=208;
//************************
// 割り込み処理ルーチン
void interrupt Intr(void)
{
  //  (256-PERIOD)*256/5MHz
  // =(256-100)*2/5MHz = 62.4μs
  // 62.4μs毎に割り込む
  if(T0IF) {       // 割り込みがtimer0からか？
    counter++;
    TMR0 = PERIOD;
    T0IF = 0;                 // 割り込みフラグクリア
    return;
  }
  if(ADIF) {      // 割り込みがADコンバーターからか？
    if(ADch==0) LeftCdS = ADRESH;
    else if(ADch==1) CenterCdS = ADRESH;
    else if(ADch==2) RightCdS = ADRESH;
    ConvEndFlag=true;         // データの更新あり
    ADIF = 0;                 // 割り込みフラグクリア
  }
}
  
main()
{
  initialize();               // 各種初期設定
  // 永久ループ
  while(true) {  
    overHand();               // CdSでモーターを動かす
     //checkCdS();
  }  
}

void initialize()
{
  PORTA = 0;                  // PORTAを0にする
  TRISA = 0;                  // PORTAを出力に設定する
  ANSEL = ANS4 | ANS5 | ANS6; // PORTAアナログ入力をAN4-6をON
  ADCON0 = AN4;               // ADFM=0(左詰) ADon
  OPTION = 0;                 // FOSC/4　プリスケーラ1/2
                              // 20MHz/4/2 = 2.5MHz
  PORTC = 0;                  // PORTCを0にする
  TRISC = 0x0f;               // PORTC下4bit入力、上4bit出力
  ANSELH = 0;                 // PORTCアナログ入力をOFF
  
  // Timer0 の設定
  T0IE=1;                     // Timer0 の割り込みON
  T0IF=0;                     // 割り込みフラグクリア
 
  setThreshold(65);
  
  // ADコンバーターの設定
  ADIE = 1;                   // ADコンバータの割り込み許可
  ADIF = 0;                   // 割り込みフラグクリア
  PEIE = 1;                   // 周辺装置の割り込み許可
  GIE  = 1;                      // CPUへの割り込み許可
  GODONE = 1;                 // AD変換スタート
}

byte checkCdS()
{
  byte status;

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

void setThreshold(byte th)    // 平均の指定%を閾値にする
{
  byte i, j, x, g;
  int average[3]={0,0,0};
  g = GIE;                    // 割り込み状態の保存
  GIE=0;                      // 割り込みは使わない
 // 明るいときの値 明るいと大きい値
  ADC[0]=AN4;
  ADC[1]=AN5;
  ADC[2]=AN6;
  for(j=0; j<40; j++) {       // 平均の明るさを求める
    for(i=0; i<3; i++) {
      ADCON0 = ADC[i];
      GODONE = 1;
      while(GODONE);
      x = ADRESH;
      average[i] = (average[i]*9+x)/10;
    }
  }
  for(i=0; i<3; i++)
    threshold[i]=average[i]*th/100;
  GIE = g;                    // CPUへの割り込み状態を戻す
}

void overHand()
{
  while(RA3) {
    switch(checkCdS()) {
    case 7: Rightspeed=Leftspeed=MIDDLE;    // 直進
            RC5 = RC7 = 0;
            break;
    case 6: Rightspeed = LOW;  Leftspeed = MIDDLE;
            RC5 = 0; RC7 = 0;
            break;

    case 4: Rightspeed = ~MIDDLE;  Leftspeed = MIDDLE;
            RC5 = 1; RC7 = 0;
            break;
    case 3: Rightspeed = MIDDLE; Leftspeed = LOW;
            RC5 = 0; RC7 = 0;
            break;
    case 1: Rightspeed = MIDDLE; Leftspeed = ~MIDDLE;
            RC5 = 0; RC7 = 1;
            break;
    case 2:
            Rightspeed=Leftspeed=~MIDDLE;
            RC5 = RC7 = 1;
            break;
    default:
    case 0: Rightspeed=Leftspeed=255;
            RC5 = RC7 = 1;
            break;

    }
    RC4 = counter<Rightspeed;
    RC6 = counter<Leftspeed;
  }
}