File content as of revision 8:1ab9699af5ae:

#include "mbed.h"
#include "data_board.h"
#include "data_pc.h"

 

//Serial foot(PA_9, PA_10);              // tx, rx U1

//Serial zitai_foot(PA_0, PA_1);          // tx, rx U4
  
  
  Serial zitai_foot(PA_9, PA_10);
Timer ttt;

unsigned int zitaifoot_A = 0, zitaifoot_B = 0;
unsigned int zitaifoot_num = 0;

uint16_t zitaifoot_get[9] = {0};                                                    
uint16_t zitaifoot_use[9] = {0};
float zitaifoot_shou[3]= {0};
float foot_angle_x=0,foot_angle_y=0,foot_angle_z=0;


void serial_board_isr(void)
{
    //pc.printf("bbbbb\n");
    }


void zitaifoot_decode()
{
    ttt.start();
      ttt.read(); 
    float  b=ttt.read(); 
     for(int i = 0; i < 3; i++)
    {
    zitaifoot_shou[i]=(float)((zitaifoot_use[2*i+1]<<8)|zitaifoot_use[2*i])/32768.0f*180;
    }
    foot_angle_x=zitaifoot_shou[0];
    foot_angle_y=zitaifoot_shou[1];
    foot_angle_z=zitaifoot_shou[2];
    if (foot_angle_x >= 180)
        foot_angle_x -= 2 * 180;
    if (foot_angle_y >= 180)
        foot_angle_y -= 2 * 180;
    if (foot_angle_z >=180)
        foot_angle_z-= 2 * 180;
     pc.printf("%.3f  %.3f  %.3f  %.3f\r\n",foot_angle_x,foot_angle_y,foot_angle_z,b);
}

void zitaifoot_clear()
{
    for(int i = 0; i < 9; i++){
        zitaifoot_use[i] = 0;
    }
}




          
void serial_zitai_foot_isr(void)
{
    
    static uint16_t rx;
    static uint16_t rx_data[11];
    
    uint16_t RX_CHECKCODE;//校验码
    
    rx_data[rx] = zitai_foot.getc();
    
    if(rx_data[0] == 0x55){
        // pc.printf("OK\n");
        rx++;
    }
    
    if(rx >= 11){
        RX_CHECKCODE=(rx_data[0]+rx_data[1]+rx_data[2]+rx_data[3]+rx_data[4]+rx_data[5]+rx_data[6]+rx_data[7]+rx_data[8]+rx_data[9])&0xFF;
        if(rx_data[10] == RX_CHECKCODE){
               for(int i = 0; i<6; i++)
               {
                 zitaifoot_use[i]=rx_data[i+2];
                 }    
           //  pc.printf("%d %d %d %d %d %d %d %d %d %d %d\r\n", rx_data[0],rx_data[1], rx_data[2], rx_data[3], rx_data[4], rx_data[5], rx_data[6], rx_data[7], rx_data[8], rx_data[9], rx_data[10]);  
             zitaifoot_decode(); 
             zitaifoot_clear(); 
        } 
     rx = 0;   
    }
    
    //pc.printf("%d\n", rx_data[rx]);
}












AnalogIn    LaLi_pf(PC_1);
AnalogIn    LaLi_df(PC_2);
AnalogIn    LaLi_df1(PC_3);

float La_pf_real = 0.0f, La_df_real = 0.0f,La_df1_real = 0.0f;
float Ffilter_pf[10 + 2] = {0.0f};
float pf_filter = 0.0f;
float F_pf = 0.0f;