File content as of revision 0:dd5d4837292c:

#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);       
        
        
    }
}