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

BC1C.ATWP

@@ -78,8 +78,8 @@
             <ModeSub name="Activated" value="TRUE"/>
         </Mode>
         <Parameters>
-            <ParametersSub name="DividerValue" value="4999"/>
+            <ParametersSub name="DividerValue" value="499"/>
-            <ParametersSub name="Period" value="47999"/>
+            <ParametersSub name="Period" value="2399"/>
         </Parameters>
     </TMR6>
     <TMR8>
@@ -90,6 +90,10 @@
             <ModeSub name="Channel4 mode" value="Output_CH4"/>
             <ModeSub name="Activated" value="TRUE"/>
         </Mode>
+        <Parameters>
+            <ParametersSub name="DividerValue" value="1999"/>
+            <ParametersSub name="Period" value="2399"/>
+        </Parameters>
     </TMR8>
     <TMR11>
         <Mode>

BC1C.code-workspace

@@ -31,7 +31,7 @@
             "titleBar.activeBackground": "#693002",
             "titleBar.activeForeground": "#FFF9F4"
         },
-        "cortex-debug.variableUseNaturalFormat": false
+        "cortex-debug.variableUseNaturalFormat": true
     },
     "extensions": {
     }

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,14 +2,13 @@
 
 #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)) // 浮点转存为整数
-
 /**********************************************
  * v_1 = v_{ty} + v_{tx} - (r_x + r_y) * \omega
  * v_2 = 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++) {
+  if (control_mode == 0) { // 速度控制模式
-    motion_speed_data[i] = FP2U16(v_wheel[i]);
+    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

project/src/at32f403a_407_int.c

@@ -31,6 +31,7 @@
 /* add user code begin private includes */
 #include "by_debug.h"
 #include "by_frame.h"
+#include "by_motion.h"
 /* add user code end private includes */
 
 /* private typedef -----------------------------------------------------------*/
@@ -266,6 +267,7 @@ void USART3_IRQHandler(void)
 {
   /* add user code begin USART3_IRQ 0 */
   if (SET == usart_flag_get(USART3, USART_RDBF_FLAG)) {
+    by_frame_parse_uart_handle((uint8_t)usart_data_receive(USART3));
     // usart_data_transmit(USART1, usart_data_receive(USART3));
     usart_flag_clear(USART3, USART_RDBF_FLAG);
   }
@@ -284,6 +286,7 @@ void TMR6_GLOBAL_IRQHandler(void)
 {
   /* add user code begin TMR6_GLOBAL_IRQ 0 */
   if (SET == tmr_flag_get(TMR6, TMR_OVF_FLAG)) {
+    by_motion_timer_handle();
     tmr_flag_clear(TMR6, TMR_OVF_FLAG);
   }
   /* add user code end TMR6_GLOBAL_IRQ 0 */

project/src/at32f403a_407_wk_config.c

@@ -88,21 +88,24 @@ void wk_system_clock_config(void)
   crm_clock_source_enable(CRM_CLOCK_SOURCE_LICK, TRUE);
 
   /* wait till lick is ready */
-  while (crm_flag_get(CRM_LICK_STABLE_FLAG) != SET) {
+  while(crm_flag_get(CRM_LICK_STABLE_FLAG) != SET)
+  {
   }
 
   /* enable hext */
   crm_clock_source_enable(CRM_CLOCK_SOURCE_HEXT, TRUE);
 
   /* wait till hext is ready */
-  while (crm_hext_stable_wait() == ERROR) {
+  while(crm_hext_stable_wait() == ERROR)
+  {
   }
 
   /* enable hick */
   crm_clock_source_enable(CRM_CLOCK_SOURCE_HICK, TRUE);
 
   /* wait till hick is ready */
-  while (crm_flag_get(CRM_HICK_STABLE_FLAG) != SET) {
+  while(crm_flag_get(CRM_HICK_STABLE_FLAG) != SET)
+  {
   }
 
   /* config pll clock resource */
@@ -112,7 +115,8 @@ void wk_system_clock_config(void)
   crm_clock_source_enable(CRM_CLOCK_SOURCE_PLL, TRUE);
 
   /* wait till pll is ready */
-  while (crm_flag_get(CRM_PLL_STABLE_FLAG) != SET) {
+  while(crm_flag_get(CRM_PLL_STABLE_FLAG) != SET)
+  {
   }
 
   /* config ahbclk */
@@ -131,7 +135,8 @@ void wk_system_clock_config(void)
   crm_sysclk_switch(CRM_SCLK_PLL);
 
   /* wait till pll is used as system clock source */
-  while (crm_sysclk_switch_status_get() != CRM_SCLK_PLL) {
+  while(crm_sysclk_switch_status_get() != CRM_SCLK_PLL)
+  {
   }
 
   /* disable auto step mode */
@@ -562,7 +567,7 @@ void wk_tmr6_init(void)
   /* add user code end tmr6_init 1 */
 
   /* configure counter settings */
-  tmr_base_init(TMR6, 47999, 4999);
+  tmr_base_init(TMR6, 2399, 499);
   tmr_cnt_dir_set(TMR6, TMR_COUNT_UP);
   tmr_period_buffer_enable(TMR6, FALSE);
 
@@ -638,7 +643,7 @@ void wk_tmr8_init(void)
   gpio_init(GPIOC, &gpio_init_struct);
 
   /* configure counter settings */
-  tmr_base_init(TMR8, 65535, 0);
+  tmr_base_init(TMR8, 2399, 1999);
   tmr_cnt_dir_set(TMR8, TMR_COUNT_UP);
   tmr_clock_source_div_set(TMR8, TMR_CLOCK_DIV1);
   tmr_repetition_counter_set(TMR8, 0);
@@ -706,6 +711,7 @@ void wk_tmr8_init(void)
   tmr_brkdt_struct.deadtime = 0;
   tmr_brkdt_config(TMR8, &tmr_brkdt_struct);
 
+
   tmr_output_enable(TMR8, TRUE);
 
   tmr_counter_enable(TMR8, TRUE);
@@ -923,7 +929,7 @@ void wk_can1_init(void)
    */
 
   /*can1 rx0 interrupt config--------------------------------------------------------*/ 
-  // can_interrupt_enable(CAN1, CAN_RF0MIEN_INT, TRUE);
+  //can_interrupt_enable(CAN1, CAN_RF0MIEN_INT, TRUE);
 
   /* add user code begin can1_init 2 */
   can_interrupt_enable(CAN1, CAN_RF0MIEN_INT, TRUE);
@@ -1012,7 +1018,7 @@ void wk_can2_init(void)
    */
 
   /*can2 rx0 interrupt config--------------------------------------------------------*/ 
-  // can_interrupt_enable(CAN2, CAN_RF0MIEN_INT, TRUE);
+  //can_interrupt_enable(CAN2, CAN_RF0MIEN_INT, TRUE);
 
   /* add user code begin can2_init 2 */
   can_interrupt_enable(CAN2, CAN_RF0MIEN_INT, TRUE);

project/src/main.c

@@ -34,6 +34,8 @@
 #include "by_debug.h"
 #include "by_crc16.h"
 #include "by_frame.h"
+#include "by_messy.h"
+#include "by_motion.h"
 /* add user code end private includes */
 
 /* private typedef -----------------------------------------------------------*/
@@ -138,8 +140,12 @@ int main(void)
   flash_ee_init();
   LOGD("eeprom init done");
 
-  /* frame init */
+  /* motion init */
-  by_frame_init();
+  by_motion_init();
+  LOGD("motion init done");
+
+  /* messy init */
+  by_messy_init();
   LOGD("frame init done");
 
   LOGI("init done");
@@ -148,7 +154,8 @@ int main(void)
 
   while (1) {
     /* add user code begin 3 */
-    // DWT_Delay(1000000);
+    by_messy_loop();
+    by_motion_loop();
     /* add user code end 3 */
   }
 }