/**********************************************************************************
 *  Program Name:キャリーロボットのプログラム
 *  Author      :野寄修平
 *  Date        : 2006-12-15 13:11:20
 *  explanation :
 *               
 *				 JAPANROBOTECH C-Language Development Environment
 **********************************************************************************/

#include "JRT102.h"      // Hardware definition
#include "RoboDesigner.h"// RoboDesigner Hardware definition
#include "TiCollaLib.h"  // Library definition
#include "vectors.h"     // Interruption vector definition
#include "initialize.h"  // Ininitalize
#include "interrupt.h"   // Interruption


/*--------------------------------------------------------------------------------*/
/* Variable declaration                                                           */        
/*--------------------------------------------------------------------------------*/

#define		threshold	45					
		byte ir1;
		byte ir2;

/*--------------------------------------------------------------------------------*/
/* Prototype declaration                                                          */        
/*--------------------------------------------------------------------------------*/

void Forward( void );
void Backward( void );
void turn_R( void );
void turn_L( void );
void Mot_Stop( void );

/*--------------------------------------------------------------------------------*/
/*  main                                                                          */
/*--------------------------------------------------------------------------------*/

void main(void) {
	
	init();		//Initialization
	
	MOTOR_SPD(PWM_SIDE_CN11, MOTOR_STEP_MIN);

	while(1){

	AD_GETDATA(AD_CN3, &ir1);
	AD_GETDATA(AD_CN4, &ir2);	

if(CN1==1){                						//茶托の上に物がある
		if(ir1<threshold){
			       if(ir2<threshold){　　　　　　　　　　	//センサーが２つとも白でない時
				Forward();  　　　　　　　　　　　　　 	//前進                          
			}else{　　　　　　　　　　　　　　　　　　　　	//センサー２だけ白の時
				turn_R();　　　　　　　　　　　　　　　	//右超信地旋回　　　　　　　　　　　　　　　				
			　　　}				
			}else{
			if(ir2<threshold){                             	//センサー１だけ白の時                          
				turn_L();                              	//左超信地旋回
			}else{                                        	//センサーが２つとも白の時
				turn_R();                              	//右超信地旋回　　
			　　　}			
		　　　　　　　}		
			}else{						//茶托の上に物がない
				Mot_Stop();
				DELAY_SEC(500);				//停止５秒
	
                        　　　}
                
		
		COPCTL = 0;	
	}
}

/*--------------------------------------------------------------------------------*/
/*  function                                                                      */
/*--------------------------------------------------------------------------------*/

void Forward( void ){			//前進
	M1_IN1 = 1;
	M1_IN2 = 0;
	M2_IN1 = 1;
	M2_IN2 = 0;
}

void Backward( void ){			//後退
	M1_IN1 = 0;
	M1_IN2 = 1;
	M2_IN1 = 0;
	M2_IN2 = 1;
}

void turn_R( void ){			//右超信地旋回
	M1_IN1 = 1;
	M1_IN2 = 0;
	M2_IN1 = 0;
	M2_IN2 = 1;
}

void turn_L( void ){			//左超信地旋回
	M1_IN1 = 0;
	M1_IN2 = 1;
	M2_IN1 = 1;
	M2_IN2 = 0;
}

void Mot_Stop( void ){			//停止
	M1_IN1 = 0;
	M1_IN2 = 0;
	M2_IN1 = 0;
	M2_IN2 = 0;
}