Diff: DATA_BOARD/data_board.cpp

Revision:

7:d1b09098579b

Parent:

6:902ba9999d6c

--- a/DATA_BOARD/data_board.cpp	Tue Jan 11 13:34:14 2022 +0000
+++ b/DATA_BOARD/data_board.cpp	Tue Feb 22 14:26:28 2022 +0000
@@ -2,118 +2,102 @@
 #include "data_board.h"
 #include "data_pc.h"
 
-Serial foot(PC_10, PC_11);              // tx, rx U3
-
-  
-
-unsigned int flag_485_A = 0, flag_485_B = 0;
-unsigned int S485num = 0;
-
-uint16_t S485get[10] = {0};                                                    
-uint16_t S485use[10] = {0};
+ 
 
-////////////////////////////////////////////////////////////////////////////////
-//   Gait_now           当前相位          Gait_per_now  当前步态时刻      Gait_change 步态改变标志位    
-//   Gait_num_valid     步数统计          time_portion_now    当前相位时间段
-////////////////////////////////////////////////////////////////////////////////
-
-unsigned int Gait_num_valid = 0, Gait_now = 0, Gait_per_now = 0, Gait_cycle_now = 0, time_portion_now=0;
-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;
-float COP_Y = 0.0f, COP_X = 0.0f;
-
-unsigned int Gait_num_valid_0before = 0, Gait_now_0before = 0;
-float Gait_per_now_0before = 0.0f, COP_Y_0before = 0.0f, COP_X_0before = 0.0f, Gait_cycle_now_0before = 0.0f;
-
-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;
-float COP_Y_real = 0.0f, COP_X_real = 0.0f;
+//Serial foot(PA_9, PA_10);              // tx, rx U1
 
+//Serial zitai_foot(PA_0, PA_1);          // tx, rx U4
+  
+  
+  Serial zitai_foot(PA_9, PA_10);
+Timer ttt;
 
-void gait_decode()
-{
-    Gait_now = S485use[0];
-    Gait_per_now = S485use[1]*100 + S485use[2]*10 + S485use[3];
-    time_portion_now = S485use[4]*100000 + S485use[5]*10000 + S485use[6]*1000 + S485use[7]*100 + S485use[8]*10 + S485use[9];
-              
-        
-    Gait_num_valid_real = Gait_num_valid;
-    Gait_now_real = Gait_now;
-    Gait_per_now_real = Gait_per_now;
-    COP_Y_real = COP_Y;
-    COP_X_real = COP_X;
-    Gait_cycle_now_real = Gait_cycle_now;
-    time_portion_now_real=time_portion_now;
-    
-    pc.printf("%01d---%03d---%06d\n",Gait_now_real,Gait_per_now_real,time_portion_now_real);
-}
+unsigned int zitaifoot_A = 0, zitaifoot_B = 0;
+unsigned int zitaifoot_num = 0;
 
-void gait_clear()
-{
-    for(int i = 0; i < 10; i++){
-        S485use[i] = 0;
-    }
-}
+uint16_t zitaifoot_get[9] = {0};                                                    
+uint16_t zitaifoot_use[9] = {0};
+float zitaifoot_shou[3]= {0};
+float foot_angle_x=0,foot_angle_y=0,foot_angle_z=0;
 
 
 void serial_board_isr(void)
 {
-//    pc.printf("begin\n");
-    while(foot.readable())
-    {       
-        uint16_t c = foot.getc();
-        if(c == 'A')
-        {
-            flag_485_A = 1;
-            
-            flag_485_B = 0;
-            S485num = 0;
-            for(unsigned int i = 0; i < 10; i++)
-            {
-                S485get[i] = 0;      
-            }
-            
-            break;  
-        }
-        if(c == 'B')
-        {
-            flag_485_B = 1;
-        }
-        
-        if(flag_485_A == 1)
-        {
-            if((flag_485_B != 1) && (S485num < 10))
-            {
-                S485get[S485num] = c;    
-            }
-            
-            S485num++;
-            
-            if((flag_485_B == 1) && (S485num != 11))
-            {    
-                flag_485_A = 0;
-                flag_485_B = 0;
-                S485num    = 0;     
-            }
-            
-            if((flag_485_B == 1) && (S485num == 11))
-            {
-                flag_485_A = 0;
-                flag_485_B = 0;
-                S485num    = 0;
-                
-                for(unsigned int i = 0; i < 10; i++)
-                {
-                    S485use[i] = S485get[i] - '0';
-                }
-                
-                gait_decode();
-               // pc.printf("OK\n");
-                gait_clear(); 
-            } 
-        }
+    //pc.printf("bbbbb\n");
+    }
+
+
+void zitaifoot_decode()
+{
+    ttt.start();
+      ttt.read(); 
+    float  b=ttt.read(); 
+     for(int i = 0; i < 3; i++)
+    {
+    zitaifoot_shou[i]=(float)((zitaifoot_use[2*i+1]<<8)|zitaifoot_use[2*i])/32768.0f*180;
+    }
+    foot_angle_x=zitaifoot_shou[0];
+    foot_angle_y=zitaifoot_shou[1];
+    foot_angle_z=zitaifoot_shou[2];
+    if (foot_angle_x >= 180)
+        foot_angle_x -= 2 * 180;
+    if (foot_angle_y >= 180)
+        foot_angle_y -= 2 * 180;
+    if (foot_angle_z >=180)
+        foot_angle_z-= 2 * 180;
+     pc.printf("%.3f  %.3f  %.3f  %.3f\r\n",foot_angle_x,foot_angle_y,foot_angle_z,b);
+}
+
+void zitaifoot_clear()
+{
+    for(int i = 0; i < 9; i++){
+        zitaifoot_use[i] = 0;
     }
 }
 
- 
+
+
+
+          
+void serial_zitai_foot_isr(void)
+{
+    
+    static uint16_t rx;
+    static uint16_t rx_data[11];
+    
+    uint16_t RX_CHECKCODE;//校验码
+    
+    rx_data[rx] = zitai_foot.getc();
+    
+    if(rx_data[0] == 0x55){
+        // pc.printf("OK\n");
+        rx++;
+    }
+    
+    if(rx >= 11){
+        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;
+        if(rx_data[10] == RX_CHECKCODE){
+               for(int i = 0; i<6; i++)
+               {
+                 zitaifoot_use[i]=rx_data[i+2];
+                 }    
+           //  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]);  
+             zitaifoot_decode(); 
+             zitaifoot_clear(); 
+        } 
+     rx = 0;   
+    }
+    
+    //pc.printf("%d\n", rx_data[rx]);
+}
+
+
+
+
+
+
+
+