main.cpp

00001 #include "mbed.h"
00002 #include <cstring>
00003 #include "math_ops.h"
00004 #include "leg_message.h"
00005 #include "CAN.h"
00006 #include "used_leg_message.h"
00007 #include "data_pc.h"
00008 #include "data_board.h"
00009 #include "mode.h"
00010 #include "data_command.h"
00011 
00012 
00013 ////////////////////////////////////////////////////////////////////////////////
00014 //                             框架搭建完毕                                     //
00015 ////////////////////////////////////////////////////////////////////////////////
00016 
00017 //ankle----pf
00018 //knee-----df
00019 
00020 
00021 int main()
00022 {
00023 
00024 ////////////////////////初始化//////////////////////////////////////
00025     pc.baud(115200);
00026     pc.attach(&serial_pc_isr);
00027 
00028     foot.baud(115200);
00029     foot.attach(&serial_board_isr);
00030 
00031 //    command.baud(115200);
00032 //    command.attach(&serial_command_isr);
00033 
00034     ankle_can.frequency(1000000);
00035     ankle_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
00036     ankle_rxMsg.len = 6;
00037     ankle_txMsg.len   = 8;
00038     ankle_txMsg.id    = 0x01;
00039 
00040     knee_can.frequency(1000000);
00041     knee_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
00042     knee_rxMsg.len = 6;
00043     knee_txMsg.len   = 8;
00044     knee_txMsg.id    = 0x01;
00045 ////////////////////////////////////////////////////////////////////////////////
00046     wait(5);
00047 
00048 
00049     EnterMotorMode(&knee_txMsg);
00050     EnterMotorMode(&ankle_txMsg);
00051 
00052 //////////////////////////////////////////position/////////////////////////////////////////
00053     a_control.ankle.p_des=3.14/8;
00054     a_control.ankle.v_des=0;
00055     a_control.ankle.kp=5;
00056     a_control.ankle.kd=0;
00057     a_control.ankle.t_ff=0;
00058 
00059     a_control.knee.p_des=3.14/8;
00060     a_control.knee.v_des=0;
00061     a_control.knee.kp=5;
00062     a_control.knee.kd=0;
00063     a_control.knee.t_ff=0;
00064     PackAll();
00065     WriteAll();
00066 
00067 
00068     while(1) {
00069 
00070 
00071         ankle_can.read(ankle_rxMsg);
00072         unpack_reply(ankle_rxMsg, &a_state);
00073         wait_us(10);
00074         knee_can.read(knee_rxMsg);
00075         unpack_reply(knee_rxMsg, &a_state);
00076 
00077         float ankp = a_state.ankle_state.p;                                                     // 从CAN获得的当前位置
00078         float ankv = a_state.ankle_state.v;
00079         float ankt = a_state.ankle_state.t;
00080 
00081         float knp = a_state.knee_state.p;
00082         float knv = a_state.knee_state.v;
00083         float knt = a_state.knee_state.t;
00084 
00085         pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",ankp,ankv,ankt,knp,knv,knt);
00086 
00087 ///////////////////////////////////////velocity///////////////////////////////////////////////
00088 
00089 //        a_control.ankle.p_des=0;
00090 //        a_control.ankle.v_des=100*2*3.14/60/49;
00091 //        a_control.ankle.kp=0;
00092 //        a_control.ankle.kd=4;
00093 //        a_control.ankle.t_ff=0;
00094 //
00095 //        a_control.knee.p_des=0;
00096 //        a_control.knee.v_des=100*2*3.14/60/49;
00097 //        a_control.knee.kp=0;
00098 //        a_control.knee.kd=4;
00099 //        a_control.knee.t_ff=0;
00100 //        PackAll();
00101 //        WriteAll();
00102 
00103 ////////////////////////////////////////////////////////////////////////////////////////////////
00104     }
00105 }