Shao Rui / CH_Communicatuin_Test

123

Dependencies:   mbed-dev_spine

Control/control.cpp@9:bf02fd2d7a0a, 2020-02-19 (annotated)

Committer:
shaorui
Date:
Wed Feb 19 05:22:32 2020 +0000
Revision:
9:bf02fd2d7a0a
Parent:
8:95a914f962bd 
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shaorui 8:95a914f962bd 1 #include "control.h"
shaorui 8:95a914f962bd 2 #include "mbed.h"
shaorui 8:95a914f962bd 3 #include "data_pc.h"
shaorui 8:95a914f962bd 4 #define GET_LOW_BYTE(A) ((uint8_t)(A))
shaorui 8:95a914f962bd 5 //宏函数 获得A的低八位
shaorui 8:95a914f962bd 6 #define GET_HIGH_BYTE(A) ((uint8_t)((A) >> 8))
shaorui 8:95a914f962bd 7 //宏函数 获得A的高八位
shaorui 8:95a914f962bd 8
shaorui 8:95a914f962bd 9 uint8_t LobotTxBuf[128];
shaorui 8:95a914f962bd 10 uint16_t batteryVolt;
shaorui 8:95a914f962bd 11 uint8_t LobotRxBuf[16];
shaorui 8:95a914f962bd 12
shaorui 8:95a914f962bd 13 /*********************************************************************************
shaorui 8:95a914f962bd 14 * Function: moveServo
shaorui 8:95a914f962bd 15 * Description： 控制单个舵机转动
shaorui 8:95a914f962bd 16 * Parameters: sevoID:舵机ID，Position:目标位置,Time:转动时间
shaorui 8:95a914f962bd 17 舵机ID取值:0<=舵机ID<=31,Time取值: Time > 0
shaorui 8:95a914f962bd 18 * Return: 无返回
shaorui 8:95a914f962bd 19 * Others:
shaorui 8:95a914f962bd 20 **********************************************************************************/
shaorui 8:95a914f962bd 21
shaorui 8:95a914f962bd 22 void moveServo(uint8_t servoID, uint16_t Position, uint16_t Time)
shaorui 8:95a914f962bd 23 {
shaorui 8:95a914f962bd 24
shaorui 8:95a914f962bd 25
shaorui 8:95a914f962bd 26 uint8_t i;
shaorui 8:95a914f962bd 27 if (servoID > 31 || !(Time > 0)) { //舵机ID不能打于31,可根据对应控制板修改
shaorui 8:95a914f962bd 28 return;
shaorui 8:95a914f962bd 29 }
shaorui 8:95a914f962bd 30 LobotTxBuf[0] = LobotTxBuf[1] = FRAME_HEADER; //填充帧头
shaorui 8:95a914f962bd 31 LobotTxBuf[2] = 8;
shaorui 8:95a914f962bd 32 LobotTxBuf[3] = CMD_SERVO_MOVE; //数据长度=要控制舵机数*3+5，此处=1*3+5//填充舵机移动指令
shaorui 8:95a914f962bd 33 LobotTxBuf[4] = 1; //要控制的舵机个数
shaorui 8:95a914f962bd 34 LobotTxBuf[5] = GET_LOW_BYTE(Time); //取得时间的低八位
shaorui 8:95a914f962bd 35 LobotTxBuf[6] = GET_HIGH_BYTE(Time); //取得时间的高八位
shaorui 8:95a914f962bd 36 LobotTxBuf[7] = servoID; //舵机ID
shaorui 8:95a914f962bd 37 LobotTxBuf[8] = GET_LOW_BYTE(Position); //取得目标位置的低八位
shaorui 8:95a914f962bd 38 LobotTxBuf[9] = GET_HIGH_BYTE(Position); //取得目标位置的高八位
shaorui 8:95a914f962bd 39 // shouzhua.write( *(&LobotTxBuf),10);
shaorui 8:95a914f962bd 40
shaorui 8:95a914f962bd 41 for(i=0;i<10;i++)
shaorui 8:95a914f962bd 42 {
shaorui 8:95a914f962bd 43 shouzhua.putc( LobotTxBuf[i]);
shaorui 8:95a914f962bd 44 }
shaorui 8:95a914f962bd 45 //uartWriteBuf(LobotTxBuf, 10);
shaorui 8:95a914f962bd 46
shaorui 8:95a914f962bd 47 }
shaorui 8:95a914f962bd 48
shaorui 8:95a914f962bd 49 /*
shaorui 8:95a914f962bd 50 void getBatteryVoltage(void)
shaorui 8:95a914f962bd 51 {
shaorui 8:95a914f962bd 52 // uint16_t Voltage = 0;
shaorui 8:95a914f962bd 53 uint8_t i;
shaorui 8:95a914f962bd 54 LobotTxBuf[0] = FRAME_HEADER; //填充帧头
shaorui 8:95a914f962bd 55 LobotTxBuf[1] = FRAME_HEADER;
shaorui 8:95a914f962bd 56 LobotTxBuf[2] = 2; //数据长度，数据帧除帧头部分数据字节数，此命令固定为2
shaorui 8:95a914f962bd 57 LobotTxBuf[3] = CMD_GET_BATTERY_VOLTAGE; //填充获取电池电压命令
shaorui 8:95a914f962bd 58
shaorui 8:95a914f962bd 59 //uartWriteBuf(LobotTxBuf, 4); //发送
shaorui 8:95a914f962bd 60 for(i=0;i<4;i++)
shaorui 8:95a914f962bd 61 {
shaorui 8:95a914f962bd 62 shouzhua.putc(LobotTxBuf[i]);
shaorui 8:95a914f962bd 63 }
shaorui 8:95a914f962bd 64 }
shaorui 8:95a914f962bd 65
shaorui 8:95a914f962bd 66 void receiveHandle()
shaorui 8:95a914f962bd 67 {
shaorui 8:95a914f962bd 68
shaorui 8:95a914f962bd 69 //可以根据二次开发手册添加其他指令
shaorui 8:95a914f962bd 70 if (isUartRxCompleted) {
shaorui 8:95a914f962bd 71 isUartRxCompleted = false;
shaorui 8:95a914f962bd 72 switch (LobotRxBuf[3]) {
shaorui 8:95a914f962bd 73 case CMD_GET_BATTERY_VOLTAGE: //获取电压
shaorui 8:95a914f962bd 74 batteryVolt = (((uint16_t)(LobotRxBuf[5])) << 8) | (LobotRxBuf[4]);
shaorui 8:95a914f962bd 75 break;
shaorui 8:95a914f962bd 76 default:
shaorui 8:95a914f962bd 77 break;
shaorui 8:95a914f962bd 78 }
shaorui 8:95a914f962bd 79 }
shaorui 8:95a914f962bd 80 }
shaorui 8:95a914f962bd 81
shaorui 8:95a914f962bd 82 */
shaorui 8:95a914f962bd 83 void shouzhuaprocess(void)
shaorui 8:95a914f962bd 84 {
shaorui 8:95a914f962bd 85
shaorui 8:95a914f962bd 86 //NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//shezhi zhongduanyouxianji
shaorui 8:95a914f962bd 87 moveServo(1, 2000, 1000); //1s移动1号舵机至2000位置
shaorui 8:95a914f962bd 88 wait(3);
shaorui 8:95a914f962bd 89 }
shaorui 8:95a914f962bd 90 /*
shaorui 8:95a914f962bd 91 void BaterryOut(void)
shaorui 8:95a914f962bd 92 {
shaorui 8:95a914f962bd 93 uint8_t z;
shaorui 8:95a914f962bd 94 receiveHandle(); //接收处理
shaorui 8:95a914f962bd 95 //delay_ms(1);
shaorui 8:95a914f962bd 96 wait(0.001);
shaorui 8:95a914f962bd 97
shaorui 8:95a914f962bd 98 z++;
shaorui 8:95a914f962bd 99 if(z ==2000)
shaorui 8:95a914f962bd 100 getBatteryVoltage(); //发送获取电池电压指令
shaorui 8:95a914f962bd 101 if(z == 3000) {
shaorui 8:95a914f962bd 102 printf("Bvolt:%d mv\r\n",batteryVolt); //打印电池电压
shaorui 8:95a914f962bd 103 z = 0;
shaorui 8:95a914f962bd 104 }
shaorui 8:95a914f962bd 105 }
shaorui 8:95a914f962bd 106 */