1
Diff: Control/control.cpp
- Revision:
- 8:95a914f962bd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Control/control.cpp Tue Jan 07 09:23:24 2020 +0000 @@ -0,0 +1,106 @@ +#include "control.h" +#include "mbed.h" +#include "data_pc.h" +#define GET_LOW_BYTE(A) ((uint8_t)(A)) +//宏函数 获得A的低八位 +#define GET_HIGH_BYTE(A) ((uint8_t)((A) >> 8)) +//宏函数 获得A的高八位 + +uint8_t LobotTxBuf[128]; +uint16_t batteryVolt; +uint8_t LobotRxBuf[16]; + +/********************************************************************************* + * Function: moveServo + * Description: 控制单个舵机转动 + * Parameters: sevoID:舵机ID,Position:目标位置,Time:转动时间 + 舵机ID取值:0<=舵机ID<=31,Time取值: Time > 0 + * Return: 无返回 + * Others: + **********************************************************************************/ + +void moveServo(uint8_t servoID, uint16_t Position, uint16_t Time) +{ + + + uint8_t i; + if (servoID > 31 || !(Time > 0)) { //舵机ID不能打于31,可根据对应控制板修改 + return; + } + LobotTxBuf[0] = LobotTxBuf[1] = FRAME_HEADER; //填充帧头 + LobotTxBuf[2] = 8; + LobotTxBuf[3] = CMD_SERVO_MOVE; //数据长度=要控制舵机数*3+5,此处=1*3+5//填充舵机移动指令 + LobotTxBuf[4] = 1; //要控制的舵机个数 + LobotTxBuf[5] = GET_LOW_BYTE(Time); //取得时间的低八位 + LobotTxBuf[6] = GET_HIGH_BYTE(Time); //取得时间的高八位 + LobotTxBuf[7] = servoID; //舵机ID + LobotTxBuf[8] = GET_LOW_BYTE(Position); //取得目标位置的低八位 + LobotTxBuf[9] = GET_HIGH_BYTE(Position); //取得目标位置的高八位 + // shouzhua.write( *(&LobotTxBuf),10); + + for(i=0;i<10;i++) + { + shouzhua.putc( LobotTxBuf[i]); + } + //uartWriteBuf(LobotTxBuf, 10); + +} + +/* +void getBatteryVoltage(void) +{ +// uint16_t Voltage = 0; + uint8_t i; + LobotTxBuf[0] = FRAME_HEADER; //填充帧头 + LobotTxBuf[1] = FRAME_HEADER; + LobotTxBuf[2] = 2; //数据长度,数据帧除帧头部分数据字节数,此命令固定为2 + LobotTxBuf[3] = CMD_GET_BATTERY_VOLTAGE; //填充获取电池电压命令 + + //uartWriteBuf(LobotTxBuf, 4); //发送 + for(i=0;i<4;i++) + { + shouzhua.putc(LobotTxBuf[i]); + } +} + +void receiveHandle() +{ + + //可以根据二次开发手册添加其他指令 + if (isUartRxCompleted) { + isUartRxCompleted = false; + switch (LobotRxBuf[3]) { + case CMD_GET_BATTERY_VOLTAGE: //获取电压 + batteryVolt = (((uint16_t)(LobotRxBuf[5])) << 8) | (LobotRxBuf[4]); + break; + default: + break; + } + } +} + +*/ +void shouzhuaprocess(void) + { + + //NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//shezhi zhongduanyouxianji + moveServo(1, 2000, 1000); //1s移动1号舵机至2000位置 + wait(3); +} +/* +void BaterryOut(void) +{ + uint8_t z; + receiveHandle(); //接收处理 + //delay_ms(1); + wait(0.001); + + z++; + if(z ==2000) + getBatteryVoltage(); //发送获取电池电压指令 + if(z == 3000) { + printf("Bvolt:%d mv\r\n",batteryVolt); //打印电池电压 + z = 0; +} +} +*/ \ No newline at end of file