/************************************************************************
Module# 01	
	　
	Title: ＭＩＲＳ�Vシステム　メイン関数（本戦用）
	
	File : main.c	Ver.1.0	 written on Dec.10,1995 by J.W.
				
*************************************************************************/
#include "m9401.h"

unsigned short fp_status;
unsigned short fp_control;
int errno;
int S;
int V1;
int Turn;
int Mirs;
int flag;
int mode;
int Dist[3];
int Ctrl,First;/* Control Type	*/
int Tmc;/* Timer Counter */
int Run_n;
int Int4_type;/* Types of Interrupt Level 4 */
int Us_target[US_CN];/* Distance to the target robot */
int Posi;
int Ps_d;/* Photo Sensor Data */
int Ts_d;/* Touch Sensor Data */
int Tc,Sc;
int Act;
int Th_e;
int Ze;
int Z_ref;
int Seg[4];/* 7seg_Led Data (1-4) */
int Led1;/* Green Led */
int Led2;/* Red Led */
unsigned int Mr,Ml;
unsigned int Pulse;
float Pr,Pl;float V_ref,Ers_v;/* 速度目標値と偏差累積値 */
float Th_ref,Ers_th;/* 姿勢角目標値と偏差累積値 */
float Xc,Yc;/* ＭＩＲＳの座標 [cm]  */
float V,Th,Om;/* ＭＩＲＳの状態（速度[m/s]、姿勢角[rad]、角速度[rad/s]） */
float Th0;/* 初期姿勢角[rad]  */
float Z;

extern int mirs_stop();/* */
extern int reset_mode();/*  */
extern int init();/*  */
extern int select();
extern int disp();
extern void control(); /* by Takumi Abe */
extern int command();

main()
{
	Int4_type = RESET;
	Ctrl = MIRS;
	First = 1;
	Mirs = init();
	reset_mode();
	
	asm("\tMOVE\t#$2000,SR");

	command();
	while( Mirs == ACTIVE )
	{/* 動作中 */
		control();
		Ps_d=inportb(PBDR2);
		Ts_d=inportb(EDRH);

		disp();
		select();

	}
	mirs_stop();/* ＭＩＲＳ　ストップ */
}