yezhong yezhong / yezhong_main_controller_copy_1

1

Dependencies:   mcp2515 mbed-dev-f303

DATA_BOARD/data_board.cpp@7:1ab9699af5ae, 2022-03-22 (annotated)

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