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

        Title: ＭＩＲＳ�Vシステム　ノーマルモード

        File : NOMAL.C          Ver.1.0  written on Jan.26,1996 by J.W.
                                Ver.2.0             Feb.5,1996

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

extern float Z;

extern float Th;
extern float Th_ref;
extern float V;
extern float V_ref;
extern int Tc;
extern int Tmc;
extern int Turn;
extern int Seg[];
extern int Th_e;
extern int flag;
extern int Ts_d;
extern int Dist[];
extern int Posi;
extern int Act; /* 0:go 1:turn_left 2:turn_right 3:back 4:stop */

int nomal()
{
        int act_o;
        flag=OFF;
        if(Turn==0){
                switch(Posi){
                        case 1:/*  */
                                Th_ref = PIH;
                        break;
                        case 2:
                                Th_ref = PI;
                        break;
                        case 3:
                                Th_ref = PI+PIH;
                        break;
                        case 4:
                                Th_ref = 0;
                        break;
                        default:
                                Th_ref = Th;
                        break;
                }
        }
        else{
                switch(Posi){
                        case 1:/*  */
                                Th_ref = PI2-PIH;
                        break;
                        case 2:
                                Th_ref = 0;
                        break;
                        case 3:
                                Th_ref = PIH;
                        break;
                        case 4:
                                Th_ref = PI;
                        break;
                        default:
                                Th_ref = Th;
                        break;
                }
        }

        act_o=Act;

        switch(Th_e){
                case 1:
                        Act=0;
                break;
                case 2:
                        Act=1;
                break;
                case 3:
                        Act=2;
                break;
                default:
                        Act=4;
                break;
        }

        if(act_o==0 && fabsf(Th_ref-Th) < PI/9)

                Act=0;

        if(act_o==0 && Th_ref==0){

                if(Th>PI2-PI/9 && Th< PI2) Act=0;

                if(Th>0 && Th< PI/9)       Act=0;

        }
        if(Act==0 && act_o!=0)

                Tc=0;

        /* if(Turn==0 && Posi!=0 && Act==0 && Dist[2]<=20) 
                Act=6; */ 
        if(Turn==1 && Posi!=0 && Act==0 && Dist[1]<=20)

                Act=6;


        Seg[1]=((Tmc%1000)/100);

        return(flag);

}