initial commit

by_cmd.c

@@ -0,0 +1,182 @@
+#include "by_cmd.h"
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <signal.h>
+#include <unistd.h>
+#include <pthread.h>
+#include "by_frame.h"
+#include "logc/log.h"
+
+uint8_t cmd;
+uint32_t received_data[2];
+
+uint8_t listerning_cmd;
+int received_flag;
+
+int by_cmd_init(void)
+{
+  log_debug("by_cmd init");
+  return (by_frame_init());
+}
+
+void *by_cmd_loop(void *timeout)
+{
+  while (*(int *)timeout > 0) {
+    int ret = by_frame_parse(&cmd, received_data);
+
+    if (!ret && cmd == listerning_cmd) {
+      //   log_debug("got frame cmd %2X", cmd);
+      break;
+    }
+    usleep(10);
+  }
+
+  received_flag  = 1;
+  listerning_cmd = 0;
+
+  return NULL;
+}
+
+/**
+ * @brief 注册监听的
+ *s
+ * @param cmd
+ */
+int by_cmd_reg_listerning(uint8_t cmd, int timeout)
+{
+  listerning_cmd = cmd;
+  received_flag  = 0;
+
+  pthread_t thread_listerning;
+  pthread_create(&thread_listerning, NULL, by_cmd_loop, &timeout);
+
+  while (timeout--) {
+    if (received_flag) {
+      // log_debug("received done! using %fSEC", (float)timeout_cnt * 0.00005);
+      listerning_cmd = 0;
+      received_flag  = 0;
+      return 0;
+    }
+    usleep(10);
+  }
+
+  listerning_cmd = 0;
+  received_flag  = 0;
+
+  log_warn("get cmd time out");
+
+  return -1;
+}
+
+/**
+ * @brief 发送 x 方向速度指令（无应答）
+ *
+ * @param speed
+ */
+void by_cmd_send_speed_x(float speed)
+{
+  uint8_t buff[4] = {0};
+  memcpy(buff, &speed, 4);
+
+  by_frame_send(0x31, buff, 4);
+}
+
+/**
+ * @brief 发送 y 方向速度指令（无应答）
+ *
+ * @param speed
+ */
+void by_cmd_send_speed_y(float speed)
+{
+  uint8_t buff[4] = {0};
+  memcpy(buff, &speed, 4);
+
+  by_frame_send(0x32, buff, 4);
+}
+
+/**
+ * @brief 发送角速度指令（无应答）
+ *
+ * @param speed
+ */
+void by_cmd_send_speed_omega(float speed)
+{
+  uint8_t buff[4] = {0};
+  memcpy(buff, &speed, 4);
+
+  by_frame_send(0x33, buff, 4);
+}
+
+/**
+ * @brief 发送 x 方向移动距离指令（等待应答）
+ *
+ * @param speed
+ * @param time
+ * @return true 发送成功
+ * @return false 应答超时
+ */
+int by_cmd_send_distance_x(float speed, uint32_t time)
+{
+  uint8_t buff[4] = {0};
+  memcpy(buff, &speed, 4);
+  memcpy(buff + 4, &time, 4);
+  by_frame_send(0x34, buff, 8);
+  return (by_cmd_reg_listerning(0x34, 1000));
+}
+
+/**
+ * @brief 发送 y 方向移动距离指令（等待应答）
+ *
+ * @param speed
+ * @param time
+ * @return true 发送成功
+ * @return false 应答超时
+ */
+int by_cmd_send_distance_y(float speed, uint32_t time)
+{
+  uint8_t buff[4] = {0};
+  memcpy(buff, &speed, 4);
+  memcpy(buff + 4, &time, 4);
+  by_frame_send(0x35, buff, 8);
+  return (by_cmd_reg_listerning(0x35, 1000));
+}
+
+/**
+ * @brief 发送 omega 方向旋转角度指令（等待应答）
+ *
+ * @param speed
+ * @param time
+ * @return true 发送成功
+ * @return false 应答超时
+ */
+int by_cmd_send_angle_omega(float speed, uint32_t time)
+{
+  uint8_t buff[4] = {0};
+  memcpy(buff, &speed, 4);
+  memcpy(buff + 4, &time, 4);
+  by_frame_send(0x36, buff, 8);
+  return (by_cmd_reg_listerning(0x36, 1000));
+}
+
+int by_cmd_send_reset_axis_x(void)
+{
+  uint8_t buff[4] = {0};
+  by_frame_send(0x41, buff, 4);
+  return (by_cmd_reg_listerning(0x41, 1000));
+}
+
+int by_cmd_send_reset_axis_z(void)
+{
+  uint8_t buff[4] = {0};
+  by_frame_send(0x42, buff, 4);
+  return (by_cmd_reg_listerning(0x42, 1000));
+}
+
+int by_cmd_send_reset_end_effector(void)
+{
+  uint8_t buff[4] = {0};
+  by_frame_send(0x43, buff, 4);
+  return (by_cmd_reg_listerning(0x43, 1000));
+}