00
Dependencies: mcp2515 mbed-dev-f303
DATA_BOARD/data_board.cpp@8:1ab9699af5ae, 2022-03-22 (annotated)
- Committer:
- yezhong
- Date:
- Tue Mar 22 02:03:58 2022 +0000
- Revision:
- 8:1ab9699af5ae
- Parent:
- 7:d1b09098579b
1
Who changed what in which revision?
User | Revision | Line number | New 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 | |
yezhong | 7:d1b09098579b | 5 | |
panzhan | 0:d80c66cb1b3a | 6 | |
yezhong | 7:d1b09098579b | 7 | //Serial foot(PA_9, PA_10); // tx, rx U1 |
panzhan | 0:d80c66cb1b3a | 8 | |
yezhong | 7:d1b09098579b | 9 | //Serial zitai_foot(PA_0, PA_1); // tx, rx U4 |
yezhong | 7:d1b09098579b | 10 | |
yezhong | 7:d1b09098579b | 11 | |
yezhong | 7:d1b09098579b | 12 | Serial zitai_foot(PA_9, PA_10); |
yezhong | 7:d1b09098579b | 13 | Timer ttt; |
panzhan | 0:d80c66cb1b3a | 14 | |
yezhong | 7:d1b09098579b | 15 | unsigned int zitaifoot_A = 0, zitaifoot_B = 0; |
yezhong | 7:d1b09098579b | 16 | unsigned int zitaifoot_num = 0; |
panzhan | 0:d80c66cb1b3a | 17 | |
yezhong | 7:d1b09098579b | 18 | uint16_t zitaifoot_get[9] = {0}; |
yezhong | 7:d1b09098579b | 19 | uint16_t zitaifoot_use[9] = {0}; |
yezhong | 7:d1b09098579b | 20 | float zitaifoot_shou[3]= {0}; |
yezhong | 7:d1b09098579b | 21 | float foot_angle_x=0,foot_angle_y=0,foot_angle_z=0; |
panzhan | 0:d80c66cb1b3a | 22 | |
panzhan | 0:d80c66cb1b3a | 23 | |
panzhan | 0:d80c66cb1b3a | 24 | void serial_board_isr(void) |
panzhan | 0:d80c66cb1b3a | 25 | { |
yezhong | 7:d1b09098579b | 26 | //pc.printf("bbbbb\n"); |
yezhong | 7:d1b09098579b | 27 | } |
yezhong | 7:d1b09098579b | 28 | |
yezhong | 7:d1b09098579b | 29 | |
yezhong | 7:d1b09098579b | 30 | void zitaifoot_decode() |
yezhong | 7:d1b09098579b | 31 | { |
yezhong | 7:d1b09098579b | 32 | ttt.start(); |
yezhong | 7:d1b09098579b | 33 | ttt.read(); |
yezhong | 7:d1b09098579b | 34 | float b=ttt.read(); |
yezhong | 7:d1b09098579b | 35 | for(int i = 0; i < 3; i++) |
yezhong | 7:d1b09098579b | 36 | { |
yezhong | 7:d1b09098579b | 37 | zitaifoot_shou[i]=(float)((zitaifoot_use[2*i+1]<<8)|zitaifoot_use[2*i])/32768.0f*180; |
yezhong | 7:d1b09098579b | 38 | } |
yezhong | 7:d1b09098579b | 39 | foot_angle_x=zitaifoot_shou[0]; |
yezhong | 7:d1b09098579b | 40 | foot_angle_y=zitaifoot_shou[1]; |
yezhong | 7:d1b09098579b | 41 | foot_angle_z=zitaifoot_shou[2]; |
yezhong | 7:d1b09098579b | 42 | if (foot_angle_x >= 180) |
yezhong | 7:d1b09098579b | 43 | foot_angle_x -= 2 * 180; |
yezhong | 7:d1b09098579b | 44 | if (foot_angle_y >= 180) |
yezhong | 7:d1b09098579b | 45 | foot_angle_y -= 2 * 180; |
yezhong | 7:d1b09098579b | 46 | if (foot_angle_z >=180) |
yezhong | 7:d1b09098579b | 47 | foot_angle_z-= 2 * 180; |
yezhong | 7:d1b09098579b | 48 | pc.printf("%.3f %.3f %.3f %.3f\r\n",foot_angle_x,foot_angle_y,foot_angle_z,b); |
yezhong | 7:d1b09098579b | 49 | } |
yezhong | 7:d1b09098579b | 50 | |
yezhong | 7:d1b09098579b | 51 | void zitaifoot_clear() |
yezhong | 7:d1b09098579b | 52 | { |
yezhong | 7:d1b09098579b | 53 | for(int i = 0; i < 9; i++){ |
yezhong | 7:d1b09098579b | 54 | zitaifoot_use[i] = 0; |
panzhan | 0:d80c66cb1b3a | 55 | } |
yezhong | 5:2503c88a564f | 56 | } |
yezhong | 5:2503c88a564f | 57 | |
yezhong | 7:d1b09098579b | 58 | |
yezhong | 7:d1b09098579b | 59 | |
yezhong | 7:d1b09098579b | 60 | |
yezhong | 7:d1b09098579b | 61 | |
yezhong | 7:d1b09098579b | 62 | void serial_zitai_foot_isr(void) |
yezhong | 7:d1b09098579b | 63 | { |
yezhong | 7:d1b09098579b | 64 | |
yezhong | 7:d1b09098579b | 65 | static uint16_t rx; |
yezhong | 7:d1b09098579b | 66 | static uint16_t rx_data[11]; |
yezhong | 7:d1b09098579b | 67 | |
yezhong | 7:d1b09098579b | 68 | uint16_t RX_CHECKCODE;//校验码 |
yezhong | 7:d1b09098579b | 69 | |
yezhong | 7:d1b09098579b | 70 | rx_data[rx] = zitai_foot.getc(); |
yezhong | 7:d1b09098579b | 71 | |
yezhong | 7:d1b09098579b | 72 | if(rx_data[0] == 0x55){ |
yezhong | 7:d1b09098579b | 73 | // pc.printf("OK\n"); |
yezhong | 7:d1b09098579b | 74 | rx++; |
yezhong | 7:d1b09098579b | 75 | } |
yezhong | 7:d1b09098579b | 76 | |
yezhong | 7:d1b09098579b | 77 | if(rx >= 11){ |
yezhong | 7:d1b09098579b | 78 | RX_CHECKCODE=(rx_data[0]+rx_data[1]+rx_data[2]+rx_data[3]+rx_data[4]+rx_data[5]+rx_data[6]+rx_data[7]+rx_data[8]+rx_data[9])&0xFF; |
yezhong | 7:d1b09098579b | 79 | if(rx_data[10] == RX_CHECKCODE){ |
yezhong | 7:d1b09098579b | 80 | for(int i = 0; i<6; i++) |
yezhong | 7:d1b09098579b | 81 | { |
yezhong | 7:d1b09098579b | 82 | zitaifoot_use[i]=rx_data[i+2]; |
yezhong | 7:d1b09098579b | 83 | } |
yezhong | 7:d1b09098579b | 84 | // pc.printf("%d %d %d %d %d %d %d %d %d %d %d\r\n", rx_data[0],rx_data[1], rx_data[2], rx_data[3], rx_data[4], rx_data[5], rx_data[6], rx_data[7], rx_data[8], rx_data[9], rx_data[10]); |
yezhong | 7:d1b09098579b | 85 | zitaifoot_decode(); |
yezhong | 7:d1b09098579b | 86 | zitaifoot_clear(); |
yezhong | 7:d1b09098579b | 87 | } |
yezhong | 7:d1b09098579b | 88 | rx = 0; |
yezhong | 7:d1b09098579b | 89 | } |
yezhong | 7:d1b09098579b | 90 | |
yezhong | 7:d1b09098579b | 91 | //pc.printf("%d\n", rx_data[rx]); |
yezhong | 7:d1b09098579b | 92 | } |
yezhong | 7:d1b09098579b | 93 | |
yezhong | 7:d1b09098579b | 94 | |
yezhong | 7:d1b09098579b | 95 | |
yezhong | 7:d1b09098579b | 96 | |
yezhong | 7:d1b09098579b | 97 | |
yezhong | 7:d1b09098579b | 98 | |
yezhong | 7:d1b09098579b | 99 | |
yezhong | 7:d1b09098579b | 100 | |
yezhong | 6:902ba9999d6c | 101 | |
yezhong | 6:902ba9999d6c | 102 | |
yezhong | 6:902ba9999d6c | 103 | |
yezhong | 6:902ba9999d6c | 104 | |
yezhong | 5:2503c88a564f | 105 | AnalogIn LaLi_pf(PC_1); |
yezhong | 5:2503c88a564f | 106 | AnalogIn LaLi_df(PC_2); |
yezhong | 5:2503c88a564f | 107 | AnalogIn LaLi_df1(PC_3); |
yezhong | 5:2503c88a564f | 108 | |
yezhong | 5:2503c88a564f | 109 | float La_pf_real = 0.0f, La_df_real = 0.0f,La_df1_real = 0.0f; |
yezhong | 5:2503c88a564f | 110 | float Ffilter_pf[10 + 2] = {0.0f}; |
yezhong | 5:2503c88a564f | 111 | float pf_filter = 0.0f; |
yezhong | 5:2503c88a564f | 112 | float F_pf = 0.0f; |
yezhong | 5:2503c88a564f | 113 | |
yezhong | 5:2503c88a564f | 114 | |
yezhong | 5:2503c88a564f | 115 |