/**********************************************************************************
 *  Program Name:
 *  Author      :
 *  Date        : 2006-08-20 13:16:01
 *  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                                                           */        
/*--------------------------------------------------------------------------------*/



/*--------------------------------------------------------------------------------*/
/* Prototype declaration                                                          */        
/*--------------------------------------------------------------------------------*/
extern void Forward();
extern void turnRight();
extern void turnLeft();
extern void Bk_turnRight();
extern void Bk_turnLeft();
extern void Pt_Right();
extern void Pt_Left();
extern void Back();
extern void Mot_stop();
/*--------------------------------------------------------------------------------*/
/*  main                                                                          */
/*--------------------------------------------------------------------------------*/

void main(void) {
	
	init();						//Initialization
	
		MOTOR_SPD(PWM_SIDE_CN11, MOTOR_STEP_MAX);	//　スピード設定
		M3 = 1;

	while(1){
		if ( CN3 == 1 ){			//　ＣＮ１がＯＮなら
			M3 = 1;	 Forward();			//　Ｍ３　＝　ＯＮ
		}else{					//　ＣＮ１がＯＦＦなら
							//　Ｍ３　＝　ＯＦＦ
			if ( CN1 == 0 ){
				if ( CN2 == 0 ){
					Mot_stop();
				}else{
					Back();}
			}else{
				if ( CN2 == 0 ){
					Back();
				}else{
				}
			}
			M3 = 0;
		}
		COPCTL = 0;	
	}
}

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



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

// 右旋回（右折）
void turnRight( void ){
	M1_IN1 = 1;
	M1_IN2 = 0;
	M2_IN1 = 0;
	M2_IN2 = 0;
}

// 左旋回（左折）
void turnLeft( void ){
	M1_IN1 = 0;
	M1_IN2 = 1;
	M2_IN1 = 0;
	M2_IN2 = 0;
}

// 右後方旋回（Back turn）
void Bk_turnRight( void ){
	M1_IN1 = 0;
	M1_IN2 = 0;
	M2_IN1 = 0;
	M2_IN2 = 1;
}

// 左後方旋回（Back turn）
void Bk_turnLeft( void ){
	M1_IN1 = 0;
	M1_IN2 = 0;
	M2_IN1 = 1;
	M2_IN2 = 0;
}

// 右信地旋回（右回転 pivot turn）
void Pt_Right( void ){
	M1_IN1 = 1;
	M1_IN2 = 0;
	M2_IN1 = 0;
	M2_IN2 = 1;
}

// 左信地旋回（左回転 pivot turn）
void Pt_Left( void ){
	M1_IN1 = 0;
	M1_IN2 = 1;
	M2_IN1 = 1;
	M2_IN2 = 0;
}

// 後進
void Back( void ){
	M1_IN1 = 0;
	M1_IN2 = 1;
	M2_IN1 = 0;
	M2_IN2 = 1;
}

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