File content as of revision 1:15dd461fbc2a:

/*
This is a program for globlelized controller of whole DOGO
F746ZG
*/

#include "mbed.h"
#include "LSM9DS0_SH.h"
#define DEBUG       0

#define pi          3.141592f
#define d2r         0.01745329f

#define Rms         5000            //TT rate
#define dt          0.005f
#define Task_1_NN   9
#define Task_2_NN   5
#define Task_4_NN   9

//Structure
#define Buff_size   16              //Serial Buffer

//Initial Position
#define init_leg_Angle1 0
#define init_leg_length 0.23f
#define init_leg_Angle2 0

#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))

//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡GPIO registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
//╔═════════════════╗
//║    Structure    ║
//╚═════════════════╝
DigitalOut  LD1(PB_0);              //detection
DigitalOut  LD2(PB_7);               //detection
DigitalOut  LD3(PB_14);              //detection
InterruptIn button(USER_BUTTON);    //Button press;

//╔═════════════════╗
//║     Serial      ║
//╚═════════════════╝
Serial      Debug(PD_8, PD_9);      //Serial_3 reg(TX RX) USB port

Serial      LF_Cmd(PD_5, PD_6);     //Serial_2
DigitalOut  LF_CS(PD_7);

Serial      LH_Cmd(PC_12, PD_2);    //Serial_5
DigitalOut  LH_CS(PG_2);

Serial      RF_Cmd(PG_14, PC_7);    //Serial_6
DigitalOut  RF_CS(PG_9);

Serial      RH_Cmd(PE_8, PE_7);     //Serial_7
DigitalOut  RH_CS(PE_10);

//╔═════════════════╗
//║     IMU_SPI     ║
//╚═════════════════╝
DigitalOut  SPI_CSG(PD_14,1);       //low for GYRO enable
DigitalOut  SPI_CSXM(PD_15,1);      //low for ACC/MAG enable
SPI spi(PA_7, PA_6, PA_5);          //MOSI MISO SCLK
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of GPIO registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Varible registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
//╔═════════════════╗
//║    Structure    ║
//╚═════════════════╝
Ticker  TT;                         //call a timer
uint8_t Task_1_count = 0;           //1st prior task count
uint8_t Task_2_count = 0;           //2nd prior task count
uint8_t Task_3_count = 0;           //3nd prior task count
uint8_t Task_4_count = 0;           //24nd prior task count
uint8_t Flag_1 = 0;                 //1st prior task flag
uint8_t Flag_2 = 0;                 //2nd prior task flag
uint8_t Flag_3 = 0;                 //3nd prior task flag
uint8_t Flag_4 = 0;                 //4nd prior task flag

//╔═════════════════╗
//║   I/O Serial    ║
//╚═════════════════╝
uint8_t SerBuff[Buff_size];
uint8_t Recieve_index = 0;

//╔═════════════════╗
//║   Leg Command   ║
//╚═════════════════╝

float   LF_q_0_E[3] = {            
    init_leg_Angle1, init_leg_length, init_leg_Angle2
};
float   LH_q_0_E[3] = {            
    init_leg_Angle1, init_leg_length, init_leg_Angle2
};
float   RF_q_0_E[3] = {            
    init_leg_Angle1, init_leg_length, init_leg_Angle2
};
float   RH_q_0_E[3] = {            
    init_leg_Angle1, init_leg_length, init_leg_Angle2
};

uint8_t LF_Cmd_Hex[3] = {0x00, 0x00, 0x00};
uint8_t LH_Cmd_Hex[3] = {0x00, 0x00, 0x00};
uint8_t RF_Cmd_Hex[3] = {0x00, 0x00, 0x00};
uint8_t RH_Cmd_Hex[3] = {0x00, 0x00, 0x00};

//╔═════════════════╗
//║     IMU_SPI     ║
//╚═════════════════╝
short low_byte = 0x00;              //buffer
short high_byte = 0x00;
short Buff = 0x00;
float Wx = 0.0;
float Wy = 0.0;
float Wz = 0.0;
float Ax = 0.0;
float Ay = 0.0;
float Az = 0.0;
float gDIR[1][3] = {                //g vector's direction , required unitconstrain
    {0.0,0.0,0.0},                  //X Y Z
};
float Bet = 0.002;                  //confidence of Acc data
float gUnity = 0.0;
float Gdx = 0.0;
float Gdy = 0.0;
float Gdz = 0.0;
float Adx = 0.0;
float Ady = 0.0;
float Adz = 0.0;
float Ele_phy = 0.0;                //estimation of top plate attitide
float Til_phy = 0.0;
float Ele_phy_int = 0.0;
float Til_phy_int = 0.0;
float Ele_control = 0.0;
float Til_control = 0.0;

//╔═════════════════╗
//║     Balance     ║
//╚═════════════════╝
float pitchU[3] = {0, 0, 0};
float pitchE[3] = {0, 0, 0};
float rollU = 0;

//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Function registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void    init_TIMER();           //set TT_main() rate
void    TT_main();              //timebase function rated by TT

void    init_IMU();             //initialize IMU
void    init_GXdrift();          //read GXdrift at start up
void    read_IMU();             //read IMU data give raw data
void    state_update();         //estimation of new attitude

float   lpf(float input, float output_old, float frequency);    //lpf discrete

void    pressed();
void    Balance(); 

void    Rx_irq();
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡main funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
int main()
{
    init_IMU();                 //initialize IMU
    init_GXdrift();              //read GXdrift at start up

    Debug.baud(115200);                     //set baud rate
    LF_Cmd.baud(115200);
    LH_Cmd.baud(115200);
    RF_Cmd.baud(115200);
    RH_Cmd.baud(115200);

    wait_ms(10);
    init_TIMER();                           //start TT_main

    Debug.attach(&Rx_irq,Serial::RxIrq);    //Start recieving message
    LF_CS = 1;

    wait_ms(200);
    
    button.fall(&pressed);                  //button pressed
        
    while(1) {                              //main() loop

//        if(Debug.readable()) {
//            SerBuff[Recieve_index] = Debug.getc();
//            Debug.putc(SerBuff[Recieve_index]);
//            Recieve_index = Recieve_index + 1;
//        }

//        if(device.readable()) {
//            pc.putc(device.getc());
//        }
        //Balance
        if(Flag_1 == 1) {             
            
            //Initial Position
            LF_q_0_E[1] = init_leg_length;
            LH_q_0_E[1] = init_leg_length;
            RF_q_0_E[1] = init_leg_length;
            RH_q_0_E[1] = init_leg_length;

            Balance();
            Flag_1 = 0;                 
        } 
        
        //Send Command
        if(Flag_4 == 1) {             
            
            LF_Cmd_Hex[0] = (LF_q_0_E[0]+pi/4)/(pi/2)*255;
            LF_Cmd_Hex[1] = ((LF_q_0_E[1]-0.1)/0.2)*255;
            LF_Cmd_Hex[2] = (LF_q_0_E[2]+pi/4)/(pi/2)*255;
            
            LH_Cmd_Hex[0] = (LH_q_0_E[0]+pi/4)/(pi/2)*255;
            LH_Cmd_Hex[1] = ((LH_q_0_E[1]-0.1)/0.2)*255;
            LH_Cmd_Hex[2] = (LH_q_0_E[2]+pi/4)/(pi/2)*255;
            
            RF_Cmd_Hex[0] = (RF_q_0_E[0]+pi/4)/(pi/2)*255;
            RF_Cmd_Hex[1] = ((RF_q_0_E[1]-0.1)/0.2)*255;
            RF_Cmd_Hex[2] = (RF_q_0_E[2]+pi/4)/(pi/2)*255;
            
            RH_Cmd_Hex[0] = (RH_q_0_E[0]+pi/4)/(pi/2)*255;
            RH_Cmd_Hex[1] = ((RH_q_0_E[1]-0.1)/0.2)*255;
            RH_Cmd_Hex[2] = (RH_q_0_E[2]+pi/4)/(pi/2)*255;
            
            //Debug.printf("%d, %d, %d\r", LH_Cmd_Hex[0], LH_Cmd_Hex[1], LH_Cmd_Hex[2]);
            //LED
            LD1 = 1;
            
            //Left Front Leg
            LF_CS = 0;
            wait_us(20);
            LF_Cmd.putc(LF_Cmd_Hex[0]);
            LF_Cmd.putc(LF_Cmd_Hex[1]);
            LF_Cmd.putc(LF_Cmd_Hex[2]);
            LF_Cmd.putc('Q');
            wait_us(180);
            LF_CS = 1;
            
            //Left Hind Leg
            LH_CS = 0;
            wait_us(20);
            LH_Cmd.putc(LH_Cmd_Hex[0]);
            LH_Cmd.putc(LH_Cmd_Hex[1]);
            LH_Cmd.putc(LH_Cmd_Hex[2]);
            LH_Cmd.putc('Q');
            wait_us(180);
            LH_CS = 1;
            
            //Right Front Leg
            RF_CS = 0;
            wait_us(20);
            RF_Cmd.putc(RF_Cmd_Hex[0]);
            RF_Cmd.putc(RF_Cmd_Hex[1]);
            RF_Cmd.putc(RF_Cmd_Hex[2]);
            RF_Cmd.putc('Q');
            wait_us(180);
            RF_CS = 1;
            
            //Right Hind Leg
            RH_CS = 0;
            wait_us(20);
            RH_Cmd.putc(RH_Cmd_Hex[0]);
            RH_Cmd.putc(RH_Cmd_Hex[1]);
            RH_Cmd.putc(RH_Cmd_Hex[2]);
            RH_Cmd.putc('Q');
            wait_us(180);
            RH_CS = 1;
            
            LD1 = 0;
            
            Flag_4 = 0;                 
        }     
    }
    
    
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of main funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Timebase funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void init_TIMER()                       //set TT_main{} rate
{
    TT.attach_us(&TT_main, Rms);
}
void TT_main()                          //interrupt function by TT
{
    read_IMU();                     //read IMU data give raw data
    state_update();                 //estimation of new attitude
    
    Task_1_count = Task_1_count + 1;
    if(Task_1_count > Task_1_NN) {
        Task_1_count = 0;               //Task triggering
        Flag_1 = 1;
    }
    
    Task_4_count = Task_4_count + 1;
    if(Task_4_count > Task_4_NN) {
        Task_4_count = 0;               //Task triggering
        Flag_4 = 1;
    }
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Timebase funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//

//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Button funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void    pressed(void)
{

}

void    released(void)
{
    
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of button funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//

//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Balance funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//

void Balance(void)
{
    //update pitch controller
    pitchE[0] = Ele_phy;
    pitchU[0] = 0.5*(0.0663*pitchU[1]+ 0.9337*pitchU[2]- 0.13635*(pitchE[0]+0.3535*pitchE[1]-0.6465*pitchE[2]));
    
    pitchE[2] = pitchE[1];
    pitchE[1] = pitchE[0];
    pitchU[2] = pitchU[1];
    pitchU[1] = pitchU[0];
    
    rollU = 0.05*Til_phy;
    
//    LF_q_0_E[0] =  Til_phy;
//    RF_q_0_E[0] =  -Til_phy;
//    LH_q_0_E[0] =  Til_phy;
//    RH_q_0_E[0] =  -Til_phy;
    
    LF_q_0_E[1] = LF_q_0_E[1] + pitchU[0] + rollU;
    RF_q_0_E[1] = RF_q_0_E[1] + pitchU[0] - rollU;
    LH_q_0_E[1] = LH_q_0_E[1] - pitchU[0] + rollU;
    RH_q_0_E[1] = RH_q_0_E[1] - pitchU[0] - rollU;
    
    LF_q_0_E[2] =  - Ele_phy;
    RF_q_0_E[2] =  - Ele_phy;
    LH_q_0_E[2] =  - Ele_phy;
    RH_q_0_E[2] =  - Ele_phy;
    
    Debug.printf("%.3f, %.3f\r", Til_phy, rollU);
}

//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Balance funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void init_IMU(void)                 //initialize
{
    //gloable config
    SPI_CSXM = 1;                   //high as init for disable SPI
    SPI_CSG = 1;
    spi.format(8, 3);               //byte width, spi mode
    spi.frequency(4000000);         //8MHz

    //for GYRO config
    SPI_CSG = 0;                    //start spi talking
    spi.write(CTRL_REG1_G);
    spi.write(0x9F);                //data rate 380 Hz/ cut off 25 Hz
    SPI_CSG = 1;                    //end spi talking

    SPI_CSG = 0;                    //start spi talking
    spi.write(CTRL_REG4_G);
    spi.write(0x10);                //Scle 500dps
    SPI_CSG = 1;                    //end spi talking

    //for ACC config
    SPI_CSXM = 0;                   //start spi talking
    spi.write(CTRL_REG1_XM);
    spi.write(0x87);                //data rate 400 Hz/ Enable
    SPI_CSXM = 1;                   //end spi talking

    SPI_CSXM = 0;                   //start spi talking
    spi.write(CTRL_REG2_XM);
    spi.write(0xC8);                //cut off 50 Hz/ Scale +-4g
    SPI_CSXM = 1;                   //end spi talking
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_G/Xdrift funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void init_GXdrift(void)              //initialize
{
    for(int i=0; i<1000; i++) {
        read_IMU();                 //note GXdrift = 0 at this moment
        gDIR[0][0] = gDIR[0][0] - Wx;
        gDIR[0][1] = gDIR[0][1] - Wy;
        gDIR[0][2] = gDIR[0][2] - Wz;
        wait_ms(2);
    }

    Gdx = gDIR[0][0] /1000.0f;
    Gdy = gDIR[0][1] /1000.0f;
    Gdz = gDIR[0][2] /1000.0f;
    //Debug.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz);

    for(int i=0; i<1000; i++) {
        read_IMU();                 //note GXdrift = 0 at this moment
        gDIR[0][0] = gDIR[0][0] - Ax;
        gDIR[0][1] = gDIR[0][1] - Ay;
        gDIR[0][2] = gDIR[0][2] - Az;
        wait_ms(2);
    }
    Adx = gDIR[0][0] /1000.0f;
    Ady = gDIR[0][1] /1000.0f;
    Adz = gDIR[0][2] /1000.0f - 1.0f;
        

//    pc.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz);
    gDIR[0][0] = 0;
    gDIR[0][1] = 0;
    gDIR[0][2] = -1;
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_GXdrift funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void read_IMU(void)                 //read IMU data give raw data
{
    //Wx
    SPI_CSG = 0;                    //start spi talking Wx
    spi.write(0xE8);                //read B11101000 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSG = 1;                    //end spi talking
//    Wx = Buff * Gpx + Gdx;
    Wx = lpf(Buff * Gpx + Gdx, Wx, 48.0f);
    //Wy
    SPI_CSG = 0;                    //start spi talking Wx
    spi.write(0xEA);                //read B11101010 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSG = 1;                    //end spi talking
//    Wy = Buff * Gpy + Gdy;
    Wy = lpf(Buff * Gpy + Gdy, Wy, 48.0f);
    //Wz
    SPI_CSG = 0;                    //start spi talking Wx
    spi.write(0xEC);                //read B11101100 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSG = 1;                    //end spi talking
//    Wz = Buff * Gpz + Gdz;
    Wz = lpf(Buff * Gpz + Gdz, Wz, 48.0f);
    //Ax
    SPI_CSXM = 0;                   //start spi talking Ax
    spi.write(0xE8);                //read B11101000 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSXM = 1;                   //end spi talking
    Ax = lpf(Buff*Apx + Adx, Ax, 13.0f);
    //Ay
    SPI_CSXM = 0;                   //start spi talking Ax
    spi.write(0xEA);                //read B11101010 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSXM = 1;                   //end spi talking
    Ay = lpf(Buff*Apy + Ady, Ay, 13.0f);
    //Az
    SPI_CSXM = 0;                   //start spi talking Ax
    spi.write(0xEC);                //read B11101100 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSXM = 1;                   //end spi talking
    Az = lpf(Buff*Apz + Adz, Az, 13.0f);
    //Debug.printf("%.4f,%.4f,%.4f\r", Ax, Ay, Az);
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓state_update funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void state_update(void)             //estimation of new attitude
{
    //predict
    gDIR[0][0] = gDIR[0][0] - (Wy*gDIR[0][2] - Wz*gDIR[0][1])*dt;
    gDIR[0][1] = gDIR[0][1] - (Wz*gDIR[0][0] - Wx*gDIR[0][2])*dt;
    gDIR[0][2] = gDIR[0][2] - (Wx*gDIR[0][1] - Wy*gDIR[0][0])*dt;

    //update
    gDIR[0][0] = (1-Bet) * gDIR[0][0] + Bet * Ax;
    gDIR[0][1] = (1-Bet) * gDIR[0][1] + Bet * Ay;
    gDIR[0][2] = (1-Bet) * gDIR[0][2] + Bet * Az;

    //nutralizing
    gUnity = sqrt( gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2] );
    for(int i=0; i<3; i++) {
        gDIR[0][i] =  gDIR[0][i] / gUnity;
    }

    //transfer
    Ele_phy = asin(gDIR[0][0]);
    Til_phy = asin(-gDIR[0][1] / cos(Ele_phy));
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//


//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
float lpf(float input, float output_old, float frequency)
{
    float output = 0;
    output = (output_old + frequency*dt*input) / (1 + frequency*dt);
    return output;
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Rx_irq funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void    Rx_irq(void)
{
    SerBuff[0] = Debug.getc();
    Debug.putc(SerBuff[0]);
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Rx_irq funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//