math.asm

基于凌阳61和pwm电机实现的智能车总程序

//////////////////////////////////////////////////////////////////////////////
// Filename : Math.asm
// Function : Calculate LCD position
// Subroutine :
//
// 1. Name  : F_Calculate_Calibration
//    Input : Xlcd_1, Ylcd_1, Xlcd_2, Ylcd_2, Xtp_1, Ytp_1, Xtp_2, Ytp_2
//
// 2. Name  : F_Check_Calibration_OK
//    Input : Xtp_1, Ytp_1, Xlcd_1, Ylcd_1
//
// 3. Name  : F_Calculate_LCD_XY
//    Input : Xtp_1 and Ytp_1
//////////////////////////////////////////////////////////////////////////////
.public F_Calculate_Calibration;
.public F_Check_Calibration_OK;
.public F_Calculate_LCD_XY;

.external R_Mul;
.external R_Div;
.external Divider_X;
.external Ratio_X;
.external Offset_X_1;
.external Offset_X_2;
.external Divider_Y;
.external Ratio_Y;
.external Offset_Y_1;
.external Offset_Y_2;
.external Xlcd_1;
.external Xlcd_2;
.external Ylcd_1;
.external Ylcd_2;
.external Xtp_1;
.external Xtp_2;
.external Ytp_1;
.external Ytp_2;
.external Temp_Low;
.external Temp_High;

.CODE
///////////////////////////////////////////////////////////////////////////////
//Filename : Divder.asm
//Function : R1/R2
//Input    : R1, R2
//Output   : R3; Integer value
//Destroy  : All
///////////////////////////////////////////////////////////////////////////////
F_Divider:
         [R_Mul] = R1;
         [R_Div] = R2;

         R1 = [R_Mul];
         R2 = [R_Div];
         R4 = R1;
         R3 = 0;
         R5 = 0;
Loop_1:
         CMP  R4,R2;
         JB   Loop_2;
         R4 -= R2;
         R3 += 1;
         JMP  Loop_1;
Loop_2:
         R5 += 1;
         CMP  R5,8;                     //Scale : 1/(2^8) = 1/256
         JG   EndDivide;
         R3 = R3 LSL 1;
         R4 = R4 LSL 1;
         JMP Loop_1;
EndDivide:
         R3 = R3 LSR 4;                 //R3/(2^8)
         R3 = R3 LSR 4;
         RETF;

///////////////////////////////////////////////////////////////////////////////
//Filename : F_Calculate_Calibration
//Function : Calculate ratio and offset
//Input    : Xlcd_1, Ylcd_1, Xlcd_2, Ylcd_2, Xtp_1, Ytp_1, Xtp_2, Ytp_2
//Output   : Divider_X, Ratio_X, Offset_X_1. Offset_X_2
//           Divider_Y, Ratio_Y, Offset_Y_1, Offset_Y_2
//Destroy  : All
//
//Equation :
//
//      Xlcd = (Ratio_X * Xtp + Offset_X_1 - Offset_X_2)/Divider_X
//      Ylcd = (Ratio_Y * Ytp + Offset_Y_1 - Offset_Y_2)/Divider_Y
//
///////////////////////////////////////////////////////////////////////////////
F_Calculate_Calibration:
//Calculate (Xlcd_2 - Xlcd_1)
        R1 = [Xlcd_1];
        R2 = [Xlcd_2];
        R2 -= R1;                       //Xlcd_2-Xlcd_1
        [Ratio_X] = R2;                 //X ratio

//Calculate (Xlcd_1*Xtp_2)
        R2 = [Xtp_2];
        MR = R1*R2, ss;                 //Xlcd_1*Xtp_2
        [Offset_X_1] = R3;

//Calculate (Xtp_2 - Xtp_1)
        R3 = [Xtp_1];
        R2 -= R3;
        [Divider_X] = R2;               //X divider

//Caculate (Xlcd_2*Xtp_1)
        R1 = [Xlcd_2];
        MR = R1*R3, ss;                 //Xlcd_2*Xtp_1
        [Offset_X_2] = R3;


//Calculate (Ylcd_2 - Ylcd_1)
        R2 = [Ylcd_1];
        R1 = [Ylcd_2];
        R1 -= R2;
//        R2 -= R1;                       //Ylcd_2-Ylcd_1
        [Ratio_Y] = R1;                 //Y ratio

//Calculate (Ylcd_1*Ytp_2)
        R1 = [Ylcd_1];
        R2 = [Ytp_2];
        MR = R1*R2, ss;                 //Ylcd_1*Ytp_2
        [Offset_Y_1] = R3;

//Calculate (Ytp_2 - Ytp_1)
        R3 = [Ytp_1];
        R2 -= R3;
//        R3 -= R2;
        [Divider_Y] = R2;               //Y divider

//Caculate (Ylcd_2*Ytp_1)
        R3 = [Ytp_1];
        R1 = [Ylcd_2];
        MR = R1*R3, ss;                 //Ylcd_2*Ytp_1
        [Offset_Y_2] = R3;
        RETF;


///////////////////////////////////////////////////////////////////////////////
//Filename : F_Calculate_LCD_XY
//Function : Calculate Xlcd and Ylcd
//Input    : Xtp_1 and Ytp_1
//Output   : Xlcd_1 and Ylcd_1
//Destroy  : All
//Equation :
//
//      Xlcd = (Ratio_X * Xtp + Offset_X_1 - Offset_X_2)/Divider_X
//      Ylcd = (Ratio_Y * Ytp + Offset_Y_1 - Offset_Y_2)/Divider_Y
//
///////////////////////////////////////////////////////////////////////////////
F_Calculate_LCD_XY:
//Calculate Xlcd
        R1 = [Xtp_1];
        R2 = [Ratio_X];
        MR = R1*R2, ss;
        R1 = [Offset_X_1];
        R1 += R3;
        R2 = [Offset_X_2];
        R1 -= R2;
        JPL L_Result_Plus_X;
        R1 = 0x0000;                    //Xlcd is negative
L_Result_Plus_X:
        R2 = [Divider_X];
        CALL F_Divider;
        [Xlcd_1] = R3;

//Calculate Ylcd
        R1 = [Ytp_1];
        R2 = [Ratio_Y];
        MR = R1*R2, ss;
        R1 = [Offset_Y_1];
        R1 += R3;
        R2 = [Offset_Y_2];
        R1 -= R2;
        JPL L_Result_Plus_Y;
        R1 = 0x0000;                    //Ylcd is negative
L_Result_Plus_Y:
        R2 = [Divider_Y];
        CALL F_Divider;
        [Ylcd_1] = R3;

        RETF;

///////////////////////////////////////////////////////////////////////////////
//Filename : F_Check_Calibration_OK
//Function : Check the points of touch panel is OK
//Input    : Xtp_1, Ytp_1, Xlcd_1, Ylcd_1
//Output   : R1 = 0xFFFF : OK  ; R1 = 0x0000 : NG
//Destroy  : All
//Equation :
//
//      Xlcd = (Ratio_X * Xtp + Offset_X_1 - Offset_X_2)/Divider_X
//      Ylcd = (Ratio_Y * Ytp + Offset_Y_1 - Offset_Y_2)/Divider_Y
//
///////////////////////////////////////////////////////////////////////////////
F_Check_Calibration_OK:

        R1 = [Xlcd_1];                  //Store temporarily
        [Xlcd_2] = R1;
        R1 = [Ylcd_1];
        [Ylcd_2] = R1;

        CALL  F_Calculate_LCD_XY;
        R1 = [Xlcd_1];
        R2 = [Xlcd_2];
        R1 -= R2;
        JPL L_Check_under_5_points_X;
        CMP R1,0xFFFB;
        JPL L_Check_Y_Axis;
        Goto L_Calibration_NG;
L_Check_under_5_points_X:
        CMP R1,0x0005;
        JMI L_Check_Y_Axis;
L_Calibration_NG:
        R1 = 0x0000;
        RETF;

L_Check_Y_Axis:
        R1 = [Ylcd_1];
        R2 = [Ylcd_2];
        R1 -= R2;
        JPL L_Check_under_5_points_Y;
        CMP R1,0xFFFB;
        JPL L_Calibration_OK;
        Goto L_Calibration_NG;
L_Check_under_5_points_Y:
        CMP R1,0x0005;
        JMI L_Calibration_OK;
        Goto L_Calibration_NG;
L_Calibration_OK:
        R1 = 0xFFFF;
        RETF;