feat: 增加上下位机通信接口 (底盘速度和位置控制)

app/by_can.c

@@ -0,0 +1,40 @@
+#include "by_can.h"
+
+#include <string.h>
+#include <assert.h>
+#include "dwt_delay.h"
+#include "by_utils.h"
+#include "by_debug.h"
+
+by_error_status by_can_send_stdd(uint32_t id, const uint8_t *data, uint8_t len, uint16_t timeout)
+{
+
+  assert(id < 0x7FF);
+
+  if (len > 8) {
+    len = 8;
+  }
+
+  uint8_t transmit_mailbox;
+  can_tx_message_type tx_message_struct;
+  tx_message_struct.standard_id = id;                                /* 设置发送数据帧的 ID=0x400 */
+  tx_message_struct.extended_id = 0;                                 /* 不设置 */
+  tx_message_struct.id_type     = CAN_ID_STANDARD;                   /* 发送数据帧类型（标准/扩展）：标准数据帧 */
+  tx_message_struct.frame_type  = CAN_TFT_DATA;                      /* 发送帧类型（远程/数据）：数据帧 */
+  tx_message_struct.dlc         = len;                               /* 发送数据长度（0~8）：8 */
+  memcpy(tx_message_struct.data, data, len);                         /* 复制发送数据 */
+  transmit_mailbox = can_message_transmit(CAN1, &tx_message_struct); /* 将以上待发送报文写入发送邮箱并请求发送 */
+
+  /* 等待该邮箱发送成功—对应邮箱发送成功标志置起 */
+  while (can_transmit_status_get(CAN1, (can_tx_mailbox_num_type)transmit_mailbox) != CAN_TX_STATUS_SUCCESSFUL) {
+    // LOGD("CAN#SEND: timeout=%d", timeout);
+    if (0 == timeout--) {
+      LOGW("CAN#TIMEOUT: ID=0x%x", id);
+
+      return BY_ERROR;
+    }
+    DWT_Delay(10);
+  }
+
+  return BY_SUCCESS;
+}

app/by_can.h

@@ -0,0 +1,9 @@
+#ifndef _BY_CAN_H__
+#define _BY_CAN_H__
+
+#include "at32f403a_407.h"
+#include "by_utils.h"
+
+by_error_status by_can_send_stdd(uint32_t id, const uint8_t *data, uint8_t len, uint16_t timeout);
+
+#endif

app/by_frame.h

@@ -14,7 +14,7 @@
 
 #define BY_FRAME_UART_INDEX (USART3)
 
-#define BY_FRAME_DATA_NUM   (3)
+#define BY_FRAME_DATA_NUM   (2)
 
 extern void by_frame_init(void);
 void by_frame_send(uint8_t cmd, uint32_t *data_array);

app/by_messy.c

@@ -0,0 +1,74 @@
+#include "by_messy.h"
+
+#include "lwprintf.h"
+#include "by_frame.h"
+#include "by_motion.h"
+
+void by_messy_init(void)
+{
+  by_frame_init();
+}
+
+void by_messy_loop(void)
+{
+  uint8_t cmd                      = 0;
+  uint32_t buff[BY_FRAME_DATA_NUM] = {0};
+
+  if (!by_frame_parse(&cmd, buff)) {
+
+    lwprintf("get cmd: %X\r\n", cmd);
+
+    switch (cmd) {
+      case 0x31: // 设置速度 x
+        memcpy(&motion_speed_struct.v_x, buff, sizeof(motion_speed_struct.v_x));
+        by_motion_set_mode(0);
+        break;
+
+      case 0x32: // 设置速度 y
+        memcpy(&motion_speed_struct.v_y, buff, sizeof(motion_speed_struct.v_y));
+        by_motion_set_mode(0);
+        break;
+
+      case 0x33: // 设置速度 omega
+        memcpy(&motion_speed_struct.v_w, buff, sizeof(motion_speed_struct.v_w));
+        by_motion_set_mode(0);
+        break;
+
+      case 0x34:                  // 设置移动距离 x
+        by_frame_send(cmd, buff); // 正确接收后直接返回原文
+        memcpy(&motion_speed_struct.v_x, buff, sizeof(motion_speed_struct.v_x));
+        by_motion_set_mode(1);
+        motion_time_struct.t_x += buff[1];
+        break;
+
+      case 0x35:                  // 设置移动距离 y
+        by_frame_send(cmd, buff); // 正确接收后直接返回原文
+        memcpy(&motion_speed_struct.v_y, buff, sizeof(motion_speed_struct.v_y));
+        by_motion_set_mode(1);
+        motion_time_struct.t_y += buff[1];
+        break;
+
+      case 0x36:                  // 设置旋转角度 omega
+        by_frame_send(cmd, buff); // 正确接收后直接返回原文
+        memcpy(&motion_speed_struct.v_w, buff, sizeof(motion_speed_struct.v_w));
+        by_motion_set_mode(1);
+        motion_time_struct.t_w += buff[1];
+        break;
+
+      case 0x41:                  // 设置转台 x 轴复位
+        by_frame_send(cmd, buff); // 正确接收后直接返回原文
+        break;
+
+      case 0x42:                  // 设置转台 z 轴复位
+        by_frame_send(cmd, buff); // 正确接收后直接返回原文
+        break;
+
+      case 0x43:                  // 设置转台末端执行器复位
+        by_frame_send(cmd, buff); // 正确接收后直接返回原文
+        break;
+
+      default:
+        break;
+    }
+  }
+}

app/by_messy.h

@@ -0,0 +1,7 @@
+#ifndef _BY_MESSY_H__
+#define _BY_MESSY_H__
+
+extern void by_messy_init(void);
+extern void by_messy_loop(void);
+
+#endif

app/by_motion.c

@@ -2,13 +2,12 @@
 
 #include <math.h>
 #include <string.h>
+#include "by_debug.h"
+#include "by_can.h"
 
-#define D_X          (0.18f) // 底盘 Y 轴上两轮中心的间距
-#define D_Y          (0.25f) // 底盘 X 轴上两轮中心的间距
-#define RX_RY        ((D_X + D_Y) / 2.f)
-
-#define FP2U16_SCALE (1000.f)                // 浮点转存为整数缩放尺度，所有设备该参数需对应
-#define FP2U16(x)    ((int16_t)(x * FP2U16_SCALE)) // 浮点转存为整数
+#define D_X   (0.18f) // 底盘 Y 轴上两轮中心的间距
+#define D_Y   (0.25f) // 底盘 X 轴上两轮中心的间距
+#define RX_RY ((D_X + D_Y) / 2.f)
 
 /**********************************************
  * v_1 = v_{ty} + v_{tx} - (r_x + r_y) * \omega
@@ -17,11 +16,20 @@
  * v_4 = v_{ty} - v_{tx} - (r_x + r_y) * \omega
  **********************************************/
 
+/*** 控制模式 ***/
+uint8_t control_mode = 0; // 0-速度模式 1-位置模式
+
+/*** 位置控制 ***/
+uint8_t control_timer      = 0; // 位置控制定时器状态
+uint32_t control_timer_cnt = 0; // 位置控制计数
+
 /*** 各轮转速，左上角为 1 号，顺时针标号 ***/
 float v_wheel[4] = {0.f};
 
 /** 目标速度 **/
 motion_speed_type motion_speed_struct;
+motion_speed_type motion_speed_struct_last;
+motion_time_type motion_time_struct;
 
 /** 下发数据包 **/
 int16_t motion_speed_data[4] = {0};
@@ -29,26 +37,88 @@ int16_t motion_speed_data[4] = {0};
 void by_motion_init(void)
 {
   memset(&motion_speed_struct, 0, sizeof(motion_speed_struct));
+  memset(&motion_speed_struct_last, 0, sizeof(motion_speed_struct_last));
+  memset(&motion_time_struct, 0, sizeof(motion_time_struct));
+  memset(motion_speed_data, 0, sizeof(motion_speed_data));
   memset(v_wheel, 0, sizeof(v_wheel));
+
+  by_can_send_stdd(0x01, (uint8_t *)&motion_speed_data, 8, 100);
 }
 
-void by_motion_calc_speed(motion_speed_type *speed)
+void by_motion_update_speed(void)
 {
-  motion_speed_struct = *speed;
+  const motion_speed_type *speed = &motion_speed_struct;
 
   v_wheel[0] = speed->v_x + speed->v_y - RX_RY * speed->v_w;
   v_wheel[1] = speed->v_x - speed->v_y + RX_RY * speed->v_w;
   v_wheel[2] = speed->v_x + speed->v_y + RX_RY * speed->v_w;
   v_wheel[3] = speed->v_x - speed->v_y - RX_RY * speed->v_w;
+
+  for (uint8_t i = 0; i < 4; i++) {
+    motion_speed_data[i] = (int16_t)v_wheel[i];
+
+    LOGD("MOTION#SPD wheel[%d] - %d", i, motion_speed_data[i]);
+  }
+
+  by_can_send_stdd(0x01, (uint8_t *)&motion_speed_data, 8, 100);
+  LOGD("MOTION#SPD updated");
 }
 
 /**
- * @brief
+ * @brief 电机控制 - 主循环中调用
  *
  */
-void by_motion_pack_speed(void)
+void by_motion_loop(void)
 {
-  for (uint8_t i = 0; i < 4; i++) {
-    motion_speed_data[i] = FP2U16(v_wheel[i]);
+  if (control_mode == 0) { // 速度控制模式
+    memset(&motion_time_struct, 0, sizeof(motion_time_struct));
+  } else { // 位置控制模式
+    if (0 == motion_time_struct.t_x) {
+      motion_speed_struct.v_x = 0;
+    }
+    if (0 == motion_time_struct.t_y) {
+      motion_speed_struct.v_y = 0;
+    }
+    if (0 == motion_time_struct.t_w) {
+      motion_speed_struct.v_w = 0;
+    }
   }
+
+  if ((motion_speed_struct.v_x != motion_speed_struct_last.v_x) || (motion_speed_struct.v_y != motion_speed_struct_last.v_y) || (motion_speed_struct.v_w != motion_speed_struct_last.v_w)) {
+    by_motion_update_speed();
+    memcpy(&motion_speed_struct_last, &motion_speed_struct, sizeof(motion_speed_type));
+  }
+}
+
+/**
+ * @brief 定时回调
+ *
+ */
+void by_motion_timer_handle(void)
+{
+  if (control_mode == 0) {
+    motion_time_struct.t_x = 0;
+    motion_time_struct.t_y = 0;
+    motion_time_struct.t_w = 0;
+  } else {
+    if (motion_time_struct.t_x > 0) {
+      motion_time_struct.t_x--;
+    }
+    if (motion_time_struct.t_y > 0) {
+      motion_time_struct.t_y--;
+    }
+    if (motion_time_struct.t_w > 0) {
+      motion_time_struct.t_w--;
+    }
+  }
+}
+
+/**
+ * @brief 设置电机控制模式
+ *
+ * @param mode 0-速度模式 1-位置模式
+ */
+void by_motion_set_mode(uint8_t mode)
+{
+  control_mode = mode;
 }

app/by_motion.h

@@ -9,4 +9,20 @@ typedef struct motion_speed_type {
   float v_w;
 } motion_speed_type;
 
+typedef struct motion_time_type {
+  uint32_t t_x;
+  uint32_t t_y;
+  uint32_t t_w;
+} motion_time_type;
+
+extern void by_motion_init(void);
+extern void by_motion_update_speed(void);
+extern void by_motion_loop(void);
+extern void by_motion_set_mode(uint8_t mode);
+extern void by_motion_timer_handle(void);
+extern void by_motion_set_mode(uint8_t mode);
+
+extern motion_speed_type motion_speed_struct;
+extern motion_time_type motion_time_struct;
+
 #endif

app/by_utils.c

@@ -0,0 +1,7 @@
+#include "by_utils.h"
+
+inline int32_t clip_s32(int32_t x, int32_t low, int32_t up)
+{
+  return (x > up ? up : x < low ? low
+                                : x);
+}

app/by_utils.h

@@ -0,0 +1,23 @@
+#ifndef _BY_UTILS_H__
+#define _BY_UTILS_H__
+
+#include "at32f403a_407.h"
+
+typedef enum {
+  BY_ERROR   = 0,
+  BY_SUCCESS = !BY_ERROR
+} by_error_status;
+
+typedef enum {
+  T_U8 = 0,
+  T_U16,
+  T_U32,
+  T_S8,
+  T_S16,
+  T_S32,
+  T_F32
+} by_data_type;
+
+int32_t clip_s32(int32_t x, int32_t low, int32_t up);
+
+#endif