Gan likun / yezhong_main_controller_copy1

111

Dependencies:   yezhong_main_controller_copy mbed1-dev

DATA_BOARD/data_board.cpp@6:902ba9999d6c, 2022-01-11 (annotated)

Committer:
yezhong
Date:
Tue Jan 11 13:34:14 2022 +0000
Revision:
6:902ba9999d6c
Parent:
5:2503c88a564f
Child:
7:d1b09098579b
11

Who changed what in which revision?

UserRevisionLine numberNew contents of line
panzhan 0:d80c66cb1b3a 1 #include "mbed.h"
panzhan 0:d80c66cb1b3a 2 #include "data_board.h"
panzhan 0:d80c66cb1b3a 3 #include "data_pc.h"
panzhan 0:d80c66cb1b3a 4
panzhan 0:d80c66cb1b3a 5 Serial foot(PC_10, PC_11); // tx, rx U3
panzhan 0:d80c66cb1b3a 6
yezhong 6:902ba9999d6c 7
panzhan 0:d80c66cb1b3a 8
panzhan 0:d80c66cb1b3a 9 unsigned int flag_485_A = 0, flag_485_B = 0;
panzhan 0:d80c66cb1b3a 10 unsigned int S485num = 0;
panzhan 0:d80c66cb1b3a 11
panzhan 0:d80c66cb1b3a 12 uint16_t S485get[10] = {0};
panzhan 0:d80c66cb1b3a 13 uint16_t S485use[10] = {0};
panzhan 0:d80c66cb1b3a 14
panzhan 0:d80c66cb1b3a 15 ////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 16 // Gait_now 当前相位 Gait_per_now 当前步态时刻 Gait_change 步态改变标志位
panzhan 0:d80c66cb1b3a 17 // Gait_num_valid 步数统计 time_portion_now 当前相位时间段
panzhan 0:d80c66cb1b3a 18 ////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 19
panzhan 0:d80c66cb1b3a 20 unsigned int Gait_num_valid = 0, Gait_now = 0, Gait_per_now = 0, Gait_cycle_now = 0, time_portion_now=0;
panzhan 0:d80c66cb1b3a 21 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;
panzhan 0:d80c66cb1b3a 22 float COP_Y = 0.0f, COP_X = 0.0f;
panzhan 0:d80c66cb1b3a 23
panzhan 0:d80c66cb1b3a 24 unsigned int Gait_num_valid_0before = 0, Gait_now_0before = 0;
panzhan 0:d80c66cb1b3a 25 float Gait_per_now_0before = 0.0f, COP_Y_0before = 0.0f, COP_X_0before = 0.0f, Gait_cycle_now_0before = 0.0f;
panzhan 0:d80c66cb1b3a 26
panzhan 0:d80c66cb1b3a 27 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;
panzhan 0:d80c66cb1b3a 28 float COP_Y_real = 0.0f, COP_X_real = 0.0f;
panzhan 0:d80c66cb1b3a 29
panzhan 0:d80c66cb1b3a 30
panzhan 0:d80c66cb1b3a 31 void gait_decode()
panzhan 0:d80c66cb1b3a 32 {
panzhan 0:d80c66cb1b3a 33 Gait_now = S485use[0];
panzhan 0:d80c66cb1b3a 34 Gait_per_now = S485use[1]*100 + S485use[2]*10 + S485use[3];
panzhan 0:d80c66cb1b3a 35 time_portion_now = S485use[4]*100000 + S485use[5]*10000 + S485use[6]*1000 + S485use[7]*100 + S485use[8]*10 + S485use[9];
panzhan 0:d80c66cb1b3a 36
panzhan 0:d80c66cb1b3a 37
panzhan 0:d80c66cb1b3a 38 Gait_num_valid_real = Gait_num_valid;
panzhan 0:d80c66cb1b3a 39 Gait_now_real = Gait_now;
panzhan 0:d80c66cb1b3a 40 Gait_per_now_real = Gait_per_now;
panzhan 0:d80c66cb1b3a 41 COP_Y_real = COP_Y;
panzhan 0:d80c66cb1b3a 42 COP_X_real = COP_X;
panzhan 0:d80c66cb1b3a 43 Gait_cycle_now_real = Gait_cycle_now;
panzhan 0:d80c66cb1b3a 44 time_portion_now_real=time_portion_now;
panzhan 0:d80c66cb1b3a 45
panzhan 0:d80c66cb1b3a 46 pc.printf("%01d---%03d---%06d\n",Gait_now_real,Gait_per_now_real,time_portion_now_real);
panzhan 0:d80c66cb1b3a 47 }
panzhan 0:d80c66cb1b3a 48
panzhan 0:d80c66cb1b3a 49 void gait_clear()
panzhan 0:d80c66cb1b3a 50 {
panzhan 0:d80c66cb1b3a 51 for(int i = 0; i < 10; i++){
panzhan 0:d80c66cb1b3a 52 S485use[i] = 0;
panzhan 0:d80c66cb1b3a 53 }
panzhan 0:d80c66cb1b3a 54 }
panzhan 0:d80c66cb1b3a 55
panzhan 0:d80c66cb1b3a 56
panzhan 0:d80c66cb1b3a 57 void serial_board_isr(void)
panzhan 0:d80c66cb1b3a 58 {
panzhan 0:d80c66cb1b3a 59 // pc.printf("begin\n");
panzhan 0:d80c66cb1b3a 60 while(foot.readable())
panzhan 0:d80c66cb1b3a 61 {
panzhan 0:d80c66cb1b3a 62 uint16_t c = foot.getc();
panzhan 0:d80c66cb1b3a 63 if(c == 'A')
panzhan 0:d80c66cb1b3a 64 {
panzhan 0:d80c66cb1b3a 65 flag_485_A = 1;
panzhan 0:d80c66cb1b3a 66
panzhan 0:d80c66cb1b3a 67 flag_485_B = 0;
panzhan 0:d80c66cb1b3a 68 S485num = 0;
panzhan 0:d80c66cb1b3a 69 for(unsigned int i = 0; i < 10; i++)
panzhan 0:d80c66cb1b3a 70 {
panzhan 0:d80c66cb1b3a 71 S485get[i] = 0;
panzhan 0:d80c66cb1b3a 72 }
panzhan 0:d80c66cb1b3a 73
panzhan 0:d80c66cb1b3a 74 break;
panzhan 0:d80c66cb1b3a 75 }
panzhan 0:d80c66cb1b3a 76 if(c == 'B')
panzhan 0:d80c66cb1b3a 77 {
panzhan 0:d80c66cb1b3a 78 flag_485_B = 1;
panzhan 0:d80c66cb1b3a 79 }
panzhan 0:d80c66cb1b3a 80
panzhan 0:d80c66cb1b3a 81 if(flag_485_A == 1)
panzhan 0:d80c66cb1b3a 82 {
panzhan 0:d80c66cb1b3a 83 if((flag_485_B != 1) && (S485num < 10))
panzhan 0:d80c66cb1b3a 84 {
panzhan 0:d80c66cb1b3a 85 S485get[S485num] = c;
panzhan 0:d80c66cb1b3a 86 }
panzhan 0:d80c66cb1b3a 87
panzhan 0:d80c66cb1b3a 88 S485num++;
panzhan 0:d80c66cb1b3a 89
panzhan 0:d80c66cb1b3a 90 if((flag_485_B == 1) && (S485num != 11))
panzhan 0:d80c66cb1b3a 91 {
panzhan 0:d80c66cb1b3a 92 flag_485_A = 0;
panzhan 0:d80c66cb1b3a 93 flag_485_B = 0;
panzhan 0:d80c66cb1b3a 94 S485num = 0;
panzhan 0:d80c66cb1b3a 95 }
panzhan 0:d80c66cb1b3a 96
panzhan 0:d80c66cb1b3a 97 if((flag_485_B == 1) && (S485num == 11))
panzhan 0:d80c66cb1b3a 98 {
panzhan 0:d80c66cb1b3a 99 flag_485_A = 0;
panzhan 0:d80c66cb1b3a 100 flag_485_B = 0;
panzhan 0:d80c66cb1b3a 101 S485num = 0;
panzhan 0:d80c66cb1b3a 102
panzhan 0:d80c66cb1b3a 103 for(unsigned int i = 0; i < 10; i++)
panzhan 0:d80c66cb1b3a 104 {
panzhan 0:d80c66cb1b3a 105 S485use[i] = S485get[i] - '0';
panzhan 0:d80c66cb1b3a 106 }
panzhan 0:d80c66cb1b3a 107
panzhan 0:d80c66cb1b3a 108 gait_decode();
yezhong 5:2503c88a564f 109 // pc.printf("OK\n");
panzhan 0:d80c66cb1b3a 110 gait_clear();
panzhan 0:d80c66cb1b3a 111 }
panzhan 0:d80c66cb1b3a 112 }
panzhan 0:d80c66cb1b3a 113 }
yezhong 5:2503c88a564f 114 }
yezhong 5:2503c88a564f 115
yezhong 6:902ba9999d6c 116
yezhong 6:902ba9999d6c 117
yezhong 6:902ba9999d6c 118
yezhong 6:902ba9999d6c 119
yezhong 6:902ba9999d6c 120
yezhong 5:2503c88a564f 121 AnalogIn LaLi_pf(PC_1);
yezhong 5:2503c88a564f 122 AnalogIn LaLi_df(PC_2);
yezhong 5:2503c88a564f 123 AnalogIn LaLi_df1(PC_3);
yezhong 5:2503c88a564f 124
yezhong 5:2503c88a564f 125 float La_pf_real = 0.0f, La_df_real = 0.0f,La_df1_real = 0.0f;
yezhong 5:2503c88a564f 126 float Ffilter_pf[10 + 2] = {0.0f};
yezhong 5:2503c88a564f 127 float pf_filter = 0.0f;
yezhong 5:2503c88a564f 128 float F_pf = 0.0f;
yezhong 5:2503c88a564f 129
yezhong 5:2503c88a564f 130
yezhong 5:2503c88a564f 131