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 #include "math_ops.h"
00012 #include "fuzzy.h"
00013 #include "calculate.h"
00014 #include "timer.h"
00015 
00016 
00017 float ankp = 0.0f;
00018 float ankv = 0.0f;
00019 float ankt = 0.0f;
00020 
00021 float knp = 0.0f;
00022 float knv = 0.0f;
00023 float knt = 0.0f;
00024 
00025 float count=0.0f;
00026 float result_knee=0.0f;
00027 float result_ankle=0.0f;
00028 
00029 ////////////////////////////////////////////////////////////////////////////////
00030 //                           PID调试                          //
00031 ////////////////////////////////////////////////////////////////////////////////
00032 
00033 
00034 int main()
00035 {
00036 
00037 ////////////////////////初始化//////////////////////////////////////
00038     pc.baud(115200);                                                            // U2
00039     pc.attach(&serial_pc_isr);
00040 
00041     foot.baud(115200);                                                          // U1
00042     foot.attach(&serial_board_isr);
00043 
00044     command.baud(115200);                                                       // U3
00045     command.attach(&serial_command_isr);
00046 
00047     ankle_can.frequency(1000000);
00048     ankle_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
00049     ankle_rxMsg.len = 6;
00050     ankle_txMsg.len   = 8;
00051     ankle_txMsg.id    = 0x01;
00052 
00053     knee_can.frequency(1000000);
00054     knee_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
00055     knee_rxMsg.len = 6;
00056     knee_txMsg.len   = 8;
00057     knee_txMsg.id    = 0x02;
00058 ////////////////////////////////////////////////////////////////////////////////
00059 
00060     wait(4);
00061     EnterMotorMode(&knee_txMsg);
00062     EnterMotorMode(&ankle_txMsg);
00063     Zero(&knee_txMsg);
00064     Zero(&ankle_txMsg);
00065       
00066     tim.start();
00067     
00068     while(1) {    
00069         ankle_can.read(ankle_rxMsg);
00070         unpack_reply(ankle_rxMsg, &a_state);
00071         wait_us(100);
00072         knee_can.read(knee_rxMsg);
00073         unpack_reply(knee_rxMsg, &a_state);
00074     
00075         ankp = a_state.ankle_state.p;                                           // 从CAN获得的当前位置
00076         ankv = a_state.ankle_state.v;
00077         ankt = a_state.ankle_state.t;
00078   
00079         knp = a_state.knee_state.p;                                            // 从CAN获得的当前位置
00080         knv = a_state.knee_state.v;
00081         knt = a_state.knee_state.t;
00082        
00083         control(ankp, knp);
00084         
00085         if(send_enable == 1){ 
00086             PackAll();
00087             WriteAll(); 
00088         }
00089 //      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);     
00090 //      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());  
00091 //      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);  
00092         
00093         pc.printf("AB");
00094         if(a_state.knee_state.p >= 0)
00095             pc.printf("+%.3f\t",a_state.knee_state.p);
00096         else
00097             pc.printf("%.3f\t",a_state.knee_state.p);
00098         
00099         if(a_control.knee.p_des >= 0)
00100             pc.printf("+%.3f\t",a_control.knee.p_des);
00101         else
00102             pc.printf("%.3f\t",a_control.knee.p_des);
00103         
00104         if(a_state.knee_state.t >= 0)
00105             pc.printf("+%.3f\t",a_state.knee_state.t);
00106         else
00107             pc.printf("%.3f\t",a_state.knee_state.t);
00108         
00109         if(a_state.ankle_state.p >= 0)
00110             pc.printf("+%.3f\t",a_state.ankle_state.p);
00111         else
00112             pc.printf("%.3f\t",a_state.ankle_state.p);
00113         
00114         if(a_control.ankle.p_des >= 0)
00115             pc.printf("+%.3f\t",a_control.ankle.p_des);
00116         else
00117             pc.printf("%.3f\t",a_control.ankle.p_des);
00118              
00119         if(a_state.ankle_state.t >= 0)
00120             pc.printf("+%.3f\n",a_state.ankle_state.t);
00121         else
00122             pc.printf("%.3f\n",a_state.ankle_state.t);       
00123         
00124         
00125     }
00126 }