Diff: main.cpp

Revision:

0:dd5d4837292c

Child:

2:9418258519ea

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,126 @@
+#include "mbed.h"
+#include <cstring>
+#include "math_ops.h"
+#include "leg_message.h"
+#include "CAN.h"
+#include "used_leg_message.h"
+#include "data_pc.h"
+#include "data_board.h"
+#include "mode.h"
+#include "data_command.h"
+#include "math_ops.h"
+#include "fuzzy.h"
+#include "calculate.h"
+#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调试                          //
+////////////////////////////////////////////////////////////////////////////////
+
+
+int main()
+{
+
+////////////////////////初始化//////////////////////////////////////
+    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);
+
+    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);
+      
+    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);       
+        
+        
+    }
+}
\ No newline at end of file