File content as of revision 9:bf02fd2d7a0a:

#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;  
}
}
*/