#include "m9402.h"
#include /* to match mcc68k */
#include
#define PI 3.14159265f
#define DEGREEIZE (180.0f/PI)
#define PERIOD 0.01f /* Period of System Clock (sec) */
#define LOSS 100 /* for re error 2/5 */
#define ROT_TEST_L 1.0f /* This value is determined by test */
#define ROT_TEST_R 1.0f /* '' */
#define ROT_THEO_L (2*PI/800.0f) /* This value is determined by theory */
/* (2*PI/6400.0f) */
#define ROT_THEO_R (2*PI/800.0f) /* '' */
/* (2*PI/6400.0f) */
/* EDLH is defined in "m9402.h" */
/* EDLL '' */
/* EDRH '' */
/* EDRL '' */
#define RADI_DTL 4.0f
#define RADI_DTR 4.0f
#define RADI_MTL 1.52f /* 4.0f */
#define RADI_MTR 1.52f /* 4.0f */
#define LOCA_DTL 10.5f
#define LOCA_DTR 10.5f
#define LOCA_MTL 5.34f /* 10.5f */
#define LOCA_MTR 5.34f /* 10.5f */
#define MIRS_INIX 125.0f /* MIRS's INItialize position (X) */
#define MIRS_INIY 50.0f /* '' (Y) */
#define MIRS_INIT (PI/2.0f) /* '' (Theta) */
/* #define KITEI */ /* no timer interrupt */
/* #define TIME 2 */ /* '' */
extern struct pos
{
int x ; /* cm */
int y ; /* cm */
int v ; /* cm/sec */
int theta ; /* degree */
int thetav ; /* degree/sec */
int way ; /* cm */
}
rot_position_i ;
extern int initial[MAXTSK] ;
extern unsigned int mmi_iivseg ;
extern unsigned char pwm_mode ;
/* int davi_l ; */
/* int davi_r ; */
float rot_get( float *rot_measure , unsigned int dummy )
/* called in timt05() */
/* dummy is never used */
{
float samp_time ; /* sampling time of Rotary Encoder */
unsigned int timer_new ; /* new value of System Clock Counter */
static unsigned int timer_old ; /* old value of System Clock Counter */
int counter[2] ; /* counted by Rotary Encoder */
float coef[2] ; /* coefficient ( count --> angular frequency ) */
int rot_measure_i[2] ; /* angular frequency [degree/sec] (integer) */
unsigned int c_h ; /* * */ /* high part of count data */
unsigned int c_l ; /* * */ /* low part of count data */
int i ; /* control variable */
static int stp_time ; /* for re error 2/5 */
static int old_c[2] ; /* for re error 2/5 ; old counter */
float rot_mid ;
float rot_ori[2] ;
if( initial[5] != 0 )
{
timer_old = 0 ;
stp_time = 0 ;
old_c[0] = 0 ;
old_c[1] = 0 ;
}
rot_measure[0] = 0.0f ; /* angular frequency [rad/sec] (float) */
rot_measure[1] = 0.0f ; /* '' */
/* determine coefficients and count values */
/* right */
c_l = inportb(EDRL) & 0xff ;
c_h = inportb(EDRH) & 0x0f ;
outportb(CRST, 0x01) ;
outportb(CRST, 0x01) ;
outportb(CRST, 0x01) ;
counter[1] = (c_h << 8) + c_l ;
/* left */
c_l = inportb(EDLL) & 0xff ;
c_h = inportb(EDLH) & 0x0f ;
outportb(CRST, 0x02) ;
outportb(CRST, 0x02) ;
outportb(CRST, 0x02) ;
counter[0] = (c_h << 8) + c_l ;
for( i=0 ; i<2 ; i++ )
{
if( (counter[i] & 0x0800) != 0x0000 )
{
counter[i] |= ~0x0fff ;
}
}
counter[0] *= -1 ; /* 2/14 ; electronics' request */
#ifdef KITEI
samp_time = PERIOD * (float)TIME ;
#else
timer_new = sys9(0,2) ;
samp_time = PERIOD * (float)(timer_new - timer_old) ;
sys8(0,2,0) ;
timer_old = 0 ;
if( samp_time == 0.0f )
return -1 ;
#endif
/* for re error 2/5,2/11 ; filter */
if( (pwm_mode&0xc6) == 0x00 )
{
stp_time += timer_new ;
for( i=0; i<2; i++ )
{
if( stp_time >= LOSS )
{
old_c[i] = counter[i] ;
counter[i] = 0 ;
}
else if( abs(counter[i]) > abs(old_c[i]) )
{
counter[0] = 0 ; /* 2/7,2/16 */
counter[1] = 0 ; /* 2/16 */
old_c[i] = 0 ; /* 2/7 */
}
else
{
old_c[i] = counter[i] ;
}
}
}
else
{
stp_time = 0 ;
old_c[0] = counter[0] ;
old_c[1] = counter[1] ;
}
/* make data */
coef[0] = ROT_TEST_L * ROT_THEO_L ;
coef[1] = ROT_TEST_R * ROT_THEO_R ;
for( i=0 ; i<2 ; i++ )
{
rot_ori[i] = coef[i] * (float)counter[i] / samp_time ;
}
/* make data */
rot_mid = ( rot_ori[0] + rot_ori[1] ) / 2.0f ;
rot_measure[0] = (RADI_MTL/RADI_DTL) * ( rot_mid + LOCA_DTL / LOCA_MTL * ( rot_ori[0] - rot_mid ) ) ;
rot_measure[1] = (RADI_MTR/RADI_DTR) * ( rot_mid + LOCA_DTR / LOCA_MTR * ( rot_ori[1] - rot_mid ) ) ;
for( i=0 ; i<2 ; i++ )
{
rot_measure_i[i] = (int) ( rot_measure[i] * DEGREEIZE ) ;
sys8(5,i+1,rot_measure_i[i]) ;
}
/* davi_l = counter[0] ; */
/* davi_r = counter[1] ; */
return( samp_time ) ;
}
void rot_posi( float *rot_measure , float samp_time )
/* called in timt05() */
{
static struct posf
{
float x ; /* cm */
float y ; /* cm */
float v ; /* cm/sec */
float theta ; /* rad *//* define x-axis = 0 */
float thetav ; /* rad/sec */
float way ; /* cm */
}
rot_position ;
float width, spin_v, ideal_moment ;
if( initial[5] != 0 )
{
rot_position.x = MIRS_INIX ;
rot_position.y = MIRS_INIY ;
rot_position.v = 0.0f ;
rot_position.theta = MIRS_INIT ;
rot_position.thetav = 0.0f ;
rot_position.way = 0.0f ;
}
width = LOCA_DTL + LOCA_DTR ;
spin_v = (-1) * RADI_DTL * rot_measure[0] + RADI_DTR * rot_measure[1] ;
ideal_moment = RADI_DTL * LOCA_DTR * rot_measure[0] + RADI_DTR * LOCA_DTL * rot_measure[1] ;
rot_position.thetav = spin_v / width ;
rot_position.v = ideal_moment / width ;
rot_position.theta += ( rot_position.thetav * samp_time ) ;
rot_position.x += ( rot_position.v * cosf(rot_position.theta) * samp_time ) ;
rot_position.y += ( rot_position.v * sinf(rot_position.theta) * samp_time ) ;
rot_position.way += fabsf( rot_position.v * samp_time ) ;
rot_position_i.thetav = (int) ( DEGREEIZE * rot_position.thetav ) ;
rot_position_i.v = (int) rot_position.v ;
rot_position_i.theta = (int) ( DEGREEIZE * rot_position.theta ) ;
if( rot_position_i.theta > 0 )
{
rot_position_i.theta %= 360 ;
}
else
{
while( rot_position_i.theta < 0 )
rot_position_i.theta += 360 ;
}
rot_position_i.x = (int) rot_position.x ;
rot_position_i.y = (int) rot_position.y ;
rot_position_i.way = (int) rot_position.way ;
mmi_iivseg ; /* for test */
}
unsigned int rot_reset( void ) /* called in timt05() */
{
return 0 ;
}
ŠJ”‹K–ñ‚É–ß‚é