/************************************************************************
Module# 19

	Title: 運動制御関係モジュ−ル（自己状態演算） 
	
	File : CONT2.C   	Ver.1.0  written on Jan.12,1996 by M.M.
				Ver.2.0  written on Jan.27,1996 By J.W.
				Ver.2.1             Jan.31,1996
				Ver.2.2		    Feb.5.1996
				
*************************************************************************/
#include  "m9401.h"
#include  "math.h"
#include  "mathf.h"

#define TH_EPS	0.0872664	/* PIQ/9 */ /* 4.5  */
/* #define S	45 */

extern int Ctrl;
extern int S;
extern int Tmc;
extern int Posi;
extern int Turn;
extern float Pr,Pl;
extern float Om,Th,V;
extern int Th_e;
extern int Ze;
extern float Th_ref;
extern int Z_ref;
extern float Xc,Yc;
extern float Kv,Ko,Kc;
extern float Z;

extern unsigned char inportu();/*  */
int signcv();
int encoder();

void calc_state()
{/* 自己状態演算 */
	float omg_r,omg_l,om_old;
	int rot_r,rot_l;
	int tmc;
	int data_rh,data_rl,data_lh,data_ll;
	float dx,dy,dz,th1;
	float r;
	
	/* Ver.2.0 ロ−タリ・エンコ−ダ読み込み */
	data_rl=inportb(EDRL);
	data_rh=inportb(EDRH);
	data_ll=inportb(EDLL);
	data_lh=inportb(EDLH);

	outportb(CRST,0x03);

	rot_r = ((data_rh & 0x0f) << 8) + (data_rl & 0xff);
        rot_l = ((data_lh & 0x0f) << 8) + (data_ll & 0xff);
	if ((rot_r & 0x800) != 0)
		rot_r = (rot_r | 0xfffff000);
	if ((rot_l & 0x800) != 0)
 		rot_l = (rot_l | 0xfffff000);

        Pr = KPG * rot_r;
	Pl = KPG * rot_l;

	tmc = Tmc;

	/* 座標の算出  */			
	dz = (Pr + Pl) / 2;
        /* dz= Pr; */
 	Z += dz;
	r = B * (Pr + Pl) / (Pr - Pl);
	th1 = (Pr - Pl) /(2* B);
	Th += th1;
	if (Th >= PI2)
		Th -= (float)PI2;
	else if (Th < 0)
		Th += (float)PI2;
	dx = dz * cosf(Th);
	dy = dz * sinf(Th);
	Xc += dx;
	Yc += dy;

	if(Turn==0){
        	if(Th_ref > PI2)
			Th_ref=Th_ref-(float)(PI2);
		if(Th_ref < 0)
			Th_ref=Th_ref+(float)(PI2);
		if(Th>Th_ref && Th 0)
				Th_e=2;
			else
				Th_e=3;
		}
		if(Th_ref==0){
			if(Th>=0 && Th PI2)
			Th_ref=Th_ref-(float)(PI2);
		if(Th_ref < 0)
			Th_ref=Th_ref+(float)(PI2);
		if(ThTh_ref-TH_EPS)
			Th_e=1;
		else{
			if (Th_ref-Th > 0)
				Th_e=2;
			else
				Th_e=3;
		}
        	if(Th_ref==0){
			if(Th<=PI2 && Th>PI2-TH_EPS)
        			Th_e=1;
			else if(Th-1*Xc && Yc-1*Xc && Yc>Xc)
			Posi = 2;
		else if(Yc>Xc && Yc<-1*Xc)
			Posi = 3;
		else if(Yc<-1*Xc && Yc< Xc)
			Posi = 4;
    }
}