control.cpp

00001 #include "control.h"
00002 #include "mbed.h"
00003 #include "data_pc.h"
00004 #define GET_LOW_BYTE(A) ((uint8_t)(A))
00005 //宏函数 获得A的低八位
00006 #define GET_HIGH_BYTE(A) ((uint8_t)((A) >> 8))
00007 //宏函数 获得A的高八位
00008 
00009 uint8_t LobotTxBuf[128];  
00010 uint16_t batteryVolt;
00011 uint8_t LobotRxBuf[16];  
00012 
00013 /*********************************************************************************
00014  * Function:  moveServo
00015  * Description： 控制单个舵机转动
00016  * Parameters:   sevoID:舵机ID，Position:目标位置,Time:转动时间
00017                     舵机ID取值:0<=舵机ID<=31,Time取值: Time > 0
00018  * Return:       无返回
00019  * Others:
00020  **********************************************************************************/
00021 
00022 void moveServo(uint8_t servoID, uint16_t Position, uint16_t Time)
00023 {
00024     
00025 
00026     uint8_t i;
00027     if (servoID > 31 || !(Time > 0)) {  //舵机ID不能打于31,可根据对应控制板修改
00028         return;
00029     }
00030     LobotTxBuf[0] = LobotTxBuf[1] = FRAME_HEADER;    //填充帧头
00031     LobotTxBuf[2] = 8;
00032     LobotTxBuf[3] = CMD_SERVO_MOVE;           //数据长度=要控制舵机数*3+5，此处=1*3+5//填充舵机移动指令
00033     LobotTxBuf[4] = 1;                        //要控制的舵机个数
00034     LobotTxBuf[5] = GET_LOW_BYTE(Time);       //取得时间的低八位
00035     LobotTxBuf[6] = GET_HIGH_BYTE(Time);      //取得时间的高八位
00036     LobotTxBuf[7] = servoID;                  //舵机ID
00037     LobotTxBuf[8] = GET_LOW_BYTE(Position);   //取得目标位置的低八位
00038     LobotTxBuf[9] = GET_HIGH_BYTE(Position);  //取得目标位置的高八位
00039    // shouzhua.write( *(&LobotTxBuf),10);
00040 
00041    for(i=0;i<10;i++)
00042     {
00043     shouzhua.putc( LobotTxBuf[i]);
00044     }
00045     //uartWriteBuf(LobotTxBuf, 10);  
00046    
00047 }
00048 
00049 /*
00050 void getBatteryVoltage(void)
00051 {
00052 //  uint16_t Voltage = 0;
00053     uint8_t i;
00054     LobotTxBuf[0] = FRAME_HEADER;  //填充帧头
00055     LobotTxBuf[1] = FRAME_HEADER;
00056     LobotTxBuf[2] = 2;             //数据长度，数据帧除帧头部分数据字节数，此命令固定为2
00057     LobotTxBuf[3] = CMD_GET_BATTERY_VOLTAGE;  //填充获取电池电压命令
00058 
00059     //uartWriteBuf(LobotTxBuf, 4);   //发送
00060     for(i=0;i<4;i++)
00061     {
00062     shouzhua.putc(LobotTxBuf[i]);
00063     }
00064 }
00065 
00066 void receiveHandle()
00067 {
00068    
00069     //可以根据二次开发手册添加其他指令
00070     if (isUartRxCompleted) {
00071         isUartRxCompleted = false;
00072         switch (LobotRxBuf[3]) {
00073         case CMD_GET_BATTERY_VOLTAGE: //获取电压
00074             batteryVolt = (((uint16_t)(LobotRxBuf[5])) << 8) | (LobotRxBuf[4]);
00075             break;
00076         default:
00077             break;
00078         }
00079     }
00080 }
00081 
00082 */
00083 void shouzhuaprocess(void)
00084  {
00085     
00086     //NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//shezhi zhongduanyouxianji
00087     moveServo(1, 2000, 1000); //1s移动1号舵机至2000位置
00088     wait(3);
00089 }
00090 /*
00091 void BaterryOut(void)
00092 {
00093   uint8_t z;
00094   receiveHandle(); //接收处理
00095         //delay_ms(1);
00096         wait(0.001);
00097         
00098         z++;
00099         if(z ==2000)
00100             getBatteryVoltage(); //发送获取电池电压指令
00101         if(z == 3000) { 
00102             printf("Bvolt:%d mv\r\n",batteryVolt); //打印电池电压
00103             z = 0;  
00104 }
00105 }
00106 */