111

Dependencies:   yezhong_main_controller_copy mbed1-dev

DATA_BOARD/data_board.cpp

Committer:
yezhong
Date:
2022-01-11
Revision:
5:2503c88a564f
Parent:
0:d80c66cb1b3a
Child:
6:902ba9999d6c

File content as of revision 5:2503c88a564f:

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

Serial foot(PC_10, PC_11);              // tx, rx U3


unsigned int flag_485_A = 0, flag_485_B = 0;
unsigned int S485num = 0;

uint16_t S485get[10] = {0};                                                    
uint16_t S485use[10] = {0};

////////////////////////////////////////////////////////////////////////////////
//   Gait_now           当前相位          Gait_per_now  当前步态时刻      Gait_change 步态改变标志位    
//   Gait_num_valid     步数统计          time_portion_now    当前相位时间段
////////////////////////////////////////////////////////////////////////////////

unsigned int Gait_num_valid = 0, Gait_now = 0, Gait_per_now = 0, Gait_cycle_now = 0, time_portion_now=0;
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;
float COP_Y = 0.0f, COP_X = 0.0f;

unsigned int Gait_num_valid_0before = 0, Gait_now_0before = 0;
float Gait_per_now_0before = 0.0f, COP_Y_0before = 0.0f, COP_X_0before = 0.0f, Gait_cycle_now_0before = 0.0f;

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;
float COP_Y_real = 0.0f, COP_X_real = 0.0f;


void gait_decode()
{
    Gait_now = S485use[0];
    Gait_per_now = S485use[1]*100 + S485use[2]*10 + S485use[3];
    time_portion_now = S485use[4]*100000 + S485use[5]*10000 + S485use[6]*1000 + S485use[7]*100 + S485use[8]*10 + S485use[9];
              
        
    Gait_num_valid_real = Gait_num_valid;
    Gait_now_real = Gait_now;
    Gait_per_now_real = Gait_per_now;
    COP_Y_real = COP_Y;
    COP_X_real = COP_X;
    Gait_cycle_now_real = Gait_cycle_now;
    time_portion_now_real=time_portion_now;
    
    pc.printf("%01d---%03d---%06d\n",Gait_now_real,Gait_per_now_real,time_portion_now_real);
}

void gait_clear()
{
    for(int i = 0; i < 10; i++){
        S485use[i] = 0;
    }
}


void serial_board_isr(void)
{
//    pc.printf("begin\n");
    while(foot.readable())
    {       
        uint16_t c = foot.getc();
        if(c == 'A')
        {
            flag_485_A = 1;
            
            flag_485_B = 0;
            S485num = 0;
            for(unsigned int i = 0; i < 10; i++)
            {
                S485get[i] = 0;      
            }
            
            break;  
        }
        if(c == 'B')
        {
            flag_485_B = 1;
        }
        
        if(flag_485_A == 1)
        {
            if((flag_485_B != 1) && (S485num < 10))
            {
                S485get[S485num] = c;    
            }
            
            S485num++;
            
            if((flag_485_B == 1) && (S485num != 11))
            {    
                flag_485_A = 0;
                flag_485_B = 0;
                S485num    = 0;     
            }
            
            if((flag_485_B == 1) && (S485num == 11))
            {
                flag_485_A = 0;
                flag_485_B = 0;
                S485num    = 0;
                
                for(unsigned int i = 0; i < 10; i++)
                {
                    S485use[i] = S485get[i] - '0';
                }
                
                gait_decode();
               // pc.printf("OK\n");
                gait_clear(); 
            } 
        }
    }
}

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;