File content as of revision 0:6aa5928be32a:

#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 ankle_real_p = 0.0f;
float ankle_real_v = 0.0f;
float ankle_real_t = 0.0f;
float knee_real_p = 0.0f;
float knee_real_v = 0.0f;
float knee_real_t = 0.0f;


float count=0.0f;
float result_knee=0.0f;
float result_ankle=0.0f;



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(3);
    EnterMotorMode(&knee_txMsg);
    EnterMotorMode(&ankle_txMsg);
    Zero(&knee_txMsg);
    Zero(&ankle_txMsg);

    NVIC_SetPriority(USART1_IRQn, 1);                                           // command中断优先级高于board
    NVIC_SetPriority(USART3_IRQn, 3);

    fuzzy_init(&ankleFuzzy, 1.0f, 1.0f, 0.14f, 0.14f);
    fuzzy_init(&kneeFuzzy, 1.0f, 1.0f, 0.14f, 0.14f);

    fuzzytimer.start();
    tim.start();

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

        ankle_real_p = a_state.ankle_state.p;                                   // 从CAN获得的当前位置
        ankle_real_v = a_state.ankle_state.v;
        ankle_real_t = a_state.ankle_state.t;

        knee_real_p = a_state.ankle_state.p;                                    // 从CAN获得的当前位置
        knee_real_v = a_state.ankle_state.v;
        knee_real_t = a_state.ankle_state.t;

        control(ankle_real_p, knee_real_p);

        if(send_enable == 1) {
            PackAll();
            WriteAll();
////////////////////////////////////////////////////////////////////////////////
//                            状态输出 与labview配套                           //
////////////////////////////////////////////////////////////////////////////////
            command.printf("AB");
            if(a_state.knee_state.p >= 0)
                command.printf("+%.3f\t",a_state.knee_state.p);
            else
                command.printf("%.3f\t",a_state.knee_state.p);
            
            if(a_control.knee.p_des >= 0)
                command.printf("+%.3f\t",a_control.knee.p_des);
            else
                command.printf("%.3f\t",a_control.knee.p_des);
            
            if(a_state.knee_state.t >= 0)
                command.printf("+%.3f\t",a_state.knee_state.t);
            else
                command.printf("%.3f\t",a_state.knee_state.t);
            
            if(a_state.ankle_state.p >= 0)
                command.printf("+%.3f\t",a_state.ankle_state.p);
            else
                command.printf("%.3f\t",a_state.ankle_state.p);
            
            if(a_control.ankle.p_des >= 0)
                command.printf("+%.3f\t",a_control.ankle.p_des);
            else
                command.printf("%.3f\t",a_control.ankle.p_des);
                 
            if(a_state.ankle_state.t >= 0)
                command.printf("+%.3f\n",a_state.ankle_state.t);
            else
                command.printf("%.3f\n",a_state.ankle_state.t);    
            }            
    }
}