Shao Rui / CH_Communicatuin_Test2

1

Dependencies:   mbed-dev_spine

main.cpp@6:aad89fd109c2, 2019-12-02 (annotated)

Committer:
WXD
Date:
Mon Dec 02 14:22:07 2019 +0000
Revision:
6:aad89fd109c2
Parent:
5:6a95726e45b0
Child:
7:4362ea3d5300
123

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);
WXD 6:aad89fd109c2 18 dianciji.baud(115200);
WXD 6:aad89fd109c2 19 shouzhua.baud(115200);
benkatz 0:d6186b8990c5 20
WXD 5:6a95726e45b0 21 command.baud(115200);
WXD 5:6a95726e45b0 22 command.attach(&serial_command_isr);
WXD 6:aad89fd109c2 23 //sf_m_c = 0; // 主要为接收模式
WXD 6:aad89fd109c2 24
WXD 6:aad89fd109c2 25
WXD 5:6a95726e45b0 26
WXD 5:6a95726e45b0 27 board.baud(115200);
WXD 5:6a95726e45b0 28 board.attach(&serial_board_isr);
WXD 5:6a95726e45b0 29 sf_m_b = 1; // 主要为发送模式
benkatz 1:79e0d4791936 30
WXD 5:6a95726e45b0 31 pf_can.frequency(1000000);
WXD 5:6a95726e45b0 32 pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
WXD 5:6a95726e45b0 33 pf_rxMsg.len = 6;
WXD 5:6a95726e45b0 34 PF_can.len = 8;
WXD 5:6a95726e45b0 35 PF_can.id = 0x0a;
benkatz 0:d6186b8990c5 36
WXD 5:6a95726e45b0 37 df_can.frequency(1000000);
WXD 5:6a95726e45b0 38 df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
WXD 5:6a95726e45b0 39 df_rxMsg.len = 6;
WXD 5:6a95726e45b0 40 DF_can.len = 8;
WXD 5:6a95726e45b0 41 DF_can.id = 0x0b;
benkatz 1:79e0d4791936 42
WXD 5:6a95726e45b0 43 NVIC_SetPriority(USART1_IRQn, 3); // pc中断优先级高于board
WXD 5:6a95726e45b0 44 NVIC_SetPriority(USART2_IRQn, 4);
benkatz 3:9ef9b4c66648 45
benkatz 3:9ef9b4c66648 46
WXD 5:6a95726e45b0 47 while(1)
WXD 5:6a95726e45b0 48 {
WXD 5:6a95726e45b0 49 counter++;
WXD 5:6a95726e45b0 50
WXD 6:aad89fd109c2 51 //获取AD
WXD 6:aad89fd109c2 52 ad1 = AD1.read() * 3300;
WXD 6:aad89fd109c2 53 ad2 = AD2.read() * 3300;
WXD 6:aad89fd109c2 54
WXD 5:6a95726e45b0 55 //////////////////////// CAN获取电机位置速度信息 //////////////////////////
WXD 5:6a95726e45b0 56 pf_can.read(pf_rxMsg);
WXD 5:6a95726e45b0 57 unpack_reply(pf_rxMsg, &a_state);
WXD 5:6a95726e45b0 58 wait_us(10);
WXD 5:6a95726e45b0 59 df_can.read(df_rxMsg);
WXD 5:6a95726e45b0 60 unpack_reply(df_rxMsg, &a_state);
WXD 6:aad89fd109c2 61
WXD 6:aad89fd109c2 62 ppf = a_state.pf.p; // 从CAN获得的当前位置
WXD 6:aad89fd109c2 63 pdf = a_state.df.p;
WXD 6:aad89fd109c2 64 //====================================================================//
WXD 5:6a95726e45b0 65
WXD 5:6a95726e45b0 66 /////////////////////// 485获取步态，力传感器信息 //////////////////////////
WXD 5:6a95726e45b0 67 sf_m_b = 1; // 发送模式
WXD 5:6a95726e45b0 68 wait_us(200); // 等待选择引脚电压稳定
WXD 5:6a95726e45b0 69 board.printf("A1B");
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(3); // 等待从机上传数据
WXD 5:6a95726e45b0 74 gait_decode();
WXD 5:6a95726e45b0 75 //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 76 gait_clear();
WXD 5:6a95726e45b0 77
WXD 5:6a95726e45b0 78 sf_m_b = 1; // 发送模式
WXD 5:6a95726e45b0 79 wait_us(200); // 等待选择引脚电压稳定
WXD 5:6a95726e45b0 80 board.printf("A2B");
WXD 5:6a95726e45b0 81 wait_ms(1); // mbed特色，需要等待数据完全发送
WXD 5:6a95726e45b0 82 sf_m_b = 0; // 接收模式
WXD 5:6a95726e45b0 83 wait_us(200); // 等待选择引脚电压稳定
WXD 5:6a95726e45b0 84 wait_ms(4); // 等待从机上传数据
WXD 5:6a95726e45b0 85 ad_decode();
WXD 5:6a95726e45b0 86 //pc.printf("A2B: %f - %f\n\r", ad_pf, ad_df);
WXD 5:6a95726e45b0 87 ad_clear();
WXD 6:aad89fd109c2 88 //====================================================================//
WXD 5:6a95726e45b0 89
WXD 5:6a95726e45b0 90 ////////////////////////// 电机状态控制，命令发送 //////////////////////////
WXD 5:6a95726e45b0 91 control();
WXD 5:6a95726e45b0 92
WXD 5:6a95726e45b0 93 if(send_enable == 1)
WXD 5:6a95726e45b0 94 {
WXD 5:6a95726e45b0 95 PackAll();
WXD 5:6a95726e45b0 96 WriteAll();
WXD 6:aad89fd109c2 97 //send_enable = 0;
benkatz 3:9ef9b4c66648 98 }
WXD 5:6a95726e45b0 99 /**********************************************************************/
benkatz 3:9ef9b4c66648 100
WXD 5:6a95726e45b0 101 ////////////////////////////// 输出状态信息 //////////////////////////////
WXD 5:6a95726e45b0 102 /*
WXD 5:6a95726e45b0 103 if(counter >= 20000)
WXD 5:6a95726e45b0 104 {
WXD 5:6a95726e45b0 105 //pc.printf("\n\rpf- %.3f %.3f %.3f \n\r", a_state.pf.p, a_state.pf.v, a_state.pf.t);
WXD 5:6a95726e45b0 106 //pc.printf("df- %.3f %.3f %.3f \n\r", a_state.df.p, a_state.df.v, a_state.df.t);
WXD 5:6a95726e45b0 107
WXD 5:6a95726e45b0 108 counter = 0;
WXD 5:6a95726e45b0 109 }
WXD 5:6a95726e45b0 110 */
benkatz 3:9ef9b4c66648 111
WXD 5:6a95726e45b0 112 //pc.printf("\n\rPpd: %f - %f\r", SP_pf, SP_df);
WXD 6:aad89fd109c2 113 pc.printf("\n\rC: %f - %f\r", a_control.pf.p_des, a_control.df.p_des);
WXD 6:aad89fd109c2 114 pc.printf("\n\rP: %f - %f\r", a_state.pf.p, a_state.df.p);
WXD 6:aad89fd109c2 115
WXD 6:aad89fd109c2 116 //dianciji.printf(); //电刺激输出
WXD 6:aad89fd109c2 117 //shouzhua.printf(); //手抓输出
WXD 6:aad89fd109c2 118
WXD 6:aad89fd109c2 119 //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 120 //pc.printf("A2B: %f - %f\n\r", ad_pf_real, ad_df_real);
WXD 5:6a95726e45b0 121 /**********************************************************************/
benkatz 3:9ef9b4c66648 122 }
WXD 5:6a95726e45b0 123 }