Shao Rui / CH_Communicatuin_Test2

1

Dependencies:   mbed-dev_spine

main.cpp@11:360230a179b9, 2020-03-13 (annotated)

Committer:
shaorui
Date:
Fri Mar 13 03:38:13 2020 +0000
Revision:
11:360230a179b9
Parent:
10:f94e325fc1e6 
1

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
shaorui 8:95a914f962bd 16 pc.baud(115200);
shaorui 8:95a914f962bd 17 shouzhua.baud(9600);
shaorui 8:95a914f962bd 18 command.baud(115200);
shaorui 8:95a914f962bd 19 /*******shaorui add******
shaorui 8:95a914f962bd 20 EnterMotorMode(&EF_can); // 电机位置锁定
shaorui 8:95a914f962bd 21 send_enable = 0;
shaorui 8:95a914f962bd 22 state=MOTOR_MODE;
shaorui 8:95a914f962bd 23 pc.printf("Enter Motor Mode ");
shaorui 8:95a914f962bd 24 ***************************/
shaorui 8:95a914f962bd 25 command.attach(&serial_command_isr);
shaorui 8:95a914f962bd 26
shaorui 8:95a914f962bd 27 // 主要为接收模式 // 主要为发送模式
shaorui 8:95a914f962bd 28 ef_can.frequency(1000000);
shaorui 8:95a914f962bd 29 ef_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
shaorui 8:95a914f962bd 30 ef_rxMsg.len = 6;
shaorui 8:95a914f962bd 31 EF_can.len = 8;
shaorui 8:95a914f962bd 32 EF_can.id = 0x01;
benkatz 1:79e0d4791936 33
shaorui 8:95a914f962bd 34 wf_can.frequency(1000000);
shaorui 8:95a914f962bd 35 wf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
shaorui 8:95a914f962bd 36 wf_rxMsg.len = 6;
shaorui 8:95a914f962bd 37 WF_can.len = 8;
shaorui 8:95a914f962bd 38 WF_can.id = 0x0b;
benkatz 1:79e0d4791936 39
WXD 5:6a95726e45b0 40 NVIC_SetPriority(USART1_IRQn, 3); // pc中断优先级高于board
WXD 5:6a95726e45b0 41 NVIC_SetPriority(USART2_IRQn, 4);
benkatz 3:9ef9b4c66648 42
benkatz 3:9ef9b4c66648 43
WXD 5:6a95726e45b0 44 while(1)
WXD 5:6a95726e45b0 45 {
WXD 5:6a95726e45b0 46 counter++;
shaorui 8:95a914f962bd 47 //获取AD
WXD 6:aad89fd109c2 48 ad1 = AD1.read() * 3300;
WXD 6:aad89fd109c2 49 ad2 = AD2.read() * 3300;
shaorui 9:bf02fd2d7a0a 50 /********************AD 通讯协议***********************/
shaorui 9:bf02fd2d7a0a 51 //将ad的值用八位表示
shaorui 9:bf02fd2d7a0a 52 //7位-0表示ad1，1表示ad2
shaorui 9:bf02fd2d7a0a 53 //6-0位表示ad值
shaorui 9:bf02fd2d7a0a 54 ad1=ad1/26; //将ad1值限制在0-6bit,7bit set to be 0
shaorui 9:bf02fd2d7a0a 55 ad2=ad2/26+128; //将ad2值限制在0-6bit,7 bit set to be 1
shaorui 8:95a914f962bd 56 command.putc(ad1);
shaorui 8:95a914f962bd 57 command.putc(ad2);
shaorui 9:bf02fd2d7a0a 58 /********************AD 通讯协议***********************/
WXD 5:6a95726e45b0 59 //////////////////////// CAN获取电机位置速度信息 //////////////////////////
shaorui 8:95a914f962bd 60 ef_can.read(ef_rxMsg);
shaorui 8:95a914f962bd 61 unpack_reply(ef_rxMsg, &a_state);
WXD 5:6a95726e45b0 62 wait_us(10);
shaorui 8:95a914f962bd 63 wf_can.read(wf_rxMsg);
shaorui 8:95a914f962bd 64 unpack_reply(wf_rxMsg, &a_state);
WXD 5:6a95726e45b0 65
shaorui 8:95a914f962bd 66 pef = a_state.ef.p; // 从CAN获得的当前位置
shaorui 8:95a914f962bd 67 pwf = a_state.wf.p;
shaorui 8:95a914f962bd 68
shaorui 8:95a914f962bd 69 command_control();
WXD 5:6a95726e45b0 70
WXD 5:6a95726e45b0 71 if(send_enable == 1)
WXD 5:6a95726e45b0 72 {
WXD 5:6a95726e45b0 73 PackAll();
WXD 5:6a95726e45b0 74 WriteAll();
WXD 6:aad89fd109c2 75 //send_enable = 0;
benkatz 3:9ef9b4c66648 76 }
shaorui 8:95a914f962bd 77 if(duoji_command==1)
shaorui 11:360230a179b9 78 {
shaorui 11:360230a179b9 79 moveServo(1, shouzhua_control.p_des, 1000); //1s移动1号舵机至指定的位置
shaorui 11:360230a179b9 80 printf("Shouzhua Move Sucessfully1 ! \n\r");
ccxx1200 10:f94e325fc1e6 81 wait(1);
shaorui 8:95a914f962bd 82 }
shaorui 8:95a914f962bd 83
shaorui 8:95a914f962bd 84 //pc.printf("\n\rPpd: %f - %f\r", SP_pf, SP_df);
shaorui 8:95a914f962bd 85 // pc.printf("\n\rC: %f - %f\r", a_control.ef.p_des, a_control.wf.p_des);
shaorui 8:95a914f962bd 86 // pc.printf("\n\rP: %f - %f\r", a_state.ef.p, a_state.wf.p);
shaorui 8:95a914f962bd 87 pc.printf("%.3x,%.3x\n\r", EF_can.data[0],EF_can.data[1]);
shaorui 8:95a914f962bd 88 pc.printf("send_enbale:%.3d, p_des: %.3f\n\r", send_enable, a_control.ef.p_des);
shaorui 8:95a914f962bd 89 wait(2);
shaorui 8:95a914f962bd 90 pc.printf("AD: %.3f\n\r", ad2);
shaorui 8:95a914f962bd 91 }
WXD 5:6a95726e45b0 92
shaorui 8:95a914f962bd 93
WXD 5:6a95726e45b0 94 }