/************************************************************************
Module# 20,25	
	
	Title: 運動制御関係モジュ−ル（運動制御） 
	
	File : cont3.c	Ver.1.0  written on Jan.12,1996 by M.M.
			Ver.1.1 	    Jan.28,1996 by J.W.
			Ver.2.0		    Feb.13,1996
                        Ver.2.1		    Feb.14,1996
				    
*************************************************************************/
#include  "m9401.h"
#include  "math.h"

/* #define V1 	45 */
#define V1_low	3 
#define V2 	20 
#define V3	53

/*extern int Mag;*/
extern float V_ref,Th_ref;
extern float Om,Th,V;
extern float Th0;
extern int V1;
extern int Tc;
extern int Act;
extern float Pr,Pl;
extern unsigned int Mr, Ml;
extern unsigned int Pulse;
extern int Run_n;
extern float Xc,Yc;
extern int Tmc,Ctrl;
int mtr_l,mtr_r;

extern unsigned char outportu(); /*  */

unsigned int limit();

void control()
{/* 運動制御 */
	
	switch(Act) {
		case 0: /* strait */
			if (Tc == 0){ 
				mtr_r = V1;
				mtr_l = V1;
			}
			else if((mtr_l<(V1_low))||(mtr_r<(V1_low))){
				mtr_l=V1;
				mtr_r=V1;
			}
			else if (Pr < Pl){
				mtr_r += 1;
				mtr_l -= 1;
			}
			else if (Pr > Pl){
				mtr_r -= 1;
				mtr_l += 1;
			}
		break;
		case 1: /* turn left */
			mtr_r = V2;
			mtr_l = -V2;
		break;
		case 2: /* turn right */
			mtr_r = -V2;
			mtr_l = V2;
		break;
		case 3:/* back */
			mtr_r = -40;
			mtr_l = -40;
		break;
		case 4:/* stop */
			mtr_r = 0;
			mtr_l = 0;
		break;
                case 5: /* strait */
                        if (Tc == 0){
                                mtr_r = V3;
                                mtr_l = V3;
                        }
                        else if((mtr_l<(V1_low))||(mtr_r<(V1_low))){
                                mtr_l=V3;
                                mtr_r=V3;
			}
                        else if (Pr < Pl){
                                mtr_r += 1;
                                mtr_l -= 1;
                        }
                        else if (Pr > Pl){
                                mtr_r -= 1;
                                mtr_l += 1;
                        }
		break;
		case 6:/* cover */
			mtr_r = mtr_r + 5;
			mtr_l = mtr_l;
                break;
	   	default:
  			mtr_r = 0;
			mtr_l = 0;
		break;
	}
	Tc++; 
	limit(mtr_r);
        Mr = Pulse;
	limit(mtr_l);
        Ml = Pulse;
	outportu(PADR1,Mr);
	outportu(PBDR1,Ml);
}

unsigned int limit(data)
int data;
{/* モ−タ指令デ−タ・リミッタ− Ver.2.2 */
	if( data < -127)
		Pulse = 0xff;
	else if( data < 0 )
		Pulse = ((unsigned char)fabs(data) << 1) | 0x01;
	else if( data < 127 )
		Pulse = ((unsigned char)data << 1) | 0x00;
	else
		Pulse = 0xfe;
			
	return(ON);
}