Renesas V850 Series - Page 77

CHAPTER 4 PROGRAM LIST
Application Note U17209EJ1V0AN
80
cont_time1 = ( TM3 - s_time ) * 10 / 16; /* Convert to uSEC */
}
} else {
POER0 = 0x00 ; /* All phases off */
now_speed = 0;
cont_time1 = 0;
}
}
/****************************************************************************** /
/* Calculation processing of speed, etc. */
/****************************************************************************** /
void fcalcu( signed int *wrm, signed int *trm )
{
signed short es_trm, cur_time, delta, i ;
signed int wwrm, wk, *p1, *p2;
//
// Speed and position calculation from zero-cross point
//
cur_time = TM4 ;
delta = ( (RAD/6/P) * cur_time ) / sa_time ; /* Calculation of rotor position */
/* difference from reference point */
/* (radian) */
if ( object_speed >= 0 ) {
es_trm = base_position + delta;
} else {
es_trm = base_position - delta;
if ( es_trm < 0 ) es_trm += (RAD/P);
}
total_sa -= before_posi[20][1] ;
p1 = (int *)before_posi[19] ;
p2 = (int *)before_posi[20] ;
for ( i = 0; i <= 19 ; i++ ) {
*p2-- = *p1-- ;
}
before_posi[0][0] = *trm = es_trm % (RAD/P) ;
wk = before_posi[0][0] - before_posi[1][0] ;
if ( abs(wk) > (RAD/2/P) ) {
if ( wk < 0 ) {
wk = (RAD/P) + wk ;
} else {
wk = wk - (RAD/P) ;
}
}
before_posi[1][1] = wk ;