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

        Title: ＭＩＲＳ�Vシステム　超音波モード
        
        File : SS_MODE.C        Ver.1.0  written on Feb.14,1996 by J.W.

*************************************************************************/
#include "m9401.h"
#include "mathf.h"

extern float Th;
extern float Th_ref;
extern int Dist[];
extern int flag;
extern int Ps_d;
extern int Ts_d;
extern int Th_e;
extern int Posi;
extern int Turn;
extern int Seg[];
extern int mode;
extern int Act; /* 0:strait 1:turn_left 2:turn_right 3:back 4:stop */

int ss_mode()
{
        static int sposi;
        static int Ssc;
        if(flag==ON){
                Act=1;
                if (Turn==0) Th_ref=Th+PIH;
                else         Th_ref=Th-PIH;
                flag=OFF;
                Ssc=0;
                sposi=Posi;
        }
        if(Ssc==0 && Th_e==1){
                Act=0;
                Ssc=1;
        }
        if(Ssc==1 && sposi!=Posi){
                flag=ON;
                mode=NOMAL;
        }
        if(Act==0 && Posi!=0 && Dist[0]<=20) flag==ON;   
        if(Ps_d!=0x00){
                flag=ON;
                mode=PS_MODE;
        }
        else if((Ts_d & 0x80) == 0x80){
                flag=ON;
                mode=ESCAPE;
        }
        Seg[1]=sposi;
}