yezhong yezhong / panzhan_main_controller_continuous_LX

11

Dependencies:   mbed-dev-f303

DATA_BOARD/data_board.cpp@0:dd5d4837292c, 2021-06-09 (annotated)

Committer:
panzhan
Date:
Wed Jun 09 01:41:45 2021 +0000
Revision:
0:dd5d4837292c
Child:
2:9418258519ea
continuous motion

Who changed what in which revision?

UserRevisionLine numberNew contents of line
panzhan 0:dd5d4837292c 1 #include "data_board.h"
panzhan 0:dd5d4837292c 2
panzhan 0:dd5d4837292c 3
panzhan 0:dd5d4837292c 4
panzhan 0:dd5d4837292c 5
panzhan 0:dd5d4837292c 6 Serial foot(PA_9, PA_10); // tx, rx U1
panzhan 0:dd5d4837292c 7
panzhan 0:dd5d4837292c 8 unsigned int flag_foot_A = 0, flag_foot_B = 0;
panzhan 0:dd5d4837292c 9 unsigned int Sfootnum = 0;
panzhan 0:dd5d4837292c 10
panzhan 0:dd5d4837292c 11 uint16_t Sfootget[10] = {0};
panzhan 0:dd5d4837292c 12 uint16_t Sfootuse[10] = {0};
panzhan 0:dd5d4837292c 13
panzhan 0:dd5d4837292c 14 ////////////////////////////////////////////////////////////////////////////////
panzhan 0:dd5d4837292c 15 // Gait_now 当前相位 Gait_per_now 当前步态时刻 Gait_change 步态改变标志位
panzhan 0:dd5d4837292c 16 // Gait_num_valid 步数统计 time_portion_now 当前相位时间段
panzhan 0:dd5d4837292c 17 ////////////////////////////////////////////////////////////////////////////////
panzhan 0:dd5d4837292c 18
panzhan 0:dd5d4837292c 19 unsigned int Gait_num_valid = 0, Gait_now = 0, Gait_per_now = 0, Gait_cycle_now = 0, time_portion_now=0;
panzhan 0:dd5d4837292c 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:dd5d4837292c 21
panzhan 0:dd5d4837292c 22
panzhan 0:dd5d4837292c 23 unsigned int Gait_num_valid_0before = 0, Gait_now_0before = 0;
panzhan 0:dd5d4837292c 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:dd5d4837292c 25
panzhan 0:dd5d4837292c 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:dd5d4837292c 27
panzhan 0:dd5d4837292c 28
panzhan 0:dd5d4837292c 29 void gait_decode()
panzhan 0:dd5d4837292c 30 {
panzhan 0:dd5d4837292c 31 Gait_now = Sfootuse[0];
panzhan 0:dd5d4837292c 32 Gait_per_now = Sfootuse[1]*100 + Sfootuse[2]*10 + Sfootuse[3];
panzhan 0:dd5d4837292c 33 time_portion_now = Sfootuse[4]*100000 + Sfootuse[5]*10000 + Sfootuse[6]*1000 + Sfootuse[7]*100 + Sfootuse[8]*10 + Sfootuse[9];
panzhan 0:dd5d4837292c 34
panzhan 0:dd5d4837292c 35
panzhan 0:dd5d4837292c 36 Gait_num_valid_real = Gait_num_valid;
panzhan 0:dd5d4837292c 37 Gait_now_real = Gait_now;
panzhan 0:dd5d4837292c 38 Gait_per_now_real = Gait_per_now;
panzhan 0:dd5d4837292c 39 Gait_cycle_now_real = Gait_cycle_now;
panzhan 0:dd5d4837292c 40 time_portion_now_real=time_portion_now;
panzhan 0:dd5d4837292c 41
panzhan 0:dd5d4837292c 42 command.printf("%01d---%03d---%06d\n",Gait_now_real,Gait_per_now_real,time_portion_now_real);
panzhan 0:dd5d4837292c 43 }
panzhan 0:dd5d4837292c 44
panzhan 0:dd5d4837292c 45 void gait_clear()
panzhan 0:dd5d4837292c 46 {
panzhan 0:dd5d4837292c 47 for(int i = 0; i < 10; i++){
panzhan 0:dd5d4837292c 48 Sfootuse[i] = 0;
panzhan 0:dd5d4837292c 49 }
panzhan 0:dd5d4837292c 50 }
panzhan 0:dd5d4837292c 51
panzhan 0:dd5d4837292c 52
panzhan 0:dd5d4837292c 53 void serial_board_isr(void)
panzhan 0:dd5d4837292c 54 {
panzhan 0:dd5d4837292c 55 // pc.printf("begin\n");
panzhan 0:dd5d4837292c 56 while(foot.readable())
panzhan 0:dd5d4837292c 57 {
panzhan 0:dd5d4837292c 58 uint16_t c = foot.getc();
panzhan 0:dd5d4837292c 59 if(c == 'A')
panzhan 0:dd5d4837292c 60 {
panzhan 0:dd5d4837292c 61 flag_foot_A = 1;
panzhan 0:dd5d4837292c 62
panzhan 0:dd5d4837292c 63 flag_foot_B = 0;
panzhan 0:dd5d4837292c 64 Sfootnum = 0;
panzhan 0:dd5d4837292c 65 for(unsigned int i = 0; i < 10; i++)
panzhan 0:dd5d4837292c 66 {
panzhan 0:dd5d4837292c 67 Sfootget[i] = 0;
panzhan 0:dd5d4837292c 68 }
panzhan 0:dd5d4837292c 69
panzhan 0:dd5d4837292c 70 break;
panzhan 0:dd5d4837292c 71 }
panzhan 0:dd5d4837292c 72 if(c == 'B')
panzhan 0:dd5d4837292c 73 {
panzhan 0:dd5d4837292c 74 flag_foot_B = 1;
panzhan 0:dd5d4837292c 75 }
panzhan 0:dd5d4837292c 76
panzhan 0:dd5d4837292c 77 if(flag_foot_A == 1)
panzhan 0:dd5d4837292c 78 {
panzhan 0:dd5d4837292c 79 if((flag_foot_B != 1) && (Sfootnum < 10))
panzhan 0:dd5d4837292c 80 {
panzhan 0:dd5d4837292c 81 Sfootget[Sfootnum] = c;
panzhan 0:dd5d4837292c 82 }
panzhan 0:dd5d4837292c 83
panzhan 0:dd5d4837292c 84 Sfootnum++;
panzhan 0:dd5d4837292c 85
panzhan 0:dd5d4837292c 86 if((flag_foot_B == 1) && (Sfootnum != 11))
panzhan 0:dd5d4837292c 87 {
panzhan 0:dd5d4837292c 88 flag_foot_A = 0;
panzhan 0:dd5d4837292c 89 flag_foot_B = 0;
panzhan 0:dd5d4837292c 90 Sfootnum = 0;
panzhan 0:dd5d4837292c 91 }
panzhan 0:dd5d4837292c 92
panzhan 0:dd5d4837292c 93 if((flag_foot_B == 1) && (Sfootnum == 11))
panzhan 0:dd5d4837292c 94 {
panzhan 0:dd5d4837292c 95 flag_foot_A = 0;
panzhan 0:dd5d4837292c 96 flag_foot_B = 0;
panzhan 0:dd5d4837292c 97 Sfootnum = 0;
panzhan 0:dd5d4837292c 98
panzhan 0:dd5d4837292c 99 for(unsigned int i = 0; i < 10; i++)
panzhan 0:dd5d4837292c 100 {
panzhan 0:dd5d4837292c 101 Sfootuse[i] = Sfootget[i] - '0';
panzhan 0:dd5d4837292c 102 }
panzhan 0:dd5d4837292c 103
panzhan 0:dd5d4837292c 104 gait_decode();
panzhan 0:dd5d4837292c 105 gait_clear();
panzhan 0:dd5d4837292c 106 }
panzhan 0:dd5d4837292c 107 }
panzhan 0:dd5d4837292c 108 }
panzhan 0:dd5d4837292c 109 }