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

        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 && Th0 && Th