Revision 2:9418258519ea, committed 2021-08-06

Comitter:

yezhong

Date:

Fri Aug 06 09:03:02 2021 +0000

Parent:

1:b7cd6d22f277

Commit message:

111

Changed in this revision

diff -r b7cd6d22f277 -r 9418258519ea DATA_BOARD/data_board.cpp
--- a/DATA_BOARD/data_board.cpp	Thu Aug 05 02:11:41 2021 +0000
+++ b/DATA_BOARD/data_board.cpp	Fri Aug 06 09:03:02 2021 +0000
@@ -8,43 +8,33 @@
 unsigned int flag_foot_A = 0, flag_foot_B = 0;
 unsigned int Sfootnum = 0;
 
-uint16_t Sfootget[10] = {0};                                                    
-uint16_t Sfootuse[10] = {0};
+uint8_t Sfootget[30] = {0};                                                    
+uint8_t Sfootuse[30] = {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;
+uint16_t shou[15]={0};
+float shou1[15]={0};
 
 
-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;
+
 
 
 void gait_decode()
-{
-    Gait_now = Sfootuse[0];
-    Gait_per_now = Sfootuse[1]*100 + Sfootuse[2]*10 + Sfootuse[3];
-    time_portion_now = Sfootuse[4]*100000 + Sfootuse[5]*10000 + Sfootuse[6]*1000 + Sfootuse[7]*100 + Sfootuse[8]*10 + Sfootuse[9];
-              
-        
-    Gait_num_valid_real = Gait_num_valid;
-    Gait_now_real = Gait_now;
-    Gait_per_now_real = Gait_per_now;
-    Gait_cycle_now_real = Gait_cycle_now;
-    time_portion_now_real=time_portion_now;
-    
-    command.printf("%01d---%03d---%06d\n",Gait_now_real,Gait_per_now_real,time_portion_now_real);
+{    
+    for(int i = 0; i < 15; i++)
+    {
+    shou[i]=(Sfootuse[2*i]<<8)|Sfootuse[2*i+1];
+    }
+    for(int i = 0; i < 15; i++)
+    {
+    shou1[i]=(float)shou[i]/4096*3.3;
+    }        
+    pc.printf("%2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f\r\n",shou1[6],shou1[7],shou1[8],shou1[9],shou1[10],shou1[11],shou1[12],shou1[13],shou1[14]);
 }
 
 void gait_clear()
 {
-    for(int i = 0; i < 10; i++){
+    for(int i = 0; i < 30; i++){
         Sfootuse[i] = 0;
     }
 }
@@ -55,50 +45,51 @@
 //    pc.printf("begin\n");
     while(foot.readable())
     {       
-        uint16_t c = foot.getc();
-        if(c == 'A')
+        uint8_t c = foot.getc();
+        if(c ==0x0A)
         {
             flag_foot_A = 1;
             
             flag_foot_B = 0;
             Sfootnum = 0;
-            for(unsigned int i = 0; i < 10; i++)
+            for(unsigned int i = 0; i < 30; i++)
             {
                 Sfootget[i] = 0;      
             }
             
             break;  
         }
-        if(c == 'B')
+        if(c ==0x0B)
         {
             flag_foot_B = 1;
         }
         
         if(flag_foot_A == 1)
         {
-            if((flag_foot_B != 1) && (Sfootnum < 10))
+            if((flag_foot_B != 1) && (Sfootnum < 30))
             {
                 Sfootget[Sfootnum] = c;    
             }
             
             Sfootnum++;
             
-            if((flag_foot_B == 1) && (Sfootnum != 11))
+            if((flag_foot_B == 1) && (Sfootnum != 31))
             {    
                 flag_foot_A = 0;
                 flag_foot_B = 0;
                 Sfootnum    = 0;     
             }
             
-            if((flag_foot_B == 1) && (Sfootnum == 11))
+            if((flag_foot_B == 1) && (Sfootnum == 31))
             {
                 flag_foot_A = 0;
                 flag_foot_B = 0;
                 Sfootnum    = 0;
                 
-                for(unsigned int i = 0; i < 10; i++)
+                for(unsigned int i = 0; i < 30; i++)
                 {
-                    Sfootuse[i] = Sfootget[i] - '0';
+         //           Sfootuse[i] = Sfootget[i] - '0';
+                      Sfootuse[i] = Sfootget[i] ;
                 }
                 
                 gait_decode();

diff -r b7cd6d22f277 -r 9418258519ea DATA_BOARD/data_board.h
--- a/DATA_BOARD/data_board.h	Thu Aug 05 02:11:41 2021 +0000
+++ b/DATA_BOARD/data_board.h	Fri Aug 06 09:03:02 2021 +0000
@@ -10,25 +10,21 @@
 
 
 
-//extern unsigned int flag_foot_A, flag_foot_B;
-//extern unsigned int Sfootnum;
-//extern uint16_t Sfootget[10];                                                    
-//extern uint16_t Sfootuse[10];
+extern unsigned int flag_foot_A, flag_foot_B;
+extern unsigned int Sfootnum;
+extern uint8_t Sfootget[30];                                                    
+extern uint8_t Sfootuse[30];
+extern uint16_t shou[15];
+extern float shou1[15];
 
 ////////////////////////////////////////////////////////////////////////////////
 
-extern unsigned int Gait_num_valid, Gait_now, Gait_per_now, Gait_cycle_now;
-extern uint16_t Gait_per_now_int, COP_Y_int, COP_X_int, Gait_cycle_now_int;
-
-extern unsigned int Gait_num_valid_0before, Gait_now_0before;
-extern float Gait_per_now_0before, COP_Y_0before, COP_X_0before, Gait_cycle_now_0before;
-
-extern unsigned int Gait_num_valid_real, Gait_now_real, Gait_per_now_real, Gait_cycle_now_real, time_portion_now_real;
 
 
 
-//void gait_decode();
-//void gait_clear();
+
+void gait_decode();
+void gait_clear();
 
 ////////////////////////////////////////////////////////////////////////////////
 

diff -r b7cd6d22f277 -r 9418258519ea DATA_COMMAND/data_command.cpp
--- a/DATA_COMMAND/data_command.cpp	Thu Aug 05 02:11:41 2021 +0000
+++ b/DATA_COMMAND/data_command.cpp	Fri Aug 06 09:03:02 2021 +0000
@@ -76,7 +76,7 @@
                 
                 command_control_flag = 1;                                                  // 命令帧接收成功
                 
-//                command.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c\n\r", Suse[0], Suse[1], Suse[2], Suse[3], Suse[4], Suse[5], Suse[6], Suse[7], Suse[8], Suse[9], Suse[10], Suse[11], Suse[12], Suse[13]);
+          //      command.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c\n\r", Suse[0], Suse[1], Suse[2], Suse[3], Suse[4], Suse[5], Suse[6], Suse[7], Suse[8], Suse[9], Suse[10], Suse[11], Suse[12], Suse[13]);
             }
         }
     }       

diff -r b7cd6d22f277 -r 9418258519ea main.cpp
--- a/main.cpp	Thu Aug 05 02:11:41 2021 +0000
+++ b/main.cpp	Fri Aug 06 09:03:02 2021 +0000
@@ -14,17 +14,6 @@
 #include "timer.h"
 
 
-float ankp = 0.0f;
-float ankv = 0.0f;
-float ankt = 0.0f;
-
-float knp = 0.0f;
-float knv = 0.0f;
-float knt = 0.0f;
-
-float count=0.0f;
-float result_knee=0.0f;
-float result_ankle=0.0f;
 
 ////////////////////////////////////////////////////////////////////////////////
 //                           PID调试                          //
@@ -33,94 +22,37 @@
 
 int main()
 {
-
+     // Timer ttt;
+     
+      
 ////////////////////////初始化//////////////////////////////////////
     pc.baud(115200);                                                            // U2
-    pc.attach(&serial_pc_isr);
+    
 
     foot.baud(115200);                                                          // U1
     foot.attach(&serial_board_isr);
 
-    command.baud(115200);                                                       // U3
-    command.attach(&serial_command_isr);
+   // command.baud(115200);                                                       // U3
+   // command.attach(&serial_command_isr);
+    
 
-    ankle_can.frequency(1000000);
-    ankle_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
-    ankle_rxMsg.len = 6;
-    ankle_txMsg.len   = 8;
-    ankle_txMsg.id    = 0x01;
-
-    knee_can.frequency(1000000);
-    knee_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
-    knee_rxMsg.len = 6;
-    knee_txMsg.len   = 8;
-    knee_txMsg.id    = 0x02;
+  
 ////////////////////////////////////////////////////////////////////////////////
 
-    wait(4);
-    EnterMotorMode(&knee_txMsg);
-    EnterMotorMode(&ankle_txMsg);
-    Zero(&knee_txMsg);
-    Zero(&ankle_txMsg);
+    NVIC_SetPriority(USART1_IRQn, 1);                                           // command中断优先级高于board
+    NVIC_SetPriority(USART3_IRQn, 3);
       
-    tim.start();
-    
-    while(1) {    
-        ankle_can.read(ankle_rxMsg);
-        unpack_reply(ankle_rxMsg, &a_state);
-        wait_us(100);
-        knee_can.read(knee_rxMsg);
-        unpack_reply(knee_rxMsg, &a_state);
-    
-        ankp = a_state.ankle_state.p;                                           // 从CAN获得的当前位置
-        ankv = a_state.ankle_state.v;
-        ankt = a_state.ankle_state.t;
-  
-        knp = a_state.knee_state.p;                                            // 从CAN获得的当前位置
-        knv = a_state.knee_state.v;
-        knt = a_state.knee_state.t;
        
-        control(ankp, knp);
-        
-        if(send_enable == 1){ 
-            PackAll();
-            WriteAll(); 
-        }
-//      pc.printf("p:%.3f\tv:%.3f\tKp:%.3f\tKd:%.3f\ttff:%.3f\n",a_control.ankle.p_des, a_control.ankle.v_des, a_control.ankle.kp, a_control.ankle.kd, a_control.ankle.t_ff);     
-//      pc.printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", a_state.knee_state.p, a_control.knee.p_des, a_state.ankle_state.p, a_control.ankle.p_des,tim.read());  
-//      pc.printf("AB%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", a_state.knee_state.p, a_control.knee.p_des, a_state.knee_state.t, a_state.ankle_state.p, a_control.ankle.p_des, a_state.ankle_state.t);  
-        
-        pc.printf("AB");
-        if(a_state.knee_state.p >= 0)
-            pc.printf("+%.3f\t",a_state.knee_state.p);
-        else
-            pc.printf("%.3f\t",a_state.knee_state.p);
-        
-        if(a_control.knee.p_des >= 0)
-            pc.printf("+%.3f\t",a_control.knee.p_des);
-        else
-            pc.printf("%.3f\t",a_control.knee.p_des);
-        
-        if(a_state.knee_state.t >= 0)
-            pc.printf("+%.3f\t",a_state.knee_state.t);
-        else
-            pc.printf("%.3f\t",a_state.knee_state.t);
-        
-        if(a_state.ankle_state.p >= 0)
-            pc.printf("+%.3f\t",a_state.ankle_state.p);
-        else
-            pc.printf("%.3f\t",a_state.ankle_state.p);
-        
-        if(a_control.ankle.p_des >= 0)
-            pc.printf("+%.3f\t",a_control.ankle.p_des);
-        else
-            pc.printf("%.3f\t",a_control.ankle.p_des);
-             
-        if(a_state.ankle_state.t >= 0)
-            pc.printf("+%.3f\n",a_state.ankle_state.t);
-        else
-            pc.printf("%.3f\n",a_state.ankle_state.t);       
-        
-        
+    
+    while(1) {   
+    /*
+       ttt.start(); 
+       ttt.read();
+       if(ttt.read()>2)
+       {
+         pc.printf("Helloee\n");  
+        ttt.reset();   
+        } 
+        */   
     }
 }
\ No newline at end of file