void decrypt(WORD *cipherTxt, WORD *plainTxt) {
WORD B = cipherTxt[1];
WORD A = cipherTxt[0];
int i;
for (i = r; i > 0; --i) {
B = ROTATE_R(B-S[2*i+1], A)^A;
A = ROTATE_R(A-S[2*i], B)^B;
}
plainTxt[1] = B - S[1];
plainTxt[0] = A - S[0];
}
void __attribute__((__interrupt__))_T1Interrupt(void){
IFS0bits.T1IF = 0;
TMR1 = 0;
nPOS1CNT = POS1CNTL + 0x10000*POS1CNTH; //convert 32bit
// nPOS1CNT = nPOS1CNT/4 + 0x30000000;
Vel1 = nPOS1CNT - nPOS1CNT_OLD;
nPOS1CNT_OLD = nPOS1CNT;
VEL3 = VEL2;
VEL2 = Vel1;
// Acceleration = (long long)nPOSCNT[0] - 2*(long long)nPOSCNT[1] + (long long)nPOSCNT[2];
// Acceleration = VEL1-VEL3;
// printf("%4x\n",POS1CNTL);
// printf("%04x %04x \n",POS1CNTH,POS1CNTL);
nDIFFERENCE = nTARGET - nPOS1CNT;
// Acceleration = (long long)nPOSCNT[0] - 2*(long long)nPOSCNT[1] + (long long)nPOSCNT[2];
// Acceleration = VEL1-VEL3;
if(-1<nDIFFERENCE && nDIFFERENCE<1){
//BRAKE();
//B_LED_OFF();R_LED_OFF();
FLAG_NR = STAT_B;
// INTE = 0;
}else{
nDUTY1 = 100*( (float)kp*nDIFFERENCE/nRESOLUTION ); //+ kd*nDERIVATION)
if(nDUTY1>=100){nDUTY1=100;}else if(nDUTY1<=-100){nDUTY1=-100;}
nDUTY1 = nDUTY1 - kd*Vel1;// nDUTY1 - kd*VEL1 ;
//if(SRbits.OV==1){R_LED_ON();while(1);}
if(nDIFFERENCE<0){
//R_LED_ON();B_LED_OFF();
if(FLAG_NR == STAT_R){INTE_R = INTE_R + 1;}
else{INTE_R = 0;}
nDUTY1 = nDUTY1 - ki*INTE_R;
//ROTATE_R(-1*nDUTY1 + ki*INTE_R );
FLAG_NR = STAT_R;
}else{
//R_LED_OFF();B_LED_ON();
if(FLAG_NR == STAT_N){INTE_N = INTE_N + 1;}
else{INTE_N = 0;}
nDUTY1 = nDUTY1 + ki*INTE_N;
//ROTATE_N(nDUTY1 + ki*INTE_N );
FLAG_NR = STAT_N;
}
if(nDUTY1>=100){nDUTY1=100;}else if(nDUTY1<=-100){nDUTY1=-100;}
if(nDUTY1<0){
ROTATE_R(-1*nDUTY1);
}else{
ROTATE_N(nDUTY1);
}
}
}
