yezhong yezhong / yezhong_main_controller_copy-

1

Dependencies:   mcp2515 mbed-dev-f303

DATA_BOARD/data_board.cpp@4:2503c88a564f, 2022-01-11 (annotated)

Committer:
yezhong
Date:
Tue Jan 11 02:19:55 2022 +0000
Revision:
4:2503c88a564f
Parent:
0:d80c66cb1b3a
Child:
5:902ba9999d6c
1

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