/*******************************************************
*	マイクロロボコン2012年モデルテスト用プログラム		日本工業大学
*	pic16f676
*
*	Motor1	Vref	Rin		Fin
*			RC5		RC4		RC3
*	
*	Motor2	Vref	Rin		Fin
*			RC0		RC1		RC2
*	
*	センサLED出力(0出力→発光)
*	sensor1	sensor2	sensor3	sensor4
*	  RA1	  RA2	  RA4	  RA5
*	
*	センサからの入力信号（0:黒・暗い、1:白・明るい）
*	RA3
*
*	AD変換可能チャンネル
*	AN0
*
*	モーター制御について　DModeという変数を使用します。DModeの値によって左右のモータの制御をします。
*	DMode = 1 正転（前進）
*			2 右曲がり
*			3 左曲がり
*			4 空転
*			
*******************************************************/

#include <pic.h>
#define _XTAL_FREQ 4000000

__CONFIG(FOSC_INTRCIO & WDTE_OFF & PWRTE_ON & MCLRE_OFF & BOREN_OFF & CP_OFF & CPD_OFF) ;

unsigned char cnt0=0;
unsigned char PWM1 = 10;
unsigned char PWM2 = 10;
unsigned char DMode = 1;
int adconv();

#define cnt0max 16


//割り込み処理
static void interrupt isr(void){
	if(T0IF==1){
		TMR0 = 255;
		cnt0++;
		cnt0 &= (cnt0max - 1);
		
		//反射センサのLEDを順次ON（0でON、1でOFF）
		if(cnt0 <= 3){//Sensor4 ON
			RA5 = 0;
			RA1 = 1;
			RA2 = 1;
			RA4 = 1;
		}
		else if(cnt0>4 && cnt0 <=6){//Sensor3 ON
			RA5 = 1;
			RA1 = 1;
			RA2 = 1;
			RA4 = 0;
		}
		else if(cnt0>7 && cnt0 <= 11){//Sensor2  ON
			RA5 = 1;
			RA1 = 1;
			RA2 = 0;
			RA4 = 1;
		}
		else{//Sensor1 ON
			RA5 = 1;
			RA1 = 0;
			RA2 = 1;
			RA4 = 1;
		}
		
		//RC0,RC5：PWM出力
		if(cnt0>=PWM1 && cnt0 >= PWM2){
			RC0=1;
			RC5=1;
		}
		else if(cnt0 >= PWM1 && cnt0 < PWM2){
			RC0 = 1;
			RC5 = 0;
		}
		else if(cnt0 >= PWM2 && cnt0 < PWM1){
			RC0 = 0;
			RC5 = 1;
		}
		else{
			RC0 = 0;
			RC5 = 0;
		}
	}
	
	T0IF = 0;
	RAIF = 0;
	
}

void main(void)
//メイン関数
{
//変数宣言
	int ADVcc=0;						//AD変換用変数
//前処理
	OSCCAL 		=	0b10000000;			//４MHｚの中央値？使う
	CMCON		= 	0b00000111;			//コンパレータOFF
	TRISA  		=	0b00001001;			//RA0,RA3入力	
	TRISC  		=	0b00000000;			//PORTC全て出力
	PORTA		=	0b00000000;
	PORTC	 	=	0b00000000;
	OPTION_REG 	=	0b11010110;			
	ANSEL		=	0b00000001;			
	GIE 		= 	1;					//割り込み許可
	T0IE 		=	1;					//タイマ割込み許可
	
	while(1){
		switch (DMode){
			case 1: //Motor1：Forward,	Motor2：Forward
					RC1=0;
					RC2=1;
					RC3=1;
					RC4=0;
					break;
			case 2: //Motor1：Brake ,	Motor2：Forward
					RC1=0;
					RC2=1;
					RC3=1;
					RC4=1;
					break;
			case 3: //Motor1：Forward ,	Motor2：Brake
					RC1=1;
					RC2=1;
					RC3=1;
					RC4=0;
					break;
			case 4: //Motor1：Free ,	Motor2：Free
					RC1=0;
					RC2=0;
					RC3=0;
					RC4=0;
					break;
			default:
					RC1=0;
					RC2=0;
					RC3=0;
					RC4=0;
					break;
		}
		if(RA3==1){//白を検出したとき
			if(RA2==0 && RA1==1 && RA5==1 && RA4==1){//中左センサ
				DMode = 1;
			}
			else if(RA2==1 && RA1==0 && RA5==1 && RA4==1){
				if(DMode == 3){
					DMode = 1;
				}else{
				DMode = 3;	
				}
			}
			else if(RA2==1 && RA1==1 && RA5==0 && RA4==1){
				if(DMode == 2){
					DMode = 1;
				}
				else{
					DMode = 2;
				}
			}
			else if(RA2==1 && RA1==1 && RA5==1 && RA4==0){
				DMode = 4;
			}
			else{
				DMode = 0;
			}
		}
	}		
}
//AD変換用関数
int adconv()
{
	int ADdata;
	ADCON0 = 0b00000000;			//左詰め,基準電圧Vdd
	ADCON1 = 0b01010000;
	ADON = 1;						//蓄電スタート
	__delay_us(5);					//PWM用にタイマー割り込みを使っているため不正確
	GO = 1;
	while(1){
		if(GO == 0){
			break;
		}
	}
	ADdata = ADRESH;				//AD変換結果上位8bit取得
	return ADdata;
}