Diff: main.cpp

Revision:

0:d80c66cb1b3a

Child:

1:a71791b81b8a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 10 09:09:58 2020 +0000
@@ -0,0 +1,105 @@
+#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"
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                             框架搭建完毕                                     //
+////////////////////////////////////////////////////////////////////////////////
+
+//ankle----pf
+//knee-----df
+
+
+int main()
+{
+
+////////////////////////初始化//////////////////////////////////////
+    pc.baud(115200);
+    pc.attach(&serial_pc_isr);
+
+    foot.baud(115200);
+    foot.attach(&serial_board_isr);
+
+//    command.baud(115200);
+//    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    = 0x01;
+////////////////////////////////////////////////////////////////////////////////
+    wait(5);
+
+
+    EnterMotorMode(&knee_txMsg);
+    EnterMotorMode(&ankle_txMsg);
+
+//////////////////////////////////////////position/////////////////////////////////////////
+//        a_control.ankle.p_des=3.14/8;
+//        a_control.ankle.v_des=0;
+//        a_control.ankle.kp=10;
+//        a_control.ankle.kd=0;
+//        a_control.ankle.t_ff=0;
+//
+//        a_control.knee.p_des=3.14/8;
+//        a_control.knee.v_des=0;
+//        a_control.knee.kp=10;
+//        a_control.knee.kd=0;
+//        a_control.knee.t_ff=0;
+//        PackAll();
+//        WriteAll();
+
+
+    while(1) {
+
+
+        ankle_can.read(ankle_rxMsg);
+        unpack_reply(ankle_rxMsg, &a_state);
+        wait_us(10);
+        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;
+        knv = a_state.knee_state.v;
+        knt = a_state.knee_state.t;
+
+        pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",ankp,ankv,ankt,knp,knv,knt);
+
+///////////////////////////////////////velocity///////////////////////////////////////////////
+
+//        a_control.ankle.p_des=0;
+//        a_control.ankle.v_des=500*2*3.14/60/49;
+//        a_control.ankle.kp=0;
+//        a_control.ankle.kd=3;
+//        a_control.ankle.t_ff=0;
+//
+//        a_control.knee.p_des=0;
+//        a_control.knee.v_des=500*2*3.14/60/49;
+//        a_control.knee.kp=0;
+//        a_control.knee.kd=3;
+//        a_control.knee.t_ff=0;
+//        PackAll();
+//        WriteAll();
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+    }
+}
\ No newline at end of file