/**************************************************************************
* Set value precision limit for table
*/
int SetTablePrecisionI(TABLE *tabPO, int prenum)
{
int i,r,c;
DOUB mD,vD;
DB_TAB DB_PrI(">> SetTablePrecisionI %p prenum=%d\n",tabPO,prenum);
VALIDATE(tabPO,TABLE_ID);
if( (prenum>MAX_TAB_PRENUM) || (prenum<MIN_TAB_PRENUM) ) {
printf("Table precision limited %d to %d digits (after decimal)\n",
MIN_TAB_PRENUM, MAX_TAB_PRENUM);
printf("Value %d won't work; ignored\n",prenum);
return(FALSE);
}
tabPO->prenum = prenum;
tabPO->prefac = 1.0;
/***
* Now set working values
*/
if(prenum<0) {
mD = 0.1;
}
else {
mD = 10.0;
}
/***
* Now set working values by incrementing n steps
*/
i = 0;
while(i<ABS_VAL(tabPO->prenum)) {
tabPO->prefac *= mD;
i++;
}
DB_TAB DB_PrI("+ prefac=%1.12f\n",tabPO->prefac);
/***
* Update all values with new settings
*/
for(r=0;r<tabPO->nrow;r++)
{
for(c=0;c<tabPO->ncol;c++)
{
GetTableValI(tabPO,r,c,&vD);
SetTableValPrecisionI(tabPO, vD, &vD);
SetTableValI(tabPO,r,c,vD);
}
}
DB_TAB DB_PrI("<< SetTablePrecisionI %p TRUE\n",tabPO);
return(TRUE);
}
void max_abs_val_reassign(int *x, int *y, int *z)
{
int a = *x, b = *y, c = *z;
if (ABS_VAL(a) > ABS_VAL(b)) {
if (ABS_VAL(a) > ABS_VAL(c))
a > 0 ? (*x = REASSIGN_ABS_VAL) : (*x = -1 * REASSIGN_ABS_VAL);
else
c > 0 ? (*z = REASSIGN_ABS_VAL) : (*z = -1 * REASSIGN_ABS_VAL);
} else {
if (ABS_VAL(b) > ABS_VAL(c))
b > 0 ? (*y = REASSIGN_ABS_VAL) : (*y = -1 * REASSIGN_ABS_VAL);
else
c > 0 ? (*z = REASSIGN_ABS_VAL) : (*z = -1 * REASSIGN_ABS_VAL);
}
}
// 125kHzで割り込み
void CMT0_CMT_INT(void)
{
static int pwmcounter;
static int prescaler;
int port, ch;
CMT0Flag_Clear();
for(port = 0; port < 2; port++)
{
for(ch = 0; ch < 8; ch++)
{
if(motor_status[port].isEnable_TCprotection & (1<<ch))
{
if(motor_status[port].deadtime_cnt[ch])
{
motor_status[port].deadtime_cnt[ch]--;
}
else
{
motor_status[port].dir_word |= DUTY_TO_DIR(motor_status[port].duty[ch])<<(ch*2);
motor_status[port].duty[ch] = ABS_VAL(motor_status[port].duty[ch]);
}
}
else motor_status[port].duty[ch] = ABS_VAL(motor_status[port].duty[ch]);
}
}
if(!pwmcounter)
{
PC.DR.WORD = (PC.DR.WORD & (~motor_status[0].isEnable_PWM)) |
(motor_status[0].dir_word & motor_status[0].isEnable_PWM);
PD.DR.WORD.L = (PD.DR.WORD.L & (~motor_status[1].isEnable_PWM)) |
(motor_status[1].dir_word & motor_status[1].isEnable_PWM);
}
else
{
if(motor_status[0].duty[0] == pwmcounter) PC.DR.BYTE.L &= 0xFC;
if(motor_status[0].duty[1] == pwmcounter) PC.DR.BYTE.L &= 0xF3;
if(motor_status[0].duty[2] == pwmcounter) PC.DR.BYTE.L &= 0xCF;
if(motor_status[0].duty[3] == pwmcounter) PC.DR.BYTE.L &= 0x3F;
if(motor_status[0].duty[4] == pwmcounter) PC.DR.BYTE.H &= 0xFC;
if(motor_status[0].duty[5] == pwmcounter) PC.DR.BYTE.H &= 0xF3;
if(motor_status[0].duty[6] == pwmcounter) PC.DR.BYTE.H &= 0xCF;
if(motor_status[0].duty[7] == pwmcounter) PC.DR.BYTE.H &= 0x3F;
if(motor_status[1].duty[0] == pwmcounter) PD.DR.BYTE.LL &= 0xFC;
if(motor_status[1].duty[1] == pwmcounter) PD.DR.BYTE.LL &= 0xF3;
if(motor_status[1].duty[2] == pwmcounter) PD.DR.BYTE.LL &= 0xCF;
if(motor_status[1].duty[3] == pwmcounter) PD.DR.BYTE.LL &= 0x3F;
if(motor_status[1].duty[4] == pwmcounter) PD.DR.BYTE.LH &= 0xFC;
if(motor_status[1].duty[5] == pwmcounter) PD.DR.BYTE.LH &= 0xF3;
if(motor_status[1].duty[6] == pwmcounter) PD.DR.BYTE.LH &= 0xCF;
if(motor_status[1].duty[7] == pwmcounter) PD.DR.BYTE.LH &= 0x3F;
}
/* 1msに1回割り込まれる */
if(!prescaler)
{
systemTimer_ms++;
if(timingDelay_ms) timingDelay_ms--;
}
if(++pwmcounter == 100) pwmcounter = 0;
if(++prescaler == 125) prescaler = 0;
set_imask(0);
}
int NumI(int argc, char **argv)
{
char bufS[100], chS[100], *cPC;
int one,op,p,eform;
double r1R, r2R, anR;
if(argc < 2)
{
NumUse(); return(FALSE);
}
op = ParseNumOpI(argv[1],argc-2);
if(op == 0)
{
NumUse(); return(FALSE);
}
eform = FALSE;
/***
* Get arguments
*/
anR = BAD_R;
switch(op)
{
/***
* 2 mand; 3 optional args
*/
case OP_ADD:
case OP_SUB:
case OP_MUL:
case OP_DIV:
case OP_MOD:
case OP_MIN:
case OP_MAX:
r1R = r2R = BAD_R;
if(argc > 4)
{
p = BAD_I;
sprintf(bufS,"%s %s",argv[2],argv[3]);
sscanf(bufS,"%lf %lf",&r1R,&r2R);
/***
* Precision; print E notation?
*/
sprintf(chS,"%s",argv[4]);
cPC = chS;
if(toupper(INT(*cPC))=='E')
{
eform = TRUE;
while((*cPC)&&(!isdigit(INT(*cPC))))
{ cPC++; }
sscanf(cPC,"%d",&p);
}
else
{
sscanf(argv[4],"%d",&p);
}
if(BAD_REAL(r1R) || BAD_REAL(r2R) || BAD_INT(p))
{
printf("Bad numerical arguments: %s %s %s\n",
argv[2],argv[3],argv[4]);
NumUse(); return(FALSE);
}
}
else
{
p = 0;
sprintf(bufS,"%s %s",argv[2],argv[3]);
sscanf(bufS,"%lf %lf",&r1R,&r2R);
if(BAD_REAL(r1R) || BAD_REAL(r2R))
{
printf("Bad numerical arguments: %s %s\n",
argv[2],argv[3]);
NumUse(); return(FALSE);
}
}
if((op == OP_DIV)&&(ABS_VAL(r2R)<MIN_SIZE))
{
printf("Divisor too small: %s\n",argv[4]);
NumUse(); return(FALSE);
}
if(op == OP_MOD)
{
p = 0;
}
break;
/***
* 1 mand; 2 optional args
*/
case OP_LOG:
case OP_LN:
case OP_LG2:
case OP_PW2:
case OP_ALOG:
case OP_EXP:
case OP_SIN:
case OP_COS:
case OP_TAN:
case OP_SQRT:
case OP_FMT:
r1R = BAD_R;
if(argc > 3)
{
p = BAD_I;
sprintf(bufS,"%s",argv[2]);
sscanf(bufS,"%lf",&r1R);
/***
* Precision; print E notation?
*/
sprintf(chS,"%s",argv[3]);
cPC = chS;
if(toupper(INT(*cPC)) == 'E')
{
eform = TRUE;
while((*cPC)&&(!isdigit(INT(*cPC))))
{ cPC++; }
sscanf(cPC,"%d",&p);
}
else
{
sscanf(argv[3],"%d",&p);
}
if(BAD_REAL(r1R) || BAD_INT(p))
{
printf("Bad numerical arguments: %s %s\n",
argv[2],argv[3]);
NumUse(); return(FALSE);
}
}
else
{
p = 0;
sprintf(bufS,"%s",argv[2]);
sscanf(bufS,"%lf",&r1R);
if(BAD_REAL(r1R))
{
printf("Bad numerical arguments: %s\n", argv[2]);
NumUse(); return(FALSE);
}
}
if( ((op==OP_LN)||(op==OP_LOG)||(op==OP_LG2)) && (r1R<MIN_LOG) )
{
printf("Argument too small / negative: %s\n",argv[2]);
NumUse(); return(FALSE);
}
break;
}
/****
* Do the work
*/
one = FALSE;
switch(op)
{
case OP_ADD: anR = r1R + r2R; one++; break;
case OP_SUB: anR = r1R - r2R; one++; break;
case OP_MUL: anR = r1R * r2R; one++; break;
case OP_DIV: anR = r1R / r2R; one++; break;
case OP_MOD: anR = INT(r1R) % INT(r2R); one++; break;
case OP_LOG: anR = log(r1R) / LOG_E_TEN_R; one++; break;
case OP_LN: anR = log(r1R); one++; break;
case OP_LG2: anR = log(r1R) / LOG_E_TWO_R; one++; break;
case OP_ALOG: anR = exp(r1R * LOG_E_TEN_R); one++; break;
case OP_EXP: anR = exp(r1R); one++; break;
case OP_PW2: anR = exp(r1R * LOG_E_TWO_R); one++; break;
case OP_SIN: anR = SIN_R(r1R); one++; break;
case OP_COS: anR = COS_R(r1R); one++; break;
case OP_TAN: anR = TAN_R(r1R); one++; break;
case OP_SQRT: anR = SQRT_R(r1R); one++; break;
case OP_MIN: anR = MIN_NUM(r1R,r2R); one++; break;
case OP_MAX: anR = MAX_NUM(r1R,r2R); one++; break;
case OP_FMT:
HandleFormatString(r1R,NULL);
break;
}
if(one)
{
if(p > 0)
{
if(eform)
sprintf(bufS,"%%1.%de\n",p);
else
sprintf(bufS,"%%1.%df\n",p);
printf(bufS,anR);
}
else
{
printf("%d\n",ROUND(anR));
}
}
return(TRUE);
}
