File content as of revision 9:bf02fd2d7a0a:

#include "mbed.h"
#include <cstring>
#include "math_ops.h"
#include "leg_message.h"
#include "CAN.h"
#include "used_leg_message.h"
#include "data_command.h"
#include "data_pc.h"
#include "mode.h"
#include "control.h"
 float pef = 0;                                                                  // 从CAN获得的位置量
 float pwf = 0;
int main()
{
   
    pc.baud(115200);
    shouzhua.baud(9600); 
    command.baud(115200);
    /*******shaorui add******
     EnterMotorMode(&EF_can);                                            // 电机位置锁定 
     send_enable = 0; 
     state=MOTOR_MODE; 
    pc.printf("Enter Motor Mode ");
   ***************************/   
    command.attach(&serial_command_isr);
  
                                                      // 主要为接收模式                                                             // 主要为发送模式
    ef_can.frequency(1000000);
    ef_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
    ef_rxMsg.len = 6;
    EF_can.len   = 8;
    EF_can.id    = 0x01;
    
    wf_can.frequency(1000000);
    wf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
    wf_rxMsg.len = 6;
    WF_can.len   = 8;
    WF_can.id    = 0x0b;
    
    NVIC_SetPriority(USART1_IRQn, 3);                                           // pc中断优先级高于board
    NVIC_SetPriority(USART2_IRQn, 4);
    
    
    while(1)
    {
        counter++;
         //获取AD
        ad1 = AD1.read() * 3300;
        ad2 = AD2.read() * 3300;
        /********************AD 通讯协议***********************/
        //将ad的值用八位表示
        //7位-0表示ad1，1表示ad2
        //6-0位表示ad值
        ad1=ad1/26;       //将ad1值限制在0-6bit,7bit set to be 0
        ad2=ad2/26+128;   //将ad2值限制在0-6bit,7 bit set to be 1
        command.putc(ad1);
        command.putc(ad2);
        /********************AD 通讯协议***********************/
        //////////////////////// CAN获取电机位置速度信息 //////////////////////////
        ef_can.read(ef_rxMsg);                                                                            
        unpack_reply(ef_rxMsg, &a_state);
        wait_us(10);
        wf_can.read(wf_rxMsg);
        unpack_reply(wf_rxMsg, &a_state);
        
        pef = a_state.ef.p;                                                     // 从CAN获得的当前位置
        pwf = a_state.wf.p;
       
       command_control();
        
        if(send_enable == 1)
        {
            PackAll();
            WriteAll();
            //send_enable = 0;
        }
       if(duoji_command==1)
       {moveServo(1, 2000, 1000); //1s移动1号舵机至2000位置
          printf("Move Sucessfully1 ! \n\r");
         wait(2);
        moveServo(1, 500, 1000); //1s移动1号舵机至500位置
        //BaterryOut();
        printf("Move Sucessfully2 ! \n\r");
        wait(2);
           
           }
      
        //pc.printf("\n\rPpd: %f - %f\r", SP_pf, SP_df);
       // pc.printf("\n\rC: %f - %f\r", a_control.ef.p_des, a_control.wf.p_des);
       // pc.printf("\n\rP: %f - %f\r", a_state.ef.p, a_state.wf.p); 
       pc.printf("%.3x,%.3x\n\r", EF_can.data[0],EF_can.data[1]);
       pc.printf("send_enbale:%.3d,  p_des: %.3f\n\r",  send_enable, a_control.ef.p_des);
            wait(2);
       pc.printf("AD: %.3f\n\r", ad2);
            }
            
            
}