data_board.cpp

00001 #include "mbed.h"
00002 #include "data_board.h"
00003 #include "data_pc.h"
00004 
00005 Serial foot(PC_10, PC_11);              // tx, rx U3
00006 
00007   
00008 
00009 unsigned int flag_485_A = 0, flag_485_B = 0;
00010 unsigned int S485num = 0;
00011 
00012 uint16_t S485get[10] = {0};                                                    
00013 uint16_t S485use[10] = {0};
00014 
00015 ////////////////////////////////////////////////////////////////////////////////
00016 //   Gait_now           当前相位          Gait_per_now  当前步态时刻      Gait_change 步态改变标志位    
00017 //   Gait_num_valid     步数统计          time_portion_now    当前相位时间段
00018 ////////////////////////////////////////////////////////////////////////////////
00019 
00020 unsigned int Gait_num_valid = 0, Gait_now = 0, Gait_per_now = 0, Gait_cycle_now = 0, time_portion_now=0;
00021 uint16_t Gait_per_now_int = 0, COP_Y_int = 0, COP_X_int = 0, Gait_cycle_now_int = 0, time_portion_now_int=0;
00022 float COP_Y = 0.0f, COP_X = 0.0f;
00023 
00024 unsigned int Gait_num_valid_0before = 0, Gait_now_0before = 0;
00025 float Gait_per_now_0before = 0.0f, COP_Y_0before = 0.0f, COP_X_0before = 0.0f, Gait_cycle_now_0before = 0.0f;
00026 
00027 unsigned int Gait_num_valid_real = 0, Gait_now_real = 0, Gait_per_now_real = 0, Gait_cycle_now_real = 0.0f, time_portion_now_real=0.0f;
00028 float COP_Y_real = 0.0f, COP_X_real = 0.0f;
00029 
00030 
00031 void gait_decode()
00032 {
00033     Gait_now = S485use[0];
00034     Gait_per_now = S485use[1]*100 + S485use[2]*10 + S485use[3];
00035     time_portion_now = S485use[4]*100000 + S485use[5]*10000 + S485use[6]*1000 + S485use[7]*100 + S485use[8]*10 + S485use[9];
00036               
00037         
00038     Gait_num_valid_real = Gait_num_valid;
00039     Gait_now_real = Gait_now;
00040     Gait_per_now_real = Gait_per_now;
00041     COP_Y_real = COP_Y;
00042     COP_X_real = COP_X;
00043     Gait_cycle_now_real = Gait_cycle_now;
00044     time_portion_now_real=time_portion_now;
00045     
00046     pc.printf("%01d---%03d---%06d\n",Gait_now_real,Gait_per_now_real,time_portion_now_real);
00047 }
00048 
00049 void gait_clear()
00050 {
00051     for(int i = 0; i < 10; i++){
00052         S485use[i] = 0;
00053     }
00054 }
00055 
00056 
00057 void serial_board_isr(void)
00058 {
00059 //    pc.printf("begin\n");
00060     while(foot.readable())
00061     {       
00062         uint16_t c = foot.getc();
00063         if(c == 'A')
00064         {
00065             flag_485_A = 1;
00066             
00067             flag_485_B = 0;
00068             S485num = 0;
00069             for(unsigned int i = 0; i < 10; i++)
00070             {
00071                 S485get[i] = 0;      
00072             }
00073             
00074             break;  
00075         }
00076         if(c == 'B')
00077         {
00078             flag_485_B = 1;
00079         }
00080         
00081         if(flag_485_A == 1)
00082         {
00083             if((flag_485_B != 1) && (S485num < 10))
00084             {
00085                 S485get[S485num] = c;    
00086             }
00087             
00088             S485num++;
00089             
00090             if((flag_485_B == 1) && (S485num != 11))
00091             {    
00092                 flag_485_A = 0;
00093                 flag_485_B = 0;
00094                 S485num    = 0;     
00095             }
00096             
00097             if((flag_485_B == 1) && (S485num == 11))
00098             {
00099                 flag_485_A = 0;
00100                 flag_485_B = 0;
00101                 S485num    = 0;
00102                 
00103                 for(unsigned int i = 0; i < 10; i++)
00104                 {
00105                     S485use[i] = S485get[i] - '0';
00106                 }
00107                 
00108                 gait_decode();
00109                // pc.printf("OK\n");
00110                 gait_clear(); 
00111             } 
00112         }
00113     }
00114 }
00115 
00116  
00117 
00118 
00119 
00120 
00121 AnalogIn    LaLi_pf(PC_1);
00122 AnalogIn    LaLi_df(PC_2);
00123 AnalogIn    LaLi_df1(PC_3);
00124 
00125 float La_pf_real = 0.0f, La_df_real = 0.0f,La_df1_real = 0.0f;
00126 float Ffilter_pf[10 + 2] = {0.0f};
00127 float pf_filter = 0.0f;
00128 float F_pf = 0.0f;
00129 
00130 
00131