增加云台相关指令

.vscode/settings.json

@@ -4,6 +4,7 @@
         "string.h": "c",
         "stdint.h": "c",
         "by_serial.h": "c",
-        "log.h": "c"
+        "log.h": "c",
+        "termios.h": "c"
     }
 }

by_cmd.c

@@ -114,12 +114,12 @@ void by_cmd_send_speed_omega(float speed)
  *
  * @param speed
  * @param time
- * @return true 发送成功
- * @return false 应答超时
+ * @return 0 发送成功
+ * @return -1 应答超时
  */
 int by_cmd_send_distance_x(float speed, uint32_t time)
 {
-  uint8_t buff[4] = {0};
+  uint8_t buff[8] = {0};
   memcpy(buff, &speed, 4);
   memcpy(buff + 4, &time, 4);
   by_frame_send(0x34, buff, 8);
@@ -136,7 +136,7 @@ int by_cmd_send_distance_x(float speed, uint32_t time)
  */
 int by_cmd_send_distance_y(float speed, uint32_t time)
 {
-  uint8_t buff[4] = {0};
+  uint8_t buff[8] = {0};
   memcpy(buff, &speed, 4);
   memcpy(buff + 4, &time, 4);
   by_frame_send(0x35, buff, 8);
@@ -179,4 +179,75 @@ 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));
+}
+
+int by_cmd_send_distance_axis_x(uint8_t speed, float distance)
+{
+  uint8_t buff[8] = {0};
+  memcpy(buff, &speed, 1);
+  memcpy(buff + 4, &distance, 4);
+  by_frame_send(0x44, buff, 8);
+  return (by_cmd_reg_listerning(0x44, 1000));
+}
+
+int by_cmd_send_distance_axis_z(uint8_t speed, float distance)
+{
+  uint8_t buff[8] = {0};
+  memcpy(buff, &speed, 1);
+  memcpy(buff + 4, &distance, 4);
+  by_frame_send(0x46, buff, 8);
+  return (by_cmd_reg_listerning(0x46, 1000));
+}
+
+int by_cmd_send_position_axis_x(uint8_t speed, float position)
+{
+  uint8_t buff[8] = {0};
+  memcpy(buff, &speed, 1);
+  memcpy(buff + 4, &position, 4);
+  by_frame_send(0x47, buff, 8);
+  return (by_cmd_reg_listerning(0x47, 1000));
+}
+
+int by_cmd_send_position_axis_z(uint8_t speed, float position)
+{
+  uint8_t buff[8] = {0};
+  memcpy(buff, &speed, 1);
+  memcpy(buff + 4, &position, 4);
+  by_frame_send(0x49, buff, 8);
+  return (by_cmd_reg_listerning(0x49, 1000));
+}
+
+int by_cmd_send_angle_claw_arm(float angle)
+{
+  uint8_t buff[4] = {0};
+  memcpy(buff, &angle, 4);
+  by_frame_send(0x50, buff, 4);
+  return (by_cmd_reg_listerning(0x50, 1000));
+}
+
+int by_cmd_send_angle_claw(float angle)
+{
+  uint8_t buff[4] = {0};
+  memcpy(buff, &angle, 4);
+  by_frame_send(0x51, buff, 4);
+  return (by_cmd_reg_listerning(0x51, 1000));
+}
+
+int by_cmd_send_ranging_start(void)
+{
+  uint8_t buff[4] = {0};
+  by_frame_send(0x55, buff, 4);
+  return (by_cmd_reg_listerning(0x55, 1000));
+}
+
+int by_cmd_recv_ranging_data(float *distance)
+{
+  uint8_t buff[4] = {0};
+  by_frame_send(0x56, buff, 4);
+  if (!by_cmd_reg_listerning(0x56, 1000)) {
+    memcpy(distance, &received_data[0], 4);
+    return 0;
+  } else {
+    return -1;
+  }
 }

by_cmd.h

@@ -19,4 +19,15 @@ int by_cmd_send_reset_axis_z(void);
 int by_cmd_send_reset_axis_x(void);
 int by_cmd_send_reset_end_effector(void);
 
+int by_cmd_send_distance_axis_x(uint8_t speed, float distance);
+int by_cmd_send_distance_axis_z(uint8_t speed, float distance);
+int by_cmd_send_position_axis_x(uint8_t speed, float position);
+int by_cmd_send_position_axis_z(uint8_t speed, float position);
+
+int by_cmd_send_angle_claw_arm(float angle);
+int by_cmd_send_angle_claw(float angle);
+
+int by_cmd_send_ranging_start(void);
+int by_cmd_recv_ranging_data(float *distance);
+
 #endif

by_frame.c

@@ -93,17 +93,17 @@ void by_frame_send(uint8_t cmd, uint8_t *data_array, uint8_t len)
   frame_buffer_send[0] = BY_FRAME_HEAD;
   frame_buffer_send[1] = cmd;
 
-  if (len > data_byte_num) {
-    len = data_byte_num;
+  if (len > BY_FRANE_DATA_LEN) {
+    len = BY_FRANE_DATA_LEN;
   }
 
   memcpy(frame_buffer_send + 2, data_array, len);
-  crc_cal = crc16_check(frame_buffer_send, 2 + data_byte_num);
+  crc_cal = crc16_check(frame_buffer_send, 2 + BY_FRANE_DATA_LEN);
 
-  frame_buffer_send[2 + data_byte_num] = (uint8_t)(crc_cal >> 8);
-  frame_buffer_send[3 + data_byte_num] = (uint8_t)(crc_cal);
+  frame_buffer_send[2 + BY_FRANE_DATA_LEN] = (uint8_t)(crc_cal >> 8);
+  frame_buffer_send[3 + BY_FRANE_DATA_LEN] = (uint8_t)(crc_cal);
 
-  by_serial_write(&serial_port, frame_buffer_send, 4 + data_byte_num);
+  by_serial_write(&serial_port, frame_buffer_send, BY_FRANE_LEN);
 }
 
 /**

test.c

@@ -5,6 +5,8 @@
 #include "by_cmd.h"
 #include "logc/log.h"
 
+#include "by_serial.h"
+
 char test_data[] = "hello world\r\n";
 char buff[20]    = {0};
 
@@ -13,24 +15,10 @@ uint32_t buff_temp[20];
 
 int main(int argc, char **argv)
 {
-  // by_serial_t serial;
-  // by_serial_init(&serial, "/dev/ttyTHS0");
-
-  // while (1) {
-  //   // by_serial_write(&serial, test_data, sizeof(test_data) - 1);
-  //   // if (0 < by_serial_read(&serial, buff, 1)) {
-  //   //   printf("%c", buff[0]);
-  //   // }
-  //   if (by_serial_get_used_buffer_len(&serial) > 1) {
-  //     by_serial_read(&serial, buff, 6);
-  //   }
-  //   printf("used buff len %d\r\n", by_serial_get_used_buffer_len(&serial));
-  //   sleep(1);
-  // }
-
-  by_cmd_init();
-
-  by_cmd_send_distance_x(20.0, 1000);
+  if (by_cmd_init()) {
+    log_error("Program exits abnormally");
+    return -1;
+  }
 
   return 0;
 }