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

	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 && ThPI && Th=0 && ThPI2-PIH && Th0 && Th