Shao Rui / CH_Communicatuin_Test2

1

Dependencies:   mbed-dev_spine

main.cpp@5:6a95726e45b0, 2019-10-08 (annotated)

Committer:
WXD
Date:
Tue Oct 08 04:35:23 2019 +0000
Revision:
5:6a95726e45b0
Parent:
3:9ef9b4c66648
Child:
6:aad89fd109c2
qqq

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 "data_board.h"
WXD 5:6a95726e45b0 10 #include "mode.h"
WXD 5:6a95726e45b0 11 #include "control.h"
WXD 5:6a95726e45b0 12 #include "calculate.h"
benkatz 3:9ef9b4c66648 13
benkatz 3:9ef9b4c66648 14
WXD 5:6a95726e45b0 15 int main()
WXD 5:6a95726e45b0 16 {
WXD 5:6a95726e45b0 17 pc.baud(115200);
benkatz 0:d6186b8990c5 18
WXD 5:6a95726e45b0 19 command.baud(115200);
WXD 5:6a95726e45b0 20 command.attach(&serial_command_isr);
WXD 5:6a95726e45b0 21 sf_m_c = 0; // 主要为接收模式
WXD 5:6a95726e45b0 22
WXD 5:6a95726e45b0 23 board.baud(115200);
WXD 5:6a95726e45b0 24 board.attach(&serial_board_isr);
WXD 5:6a95726e45b0 25 sf_m_b = 1; // 主要为发送模式
benkatz 1:79e0d4791936 26
WXD 5:6a95726e45b0 27 pf_can.frequency(1000000);
WXD 5:6a95726e45b0 28 pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
WXD 5:6a95726e45b0 29 pf_rxMsg.len = 6;
WXD 5:6a95726e45b0 30 PF_can.len = 8;
WXD 5:6a95726e45b0 31 PF_can.id = 0x0a;
benkatz 0:d6186b8990c5 32
WXD 5:6a95726e45b0 33 df_can.frequency(1000000);
WXD 5:6a95726e45b0 34 df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
WXD 5:6a95726e45b0 35 df_rxMsg.len = 6;
WXD 5:6a95726e45b0 36 DF_can.len = 8;
WXD 5:6a95726e45b0 37 DF_can.id = 0x0b;
benkatz 1:79e0d4791936 38
WXD 5:6a95726e45b0 39 NVIC_SetPriority(USART1_IRQn, 3); // pc中断优先级高于board
WXD 5:6a95726e45b0 40 NVIC_SetPriority(USART2_IRQn, 4);
benkatz 3:9ef9b4c66648 41
benkatz 3:9ef9b4c66648 42
WXD 5:6a95726e45b0 43 while(1)
WXD 5:6a95726e45b0 44 {
WXD 5:6a95726e45b0 45 counter++;
WXD 5:6a95726e45b0 46
WXD 5:6a95726e45b0 47 //////////////////////// CAN获取电机位置速度信息 //////////////////////////
WXD 5:6a95726e45b0 48 pf_can.read(pf_rxMsg);
WXD 5:6a95726e45b0 49 unpack_reply(pf_rxMsg, &a_state);
WXD 5:6a95726e45b0 50 wait_us(10);
WXD 5:6a95726e45b0 51 df_can.read(df_rxMsg);
WXD 5:6a95726e45b0 52 unpack_reply(df_rxMsg, &a_state);
WXD 5:6a95726e45b0 53 /**********************************************************************/
WXD 5:6a95726e45b0 54
WXD 5:6a95726e45b0 55 /////////////////////// 485获取步态，力传感器信息 //////////////////////////
WXD 5:6a95726e45b0 56 sf_m_b = 1; // 发送模式
WXD 5:6a95726e45b0 57 wait_us(200); // 等待选择引脚电压稳定
WXD 5:6a95726e45b0 58 board.printf("A1B");
WXD 5:6a95726e45b0 59 wait_ms(1); // mbed特色，需要等待数据完全发送
WXD 5:6a95726e45b0 60 sf_m_b = 0; // 接收模式
WXD 5:6a95726e45b0 61 wait_us(200); // 等待选择引脚电压稳定
WXD 5:6a95726e45b0 62 wait_ms(3); // 等待从机上传数据
WXD 5:6a95726e45b0 63 gait_decode();
WXD 5:6a95726e45b0 64 //pc.printf("A1B: %d - %d - %f - %f - %f\n\r", Gait_num_valid, Gait_now, Gait_per_now, COP_Y, COP_X);
WXD 5:6a95726e45b0 65 gait_clear();
WXD 5:6a95726e45b0 66
WXD 5:6a95726e45b0 67 sf_m_b = 1; // 发送模式
WXD 5:6a95726e45b0 68 wait_us(200); // 等待选择引脚电压稳定
WXD 5:6a95726e45b0 69 board.printf("A2B");
WXD 5:6a95726e45b0 70 wait_ms(1); // mbed特色，需要等待数据完全发送
WXD 5:6a95726e45b0 71 sf_m_b = 0; // 接收模式
WXD 5:6a95726e45b0 72 wait_us(200); // 等待选择引脚电压稳定
WXD 5:6a95726e45b0 73 wait_ms(4); // 等待从机上传数据
WXD 5:6a95726e45b0 74 ad_decode();
WXD 5:6a95726e45b0 75 //pc.printf("A2B: %f - %f\n\r", ad_pf, ad_df);
WXD 5:6a95726e45b0 76 ad_clear();
WXD 5:6a95726e45b0 77 /**********************************************************************/
WXD 5:6a95726e45b0 78
WXD 5:6a95726e45b0 79 ////////////////////////// 电机状态控制，命令发送 //////////////////////////
WXD 5:6a95726e45b0 80 control();
WXD 5:6a95726e45b0 81
WXD 5:6a95726e45b0 82 if(send_enable == 1)
WXD 5:6a95726e45b0 83 {
WXD 5:6a95726e45b0 84 PackAll();
WXD 5:6a95726e45b0 85 WriteAll();
benkatz 3:9ef9b4c66648 86 }
WXD 5:6a95726e45b0 87 /**********************************************************************/
benkatz 3:9ef9b4c66648 88
WXD 5:6a95726e45b0 89 ////////////////////////////// 输出状态信息 //////////////////////////////
WXD 5:6a95726e45b0 90 /*
WXD 5:6a95726e45b0 91 if(counter >= 20000)
WXD 5:6a95726e45b0 92 {
WXD 5:6a95726e45b0 93 //pc.printf("\n\rpf- %.3f %.3f %.3f \n\r", a_state.pf.p, a_state.pf.v, a_state.pf.t);
WXD 5:6a95726e45b0 94 //pc.printf("df- %.3f %.3f %.3f \n\r", a_state.df.p, a_state.df.v, a_state.df.t);
WXD 5:6a95726e45b0 95
WXD 5:6a95726e45b0 96 counter = 0;
WXD 5:6a95726e45b0 97 }
WXD 5:6a95726e45b0 98 */
benkatz 3:9ef9b4c66648 99
WXD 5:6a95726e45b0 100 //pc.printf("\n\rPpd: %f - %f\r", SP_pf, SP_df);
WXD 5:6a95726e45b0 101 //pc.printf("\n\rPpd: %f - %f\r", a_state.pf.p, a_state.df.p);
WXD 5:6a95726e45b0 102 pc.printf("A1B: %d - %d - %f - %f - %f\r", Gait_num_valid_real, Gait_now_real, Gait_per_now_real, COP_Y_real, COP_X_real);
WXD 5:6a95726e45b0 103 //pc.printf("A2B: %f - %f\n\r", ad_pf_real, ad_df_real);
WXD 5:6a95726e45b0 104 /**********************************************************************/
benkatz 3:9ef9b4c66648 105 }
WXD 5:6a95726e45b0 106 }