/************************************************************************

	Title: ＭＩＲＳ�Vシステム　回避モード
	
	File : ESCAPE.C     	Ver.1.0  written on Jan.27,1996 by J.W.
				Ver.1.1		    Feb.6,1996
				Ver.1.2		    Feb.12,1996
                                Ver.2.0		    Feb.13,1996
				
*************************************************************************/
#include "m9401.h"
#include "mathf.h"

extern float Z;
extern float Th;
extern float Th_ref;
extern float Xc,Yc;
extern int Z_ref;
extern int Tc;
extern int Sc;
extern int Posi;
extern int flag;
extern int mode;
extern int Ze;
extern unsigned int Tmc;
extern int Th_e;
extern int Ps_d;
extern int Dist[];
extern int Seg[];
extern int Ts_d;/* Touch Sensor Data */
extern int Act;	/* 0:go 1:turn_left 2:turn_right 3:back 4:stop */

int escape()
{
	static int tmc;
	static int ts_d;
	static int posi2;
	static int back;
	static float th2;
	static int Ec;
	static int posi3;

	if(flag==ON){
		flag=OFF;
		Sc=0;
                Ec=0;
		tmc=Tmc;
		ts_d=Ts_d;
		th2=Th;
		posi3=Posi;
		if(fabsf(Xc)<60 && fabsf(Yc)<60)
			posi2=0;
		else	posi2=1;

		if(posi2==0){
			back=0;		
		}
		else{
                	Z_ref=(int)Z-30;
			switch(Posi){
					case 1:
					if(Th>PIH && Th< PI+PIH)
							back=0;
					else	back=1;
				break;
				case 2:
					if(Th>PI && Th< PI2)
						back=0;
					else	back=1;
	        	        break;
				case 3:
					if((Th>=0 && ThPI2-PIH && Th< PI2))
						back=0;
					else	back=1;
				break;
				case 4:
					if(Th>0 && Th< PI)
						back=0;
					else	back=1;
				break;
				default:
					back=0;
				break;
			}
                }
	}
	if(back==0){
		Act=4;
		if(tmc+300< Tmc){
			if (Dist[0]<=20) back=ON;
			else{
				flag=ON;
				mode=NOMAL;
			}
		}
	}
	if(back==1){
		if(Ec==0){
			Act=3;
		}
                if(Ze==1) Ec=1;
		if(Sc==0 && Ec==1){
			if((ts_d & 0x20) == 0x20){ /* Left TS */
				Act=2;
				Th_ref=th2-PI+PIQ;
    		}
			else if((ts_d & 0x10) == 0x10){ /* Right TS */
				Act=1;
				Th_ref=th2+PIH+PIQ;
			}
			else{
				Act=4;
				flag=ON;
				mode=NOMAL;
			}
		}
		if (Th_e==1){
			Sc=1;
			Act=0;
        	}
		if(Posi != posi3){
			flag=ON;
			mode=NOMAL;
		}
		if (Act==0 && ((Ts_d & 0x80) == 0x80)){
			flag=ON;
		mode=ESCAPE;
		}
	}
	if(Ps_d != 0x00){
		flag=ON;
		mode=PS_MODE;
	}
	Seg[1]=posi2;
	return(flag);
}