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_command.h"
00008 #include "data_pc.h"
00009 #include "mode.h"
00010 #include "control.h"
00011  float pef = 0;                                                                  // 从CAN获得的位置量
00012  float pwf = 0;
00013 int main()
00014 {
00015    
00016     pc.baud(115200);
00017     shouzhua.baud(9600); 
00018     command.baud(115200);
00019     /*******shaorui add******
00020      EnterMotorMode(&EF_can);                                            // 电机位置锁定 
00021      send_enable = 0; 
00022      state=MOTOR_MODE; 
00023     pc.printf("Enter Motor Mode ");
00024    ***************************/   
00025     command.attach(&serial_command_isr);
00026   
00027                                                       // 主要为接收模式                                                             // 主要为发送模式
00028     ef_can.frequency(1000000);
00029     ef_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
00030     ef_rxMsg.len = 6;
00031     EF_can.len   = 8;
00032     EF_can.id    = 0x01;
00033     
00034     wf_can.frequency(1000000);
00035     wf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
00036     wf_rxMsg.len = 6;
00037     WF_can.len   = 8;
00038     WF_can.id    = 0x0b;
00039     
00040     NVIC_SetPriority(USART1_IRQn, 3);                                           // pc中断优先级高于board
00041     NVIC_SetPriority(USART2_IRQn, 4);
00042     
00043     
00044     while(1)
00045     {
00046         counter++;
00047          //获取AD
00048         ad1 = AD1.read() * 3300;
00049         ad2 = AD2.read() * 3300;
00050         /********************AD 通讯协议***********************/
00051         //将ad的值用八位表示
00052         //7位-0表示ad1，1表示ad2
00053         //6-0位表示ad值
00054         ad1=ad1/26;       //将ad1值限制在0-6bit,7bit set to be 0
00055         ad2=ad2/26+128;   //将ad2值限制在0-6bit,7 bit set to be 1
00056         command.putc(ad1);
00057         command.putc(ad2);
00058         /********************AD 通讯协议***********************/
00059         //////////////////////// CAN获取电机位置速度信息 //////////////////////////
00060         ef_can.read(ef_rxMsg);                                                                            
00061         unpack_reply(ef_rxMsg, &a_state);
00062         wait_us(10);
00063         wf_can.read(wf_rxMsg);
00064         unpack_reply(wf_rxMsg, &a_state);
00065         
00066         pef = a_state.ef.p;                                                     // 从CAN获得的当前位置
00067         pwf = a_state.wf.p;
00068        
00069        command_control();
00070         
00071         if(send_enable == 1)
00072         {
00073             PackAll();
00074             WriteAll();
00075             //send_enable = 0;
00076         }
00077        if(duoji_command==1)
00078        {
00079           moveServo(1, shouzhua_control.p_des, 1000); //1s移动1号舵机至指定的位置
00080           printf("Shouzhua Move Sucessfully1 ! \n\r");
00081          wait(1);
00082            }
00083       
00084         //pc.printf("\n\rPpd: %f - %f\r", SP_pf, SP_df);
00085        // pc.printf("\n\rC: %f - %f\r", a_control.ef.p_des, a_control.wf.p_des);
00086        // pc.printf("\n\rP: %f - %f\r", a_state.ef.p, a_state.wf.p); 
00087        pc.printf("%.3x,%.3x\n\r", EF_can.data[0],EF_can.data[1]);
00088        pc.printf("send_enbale:%.3d,  p_des: %.3f\n\r",  send_enable, a_control.ef.p_des);
00089             wait(2);
00090        pc.printf("AD: %.3f\n\r", ad2);
00091             }
00092             
00093             
00094 }