#include
#include "m9402.h"
#define A_PWM_DUTY_L PBDR1 /* depend on electronics' request */
#define A_PWM_DUTY_R PADR1 /* '' */
/* #define PWM_VELO_SLOW 15.0f */ /* MIRS's velosity (slow) */
/* #define PWM_VELO_FAST 20.0f */ /* '' (fast) */
#define PWM_SLOW_STR 30.0f /* for straight ; 2/7 ,2/11 ,2/12 ,2/13 ,2/14 ,2/16 */
#define PWM_FAST_STR 50.0f
#define PWM_SLOW_CUR 10.0f /* for curve ; 2/7 ,2/11 ,2/12 ,2/13 ,2/14 */
#define PWM_FAST_CUR 30.0f
#define PWM_SLOW_ROT 10.0f /* for rotation ; 2/7 ,2/11 ,2/12 ,2/13 ,2/14 */
#define PWM_FAST_ROT 20.0f
#define PWM_RADI_SHORT 15.0f /* MIRS's radius of motion (short) ,2/14 */
#define PWM_RADI_LONG 40.0f /* '' (long) ,2/14,2/15 */
#define RADI_DTL 4.0f /* 'dtl' means left tire for drive */
#define RADI_DTR 4.0f /* 'dtr' means right tire for drive */
#define LOCA_DTL 10.3f
#define LOCA_DTR 10.3f
#define PI 3.14159265f
#define DEGREEIZE (180.0f/PI)
#define PWM_MAX 80 /* limit of pwm duty */
#define PWM_MIN 10 /* '' , 2/13,2/14,2/14 */
#define PWM_MIN_S 20 /* '' (straight) , 2/14 */
#define PWM_NN 10 /* for PWM control , 2/13,2/14,2/14,2/16 */
#define FOOD 10 /* for feedback 2/5 , 2/13,2/14,2/16 */
#define FOOD_S 3 /* 2/16 */
#define FEED_MAX 20 /* 2/14,2/16 */
#define MOTORIZE_L 10 /* duty ratio [%] --> angular frequency [degree/sec], image */
#define MOTORIZE_R 10 /* '' */
extern unsigned char pwm_mode ; /* data of MIRS's motion */
extern int initial[MAXTSK] ;
int tire( unsigned char, int ) ;
int feedback0( int *, unsigned char, int *, int *) ; /* 2/16 */
int feedback1( int *, unsigned char, int *, int *) ; /* 2/16 */
void pwm_give( void )
{
/* index : 0 --- left , 1 --- right */
int tire_dt[2] ; /* angular frequency of tires */
static int tire_t[2] ; /* '' */
int diff_dummy[2] ; /* 2/16 */
int diff[2] ;
unsigned int i ;
char dir ; /* direction of tires. 1:forward, 0:back */
char data[2] ; /* pwm send data */
char pwm_limit[2] ; /* 2/13 */
char pwm_low[2] ; /* 2/13 */
static char pwm_duty[2] ; /* [-100,100] [%] */
static int control[2] ;
static unsigned char last_mode ; /* last pwm_mode */
static char duty_max[2] ; /* 2/15 */
static char duty_min[2] ; /* 2/15 */
static int duty_count[2] ; /* 2/15 */
static int max_flg[2] ; /* 2/15 */
static int min_flg[2] ; /* 2/15 */
if( initial[4] != 0 ) /* initialize : static */
{
tire_t[0] = 0 ;
tire_t[1] = 0 ;
pwm_duty[0] = 0 ;
pwm_duty[1] = 0 ;
control[0] = 0 ;
control[1] = 0 ;
last_mode = 0x04 ; /* useless value 0x04 */
duty_max[0] = 0 ; /* 2/15 */
duty_max[1] = 0 ; /* 2/15 */
duty_min[0] = 100 ; /* 2/15 */
duty_min[1] = 100 ; /* 2/15 */
duty_count[0] = 0 ; /* 2/15 */
duty_count[1] = 0 ; /* 2/15 */
max_flg[0] = 0 ; /* 2/15 */
max_flg[1] = 0 ; /* 2/15 */
min_flg[0] = 0 ; /* 2/15 */
min_flg[1] = 0 ; /* 2/15 */
}
diff[0] = 0 ; /* 2/7 */
diff[1] = 0 ;
diff_dummy[0] = 0 ; /* 2/16 */
diff_dummy[1] = 0 ; /* 2/16 */
tire_dt[0] = sys9(5,1) ; /* real angular frequency of tires */
tire_dt[1] = sys9(5,2) ; /* [deg/sec] */
/* feedback 2/4,2/13 */
if( last_mode != pwm_mode )
{
tire_t[0] = tire( pwm_mode, 0 ) ; /* target angular frequency of tires */
tire_t[1] = tire( pwm_mode, 1 ) ; /* [deg/sec] */
if( (pwm_mode&0xd0) == 0xd0 ) /* 2/16 */
{
pwm_duty[0] = tire_t[0] / MOTORIZE_L ;
pwm_duty[1] = 0 ;
}
else if( (last_mode&0xc6) == 0x00 ) /* 2/14 */
{
pwm_duty[0] = tire_t[0] / MOTORIZE_L ; /* 2/13,2/14 */
pwm_duty[1] = tire_t[1] / MOTORIZE_R ; /* 2/13,2/14 */
}
else if( (last_mode&0x02) == 0x02 ) /* for rotation , 2/16 */
{
pwm_duty[0] = tire_t[0] / MOTORIZE_L ; /* 2/13,2/14 */
pwm_duty[1] = tire_t[1] / MOTORIZE_R ; /* 2/13,2/14 */
}
else if( (pwm_mode&0xc2) == 0x40 ) /* for straight ,2/16 */
{
pwm_duty[0] = tire_t[0] / MOTORIZE_L ; /* 2/13,2/14 */
pwm_duty[1] = tire_t[1] / MOTORIZE_R ; /* 2/13,2/14 */
}
else if( (last_mode&0x20) != (pwm_mode&0x20) ) /* 2/14 */
{
pwm_duty[0] = tire_t[0] / MOTORIZE_L ;
pwm_duty[1] = tire_t[1] / MOTORIZE_R ;
}
control[0] = 0 ;
control[1] = PWM_NN ; /* 2/15 */
duty_max[0] = 0 ; /* 2/15 */
duty_max[1] = 0 ; /* 2/15 */
duty_min[0] = 100 ; /* 2/15 */
duty_min[1] = 100 ; /* 2/15 */
duty_count[0] = 0 ; /* 2/15 */
duty_count[1] = 0 ; /* 2/15 */
max_flg[0] = 0 ; /* 2/15 */
max_flg[1] = 0 ; /* 2/15 */
min_flg[0] = 0 ; /* 2/15 */
min_flg[1] = 0 ; /* 2/15 */
}
else if( control[0] < control[1] ) /* 2/15 */
{
control[0]++ ;
}
else
{
/* determine diff[2] in feedback0(), feedback1(), 2/16 */
(void) feedback0( diff_dummy, pwm_mode, tire_dt, tire_t ) ; /* 2/16 */
diff[0] = diff_dummy[0] ; /* for curve ,2/16 */
if( (pwm_mode&0x02) == 0x02 ) /* for rotation ,2/16 */
{
diff[0] = diff_dummy[0] ;
diff[1] = diff_dummy[1] ;
}
else /* for curve or straight , 2/16 */
{
(void) feedback1( diff_dummy, pwm_mode, tire_dt, tire_t ) ;
diff[1] = diff_dummy[1] ; /* for curve ,2/16 */
if( (pwm_mode&0xc2) == 0x40 ) /* for straight ,2/16 */
{
diff[0] = FOOD_S * diff_dummy[0] ;
diff[1] = FOOD_S * diff_dummy[1] ;
}
}
/* determine limit ,2/13 */
/* high */
pwm_limit[0] = PWM_MAX ;
pwm_limit[1] = PWM_MAX ;
if( (pwm_mode&0xc6) == 0x00 )
{
pwm_limit[0] = 0 ;
pwm_limit[1] = 0 ;
}
if( (pwm_mode&0xc2) == 0x80 )
{
pwm_limit[0] = PWM_MAX / 2 ;
}
if( (pwm_mode&0xc2) == 0xc0 )
{
pwm_limit[1] = PWM_MAX / 2 ;
}
/* low */
pwm_low[0] = PWM_MIN ; /* 2/13 */
pwm_low[1] = PWM_MIN ; /* 2/13 */
if( (pwm_mode&0xc0) == 0x40 ) /* 2/14 */
{
pwm_low[0] = PWM_MIN_S ; /* 2/14 */
pwm_low[1] = PWM_MIN_S ; /* 2/14 */
}
if( (pwm_mode&0xc2) == 0x80 ) /* 2/15 */
{
pwm_low[1] = PWM_MIN_S ; /* 2/15 */
}
if( (pwm_mode&0xc2) == 0xc0 ) /* 2/15 */
{
pwm_low[0] = PWM_MIN_S ; /* 2/15 */
}
/* 2/15,2/16 */
for( i=0 ; i<2 ; i++ )
{
if( (pwm_mode&0xc0)==0x40 || i==1 ) /* 2/15,2/16 for right tire feedback */
break ;
if( duty_count[i] >= 3 )
{
pwm_duty[i] = (duty_max[i]+duty_min[i])/2 ;
pwm_low[i] = pwm_duty[i] - 5 ;
pwm_limit[i] = pwm_duty[i] + 5 ;
if( abs(tire_dt[i]*3/2) < abs(tire_t[i]) ) /* non stop ,2/16 */
{
duty_count[i] = 0 ;
pwm_duty[0] = tire_t[0]/MOTORIZE_L ;
}
}
}
/* feedbacking */
for( i=0 ; i<2 ; i++ )
{
if( pwm_duty[i]>=pwm_limit[i] && diff[i]>0 ) /* 2/13 */
{}
else if( pwm_duty[i]<=(-pwm_limit[i]) && diff[i]<0 ) /* 2/13 */
{}
else if( pwm_duty[i]>0 && pwm_duty[i]<=pwm_low[i] && diff[i]<0 ) /* 2/13 */
{}
else if( pwm_duty[i]<0 && pwm_duty[i]>=(-pwm_low[i]) && diff[i]>0 ) /* 2/13 */
{}
else
{
pwm_duty[i] += diff[i] ;
}
/* protect 2/15 */
if( pwm_duty[i]<0 && tire_t[i]>0 ) /* 2/15 */
{
pwm_duty[i] = 1 ;
}
if( pwm_duty[i]>0 && tire_t[i]<0 ) /* 2/15 */
{
pwm_duty[i] = -1 ;
}
/* for stop 2/15 */
if( tire_t[i] == 0 )
{
pwm_duty[i] = 0 ;
}
}
/* 2/15 */
for( i=0 ; i<2 ; i++ )
{
if( duty_count[i] >= 3 )
break ;
if( pwm_duty[i] >= duty_max[i] )
{
duty_max[i] = pwm_duty[i] ;
max_flg[i] = 1 ;
}
else if( max_flg[i] == 1 )
{
duty_count[i]++ ;
max_flg[i] = 0 ;
}
else
{
duty_max[i] = pwm_duty[i]+1 ; /* 2/16 */
}
if( pwm_duty[i] <= duty_min[i] )
{
duty_min[i] = pwm_duty[i] ;
min_flg[i] = 1 ;
}
else if( min_flg[i] == 1 )
{
duty_count[i]++ ;
min_flg[i] = 0 ;
}
else
{
duty_min[i] = pwm_duty[i]-1 ;
}
}
control[0] = 0 ;
}
last_mode = pwm_mode ;
for( i=0 ; i<2 ; i++ ) /* make data for pwm */
{
dir = 1 ;
data[i] = (127*pwm_duty[i])/100 ;
if( data[i] < 0 )
{
dir = 0 ;
data[i] *= -1 ;
}
data[i] = (data[i] << 1) + dir ;
}
outportb( A_PWM_DUTY_L , (int) data[0] ) ; /* * */ /* ! */
outportb( A_PWM_DUTY_R , (int) data[1] ) ; /* * */ /* ! */
}
/* determine tire velocity */
int tire( unsigned char pwm_mode, int i ) /* i : left or right */
{
float radi_dt[2] ;
float loca_dt[2] ;
float velo_t ; /* 't' means 'target' */
float curv_t ; /* '' */
float tire_t[2] ; /* '' */
unsigned char factor ;
radi_dt[0] = RADI_DTL ; /* initialize */
radi_dt[1] = RADI_DTR ;
loca_dt[0] = (-1) * LOCA_DTL ;
loca_dt[1] = LOCA_DTR ;
/* remake ; 2/7 */
factor = pwm_mode & 0xc8 ;
switch( factor ) /* determine target velocity */
{
case 0x00: case 0x40:
velo_t = PWM_SLOW_STR ;
break ;
case 0x08: case 0x48:
velo_t = PWM_FAST_STR ;
break ;
case 0x80: case 0xc0:
velo_t = PWM_SLOW_CUR ;
break ;
case 0x88: case 0xc8:
velo_t = PWM_FAST_CUR ;
break ;
}
factor = pwm_mode & 0x0b ;
switch( factor )
{
case 0x02: case 0x03:
velo_t = PWM_SLOW_ROT ;
break ;
case 0x0a: case 0x0b:
velo_t = PWM_FAST_ROT ;
break ;
}
factor = ( pwm_mode >> 4 ) & 0x01 ;
switch( factor ) /* determine target radius */
{
case 0 : curv_t = PWM_RADI_SHORT ;
break ;
case 1 : curv_t = PWM_RADI_LONG ;
break ;
}
factor = ( pwm_mode >> 6 ) & 0x03 ;
switch( factor ) /* determine target velocity of tires */
{
case 0 : tire_t[i] = 0.0 ;
break ;
case 1 : tire_t[i] = DEGREEIZE * velo_t / radi_dt[i] ;
break ;
case 2 : tire_t[i] = DEGREEIZE * ( 1.0f + loca_dt[i] / curv_t ) * velo_t / radi_dt[i] ;
break ;
case 3 : tire_t[i] = DEGREEIZE * ( 1.0f - loca_dt[i] / curv_t ) * velo_t / radi_dt[i] ;
break ;
}
factor = ( pwm_mode >> 5 ) & 0x01 ;
switch( factor ) /* for back */
{
case 0 : tire_t[i] *= -1.0f ;
break ;
}
factor = pwm_mode & 0x03 ;
switch( factor ) /* for rotation */
{
case 2 : tire_t[0] *= -1.0f ;
tire_t[1] *= 1.0f ;
break ;
case 3 : tire_t[0] *= 1.0f ;
tire_t[1] *= -1.0f ;
break ;
}
return ( (int)tire_t[i] ) ;
}
/* make feedback data ; 2/13,2/14,2/15 Big change */
/* 2/16 */
int feedback0( int *diff, unsigned char pwm_mode, int *t_dt, int *t_t )
{
int tire_dt[2] ; /* 2/7 */
int tire_t[2] ; /* 2/7 */
int i ; /* 2/13 */
int feed_limit[2] ; /* 2/14 */
/* initialize */
tire_dt[0] = t_dt[0] ;
tire_dt[1] = t_dt[1] ;
tire_t[0] = t_t[0] ;
tire_t[1] = t_t[1] ;
diff[0] = 0 ;
diff[1] = 0 ;
/* determine feedback limit 2/14,2/15 */
feed_limit[0] = FEED_MAX ;
feed_limit[1] = FEED_MAX ;
if( (pwm_mode&0xc2) == 0x80 ) /* 2/14 */
feed_limit[0] /= 2 ; /* 2/14,2/15 */
if( (pwm_mode&0xc2) == 0xc0 ) /* 2/14 */
feed_limit[1] /= 2 ; /* 2/14,2/15 */
/* for feedback , 2/13,2/14 */
for( i=0 ; i<2 ; i++ )
{
diff[i] = (tire_t[i] - tire_dt[i]) ;
if( diff[i]>0 && diff[i]<=FOOD )
diff[i] = FOOD ;
if( diff[i]<0 && diff[i]>=(-FOOD) )
diff[i] = (-FOOD) ;
diff[i] /= FOOD ;
if( diff[i] > feed_limit[i] )
diff[i] = feed_limit[i] ;
if( diff[i] < (-feed_limit[i]) )
diff[i] = (-feed_limit[i]) ;
}
return 0 ;
}
/* make feedback data */
/* 2/16 */
int feedback1( int *diff, unsigned char pwm_mode, int *t_dt, int *t_t )
{
int tire_dt[2] ; /* 2/7 */
int tire_t[2] ; /* 2/7 */
int left_curv ;
int right_curv ;
int factor ;
int motion ;
/* initialize */
tire_dt[0] = t_dt[0] ;
tire_dt[1] = t_dt[1] ;
tire_t[0] = t_t[0] ;
tire_t[1] = t_t[1] ;
diff[0] = 0 ;
diff[1] = 0 ;
motion = 0 ; /* front */
/* for rotation mode */
factor = pwm_mode & 0x03 ;
switch( factor )
{
/* 2/5,2/16 remake */
/* left rotation */
case 2:
/* tire_dt[0] *= -1 ; */
/* tire_t[0] *= -1 ; */
/* motion = 2 ; */
/* break ; */
return 0 ;
/* right rotation */
case 3:
/* tire_dt[1] *= -1 ; */
/* tire_t[1] *= -1 ; */
/* motion = 3 ; */
/* break ; */
return 0 ;
}
/* for rear mode */
factor = ( pwm_mode >> 5 ) & 0x01 ;
if( factor == 0 )
{
tire_dt[0] *= -1 ;
tire_dt[1] *= -1 ;
tire_t[0] *= -1 ;
tire_t[1] *= -1 ;
motion = 1 ;
}
/* when MIRS moves too left , left_curv > 0 */
left_curv = tire_dt[1]*tire_t[0]-tire_dt[0]*tire_t[1] ;
/* when MIRS moves too right , right_curv > 0 */
right_curv = -left_curv ;
factor = ( pwm_mode >> 6 ) & 0x03 ;
switch( factor )
{
/* stop mode */
case 0:
break ;
/* straight mode */
case 1:
if( tire_dt[0] > tire_dt[1] )
{
diff[0] = -1 ;
diff[1] = 1 ;
}
if( tire_dt[0] < tire_dt[1] )
{
diff[0] = 1 ;
diff[1] = -1 ;
}
break ;
/* left mode */
case 2:
if( tire_dt[0] <= 0 )
{
diff[0] = 1 ;
}
if( tire_dt[1] <= 0 )
{
diff[1] = 1 ;
}
if( tire_dt[0] > 0 && tire_dt[1] > 0 )
{
if( left_curv > 0 )
{
diff[0] = 1 ;
diff[1] = -1 ;
}
if( left_curv < 0 )
{
diff[0] = -1 ;
diff[1] = 1 ;
}
}
break ;
/* right mode */
case 3:
if( tire_dt[0] <= 0 )
{
diff[0] = 1 ;
}
if( tire_dt[1] <= 0 )
{
diff[1] = 1 ;
}
if( tire_dt[0] > 0 && tire_dt[1] > 0 )
{
if( right_curv > 0 )
{
diff[0] = -1 ;
diff[1] = 1 ;
}
if( right_curv < 0 )
{
diff[0] = 1 ;
diff[1] = -1 ;
}
}
break ;
}
switch( motion )
{
/* rear */
case 1:
diff[0] *= -1 ;
diff[1] *= -1 ;
break ;
/* 2/5,2/16 remake */
/* left rotation */
case 2:
/* diff[0] *= -1 ; */
break ;
/* right rotation */
case 3:
/* diff[1] *= -1 ; */
break ;
}
/* diff[0] *= FOOD ; */
/* diff[1] *= FOOD ; */
return 0 ;
}
ŠJ”‹K–ñ‚É–ß‚é