/*=============================================*/
/*  加速度センサでロボットを動かす(Arduino側)  */
/*                          2009/03/30 miyabo  */
/*                      http://www.miyabo.com/ */
/*=============================================*/

#include <stdio.h>      //sscanfのために
#include <math.h>       //atan2()用

#define AD_NOR 512      //0Gの時の値
#define AD_RNG 300      //最大許容変化値

#define PI 3.14159265358979

/*---三角関数を毎回計算させないために---*/
const double PI_2 = PI/2;
const double PI_24 = PI/24;
const double RMIN = -1.0*PI+PI_24;
const double RMAX =  1.0*PI-PI_24;
/*--------------------------------------*/

int getVal(void);						//受信文字列を数値にして返す
int limit(int val,int vmin,int vmax);	//数値の範囲を制限

char val[5]="";						//受信バッファ
int int_x=512,int_y=512,int_z=512;	//各軸の値
int old_x=512,old_y=512,old_z=512;	//前回の値
boolean err,vcr;					//エラーフラグ、直進フラグ
double at;							//ハンドル回転角(rad)
double mS;							//モーター速度

void setup()
{
	Serial.begin(9600);
	pinMode(13,OUTPUT);

	pinMode(2,OUTPUT);    //モーター1 IN1
	pinMode(3,OUTPUT);    //モーター1 IN2
	pinMode(4,OUTPUT);    //モーター2 IN1
	pinMode(5,OUTPUT);    //モーター2 IN2
	Serial.println("start.");
}

/*=========メインループ=========*/
void loop()
{
	int_x=0;int_y=0;int_x=0;	//各軸の値をゼロクリア
	static byte ct;				//ループカウンタ
	err=0;		//エラーフラグ
	ct=0;		//カウンター
	while(1)
	{
		/*---データ受信部---*/
		switch(Serial.read())				//ヘッダ文字で処理を振り分け(X,Y,Z)
		{
			case 'X':int_x=getVal();break;	//受信ルーチンから数値部分をもらう
			case 'Y':int_y=getVal();break;
			case 'Z':int_z=getVal();break;
			default:break;
		}
		if(int_x!=0 && int_y!=0 && int_z!=0)break;	//3軸全部受信完了

		/*送信側が受信完了に気付かない場合受信完了したことにする*/
		if(ct>100){Serial.flush();Serial.println('o');ct=0;}

		ct++;
	}

	/*許容値オーバーの急激な変化はエラーとし、前回の値を使用*/
	if(AD_RNG<abs(int_x-AD_NOR)){int_x=old_x;err=1;}
	if(AD_RNG<abs(int_y-AD_NOR)){int_y=old_y;err=1;}
	if(AD_RNG<abs(int_z-AD_NOR)){int_z=old_z;err=1;}

	if(err==0){old_x=int_x;old_y=int_y;old_z=int_z;}	//エラーでない場合、値を保存

	/*ハンドルの回転角(rad)を求める                         */
	/*atan()でなくatan2()にすると0〜360度の範囲で求められる */
	at=atan2((1.0*int_y)-AD_NOR,(1.0*int_x)-AD_NOR);

	/*ハンドルを傾けるほど左右のモーターの回転数に差が出るように*/
	/*                          ≒傾けるほどよく曲がるように    */
	mS=100.0*cos(at+PI_2);

	/*ハンドルが真っ直ぐなとき、直進フラグを立てる*/
	if((RMAX<at)||(at<RMIN)){digitalWrite(13,1);vcr=1;}else{digitalWrite(13,0);vcr=0;

	if(int_z>AD_NOR+24){
		//前進
		digitalWrite(2,0);digitalWrite(3,1);
		digitalWrite(4,0);digitalWrite(5,1);
		analogWrite(10,limit(int_z-500- mS,0,255));
		analogWrite(11,limit(int_z-500 + mS,0,255));
	}else if(int_z<AD_NOR-24){
		//後退
		digitalWrite(2,1);digitalWrite(3,0);
		digitalWrite(4,1);digitalWrite(5,0);
		analogWrite(10,limit(-1*(int_z-500) - mS,0,255));
		analogWrite(11,limit(-1*(int_z-500) + mS,0,255));
	}else{
		if(vcr==1){
			//停止
			digitalWrite(2,0);digitalWrite(3,0);
			digitalWrite(4,0);digitalWrite(5,0);
			analogWrite(10,0);
			analogWrite(11,0);
		}else{
			//その場で旋回
			if(mS<0){
				digitalWrite(2,1);digitalWrite(3,0);
				digitalWrite(4,0);digitalWrite(5,1);
			}else{
				digitalWrite(2,0);digitalWrite(3,1);
				digitalWrite(4,1);digitalWrite(5,0);
			}
			analogWrite(10,limit(abs(2.5*mS),0,255));
			analogWrite(11,limit(abs(2.5*mS),0,255));
		}
	}
	Serial.println();             //念のため改行を飛ばしておく
}

int getVal(void)
{
	//staticの正しくない使い方(?)
	static byte ct;       //カウンタは8bitで十分
	static int prm;       //パラメーターは255を越えるのでint
	prm=0;
	for(ct=0;ct<4;ct++)
	{
		while(!Serial.available());	//何か文字が飛んでくるまで待つ
		val[ct]=Serial.read();		//配列に代入
	}

	/*文字列valを書式指定(%d)に従って変数prmに代入*/
	sscanf(val,"%d",&prm);

	/*受信バッファをクリア、受信完了文字を送信*/
	Serial.flush();Serial.println("o");

	return prm;
}

/*数値valの範囲をminからmaxまでに制限する*/
int limit(int val,int vmin,int vmax)
{
	if(val<vmin)return vmin;
	if(val>vmax)return vmax;
	return val;
}