Shao Rui / CH_Communicatuin_Test2

1

Dependencies:   mbed-dev_spine

main.cpp@8:95a914f962bd, 2020-01-07 (annotated)

Committer:
shaorui
Date:
Tue Jan 07 09:23:24 2020 +0000
Revision:
8:95a914f962bd
Parent:
7:4362ea3d5300
Child:
9:bf02fd2d7a0a
1234

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:d6186b8990c5 1 #include "mbed.h"
WXD 5:6a95726e45b0 2 #include <cstring>
benkatz 0:d6186b8990c5 3 #include "math_ops.h"
benkatz 3:9ef9b4c66648 4 #include "leg_message.h"
WXD 5:6a95726e45b0 5 #include "CAN.h"
WXD 5:6a95726e45b0 6 #include "used_leg_message.h"
WXD 5:6a95726e45b0 7 #include "data_command.h"
WXD 5:6a95726e45b0 8 #include "data_pc.h"
WXD 5:6a95726e45b0 9 #include "mode.h"
WXD 5:6a95726e45b0 10 #include "control.h"
shaorui 8:95a914f962bd 11 float pef = 0; // 从CAN获得的位置量
shaorui 8:95a914f962bd 12 float pwf = 0;
WXD 5:6a95726e45b0 13 int main()
WXD 5:6a95726e45b0 14 {
shaorui 8:95a914f962bd 15
WXD 5:6a95726e45b0 16
shaorui 8:95a914f962bd 17 pc.baud(115200);
shaorui 8:95a914f962bd 18 shouzhua.baud(9600);
shaorui 8:95a914f962bd 19 command.baud(115200);
shaorui 8:95a914f962bd 20 /*******shaorui add******
shaorui 8:95a914f962bd 21 EnterMotorMode(&EF_can); // 电机位置锁定
shaorui 8:95a914f962bd 22 send_enable = 0;
shaorui 8:95a914f962bd 23 state=MOTOR_MODE;
shaorui 8:95a914f962bd 24 pc.printf("Enter Motor Mode ");
shaorui 8:95a914f962bd 25 ***************************/
shaorui 8:95a914f962bd 26 command.attach(&serial_command_isr);
shaorui 8:95a914f962bd 27
shaorui 8:95a914f962bd 28 // 主要为接收模式 // 主要为发送模式
shaorui 8:95a914f962bd 29 ef_can.frequency(1000000);
shaorui 8:95a914f962bd 30 ef_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
shaorui 8:95a914f962bd 31 ef_rxMsg.len = 6;
shaorui 8:95a914f962bd 32 EF_can.len = 8;
shaorui 8:95a914f962bd 33 EF_can.id = 0x01;
benkatz 1:79e0d4791936 34
shaorui 8:95a914f962bd 35 wf_can.frequency(1000000);
shaorui 8:95a914f962bd 36 wf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
shaorui 8:95a914f962bd 37 wf_rxMsg.len = 6;
shaorui 8:95a914f962bd 38 WF_can.len = 8;
shaorui 8:95a914f962bd 39 WF_can.id = 0x0b;
benkatz 1:79e0d4791936 40
WXD 5:6a95726e45b0 41 NVIC_SetPriority(USART1_IRQn, 3); // pc中断优先级高于board
WXD 5:6a95726e45b0 42 NVIC_SetPriority(USART2_IRQn, 4);
benkatz 3:9ef9b4c66648 43
benkatz 3:9ef9b4c66648 44
WXD 5:6a95726e45b0 45 while(1)
WXD 5:6a95726e45b0 46 {
WXD 5:6a95726e45b0 47 counter++;
shaorui 8:95a914f962bd 48 //获取AD
WXD 6:aad89fd109c2 49 ad1 = AD1.read() * 3300;
WXD 6:aad89fd109c2 50 ad2 = AD2.read() * 3300;
shaorui 8:95a914f962bd 51 command.putc(ad1);
shaorui 8:95a914f962bd 52 command.putc(ad2);
WXD 5:6a95726e45b0 53 //////////////////////// CAN获取电机位置速度信息 //////////////////////////
shaorui 8:95a914f962bd 54 ef_can.read(ef_rxMsg);
shaorui 8:95a914f962bd 55 unpack_reply(ef_rxMsg, &a_state);
WXD 5:6a95726e45b0 56 wait_us(10);
shaorui 8:95a914f962bd 57 wf_can.read(wf_rxMsg);
shaorui 8:95a914f962bd 58 unpack_reply(wf_rxMsg, &a_state);
WXD 5:6a95726e45b0 59
shaorui 8:95a914f962bd 60 pef = a_state.ef.p; // 从CAN获得的当前位置
shaorui 8:95a914f962bd 61 pwf = a_state.wf.p;
shaorui 8:95a914f962bd 62
shaorui 8:95a914f962bd 63 command_control();
WXD 5:6a95726e45b0 64
WXD 5:6a95726e45b0 65 if(send_enable == 1)
WXD 5:6a95726e45b0 66 {
WXD 5:6a95726e45b0 67 PackAll();
WXD 5:6a95726e45b0 68 WriteAll();
WXD 6:aad89fd109c2 69 //send_enable = 0;
benkatz 3:9ef9b4c66648 70 }
shaorui 8:95a914f962bd 71 if(duoji_command==1)
shaorui 8:95a914f962bd 72 {moveServo(1, 2000, 1000); //1s移动1号舵机至2000位置
shaorui 8:95a914f962bd 73 printf("Move Sucessfully1 ! \n\r");
shaorui 8:95a914f962bd 74 wait(2);
shaorui 8:95a914f962bd 75 moveServo(1, 500, 1000); //1s移动1号舵机至500位置
shaorui 8:95a914f962bd 76 //BaterryOut();
shaorui 8:95a914f962bd 77 printf("Move Sucessfully2 ! \n\r");
shaorui 8:95a914f962bd 78 wait(2);
shaorui 8:95a914f962bd 79
shaorui 8:95a914f962bd 80 }
shaorui 8:95a914f962bd 81
shaorui 8:95a914f962bd 82 //pc.printf("\n\rPpd: %f - %f\r", SP_pf, SP_df);
shaorui 8:95a914f962bd 83 // pc.printf("\n\rC: %f - %f\r", a_control.ef.p_des, a_control.wf.p_des);
shaorui 8:95a914f962bd 84 // pc.printf("\n\rP: %f - %f\r", a_state.ef.p, a_state.wf.p);
shaorui 8:95a914f962bd 85 pc.printf("%.3x,%.3x\n\r", EF_can.data[0],EF_can.data[1]);
shaorui 8:95a914f962bd 86 pc.printf("send_enbale:%.3d, p_des: %.3f\n\r", send_enable, a_control.ef.p_des);
shaorui 8:95a914f962bd 87 wait(2);
shaorui 8:95a914f962bd 88 pc.printf("AD: %.3f\n\r", ad2);
shaorui 8:95a914f962bd 89 }
WXD 5:6a95726e45b0 90
shaorui 8:95a914f962bd 91
WXD 5:6a95726e45b0 92 }