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