initial commit

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 "at32f413.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_motion.c

@@ -0,0 +1,108 @@
+#include "by_motion.h"
+
+#include "pid.h"
+#include "dwt_delay.h"
+#include "by_utils.h"
+#include "by_debug.h"
+
+#define DRV_ENABLE()  gpio_bits_set(GPIOB, GPIO_PINS_15)
+#define DRV_DISABLE() gpio_bits_reset(GPIOB, GPIO_PINS_15)
+
+by_motor_param param_m1;
+by_motor_param param_m2;
+PID_TypeDef pid_m1;
+PID_TypeDef pid_m2;
+int16_t speed_m1;
+int16_t speed_m2;
+uint8_t motion_enable_flag;
+
+void by_motion_set_pwm_m1(int32_t pwm_duty)
+{
+  pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦
+  pwm_duty += 499;
+
+  // 互补 pwm 输出，499 为中值
+  tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, pwm_duty);
+}
+
+void by_motion_set_pwm_m2(int32_t pwm_duty)
+{
+  pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦
+  pwm_duty += 499;
+
+  // 互补 pwm 输出，499 为中值
+  tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_duty);
+}
+
+int16_t by_motion_get_speed_m1(void)
+{
+  param_m1.real_speed = (float)(-1 * (int16_t)tmr_counter_value_get(TMR2));
+  tmr_counter_value_set(TMR2, 0);
+  return (int16_t)param_m1.real_speed;
+}
+
+int16_t by_motion_get_speed_m2(void)
+{
+  param_m2.real_speed = (float)(-1 * (int16_t)tmr_counter_value_get(TMR3));
+  tmr_counter_value_set(TMR3, 0);
+  return (int16_t)param_m2.real_speed;
+}
+
+void by_motion_set_speed_m1(int16_t speed)
+{
+  param_m1.target_speed = speed;
+}
+
+void by_motion_set_speed_m2(int16_t speed)
+{
+  param_m2.target_speed = speed;
+}
+
+void by_motion_init(void)
+{
+  motion_enable_flag = 0;
+
+  by_motion_set_pwm_m1(0);
+  by_motion_set_pwm_m2(0);
+
+  /* set default parameters */
+  param_m1.Kp = BY_MOTION_DEFAULT_KP_M1;
+  param_m1.Ki = BY_MOTION_DEFAULT_KI_M1;
+  param_m1.Kd = BY_MOTION_DEFAULT_KD_M1;
+  param_m2.Kp = BY_MOTION_DEFAULT_KP_M2;
+  param_m2.Ki = BY_MOTION_DEFAULT_KI_M2;
+  param_m2.Kd = BY_MOTION_DEFAULT_KD_M2;
+
+  /* load parameters form ee */
+  // TODO
+
+  PID(&pid_m1, &param_m1.real_speed, &param_m1.out_pwm, &param_m1.target_speed, param_m1.Kp, param_m1.Ki, param_m1.Kd, _PID_P_ON_E, _PID_CD_DIRECT);
+  PID_SetMode(&pid_m1, _PID_MODE_AUTOMATIC);
+  PID_SetSampleTime(&pid_m1, 10);
+  PID_SetOutputLimits(&pid_m1, -400, 400);
+
+  PID(&pid_m2, &param_m2.real_speed, &param_m2.out_pwm, &param_m2.target_speed, param_m2.Kp, param_m2.Ki, param_m2.Kd, _PID_P_ON_E, _PID_CD_DIRECT);
+  PID_SetMode(&pid_m2, _PID_MODE_AUTOMATIC);
+  PID_SetSampleTime(&pid_m2, 10);
+  PID_SetOutputLimits(&pid_m2, -400, 400);
+
+  param_m1.target_speed = 17;
+
+  motion_enable_flag = 1;
+  DRV_ENABLE();
+}
+
+void by_motion_run(void)
+{
+  if (motion_enable_flag) {
+    by_motion_get_speed_m1();
+    by_motion_get_speed_m2();
+
+    PID_Compute(&pid_m1);
+    PID_Compute(&pid_m2);
+
+    LOGD("m1:%f,%f,%f", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm);
+    by_motion_set_pwm_m1((int32_t)param_m1.out_pwm);
+    // by_motion_set_pwm_m2((int32_t)param_m2.out_pwm);
+  }
+}

app/by_motion.h

@@ -0,0 +1,34 @@
+#ifndef _BY_MOTION_H__
+#define _BY_MOTION_H__
+
+#include "at32f413.h"
+
+typedef struct by_motor_param {
+  float Kp;
+  float Ki;
+  float Kd;
+  float real_speed;
+  float target_speed;
+  float out_pwm;
+} by_motor_param;
+
+#define BY_MOTION_DEFAULT_KP_M1 (13.0f)
+#define BY_MOTION_DEFAULT_KI_M1 (5.0f)
+#define BY_MOTION_DEFAULT_KD_M1 (0.1f)
+#define BY_MOTION_DEFAULT_KP_M2 (1.0f)
+#define BY_MOTION_DEFAULT_KI_M2 (0.0f)
+#define BY_MOTION_DEFAULT_KD_M2 (0.0f)
+
+extern void by_motion_init(void);
+extern void by_motion_run(void);
+extern void by_motion_set_pwm_m1(int32_t pwm_duty);
+extern void by_motion_set_pwm_m2(int32_t pwm_duty);
+extern int16_t by_motion_get_speed_m1(void);
+extern int16_t by_motion_get_speed_m2(void);
+extern void by_motion_set_speed_m1(int16_t speed);
+extern void by_motion_set_speed_m2(int16_t speed);
+
+extern int16_t speed_m1;
+extern int16_t speed_m2;
+
+#endif

app/by_param.c

@@ -0,0 +1,46 @@
+#include "by_param.h"
+
+#include <string.h>
+#include "eeprom.h"
+#include "by_debug.h"
+
+by_param_type param_index[BY_PARAM_NUM_MAX];
+uint16_t param_num = 0;
+
+by_error_status by_param_init(void)
+{
+  param_num = 0;
+  return BY_SUCCESS;
+}
+
+by_error_status by_param_reg(uint16_t reg_addr, uint16_t *data_ptr, const char *data_str)
+{
+  param_index[param_num].reg_addr = reg_addr;
+  param_index[param_num].data_ptr = data_ptr;
+  strncpy(param_index[param_num].data_str, data_str, sizeof(param_index[param_num].data_str));
+  LOGD("reg data [%s]: addr-0x%04x", param_index[param_num].data_str, param_index[param_num].reg_addr);
+  param_num++;
+  return BY_SUCCESS;
+}
+
+by_error_status by_param_load_from_ee(uint16_t reg_addr)
+{
+  for (int i = 0; i < param_num; i++) {
+    if (param_index[i].reg_addr == reg_addr) {
+      flash_ee_data_read(reg_addr, param_index[i].data_ptr);
+      return BY_SUCCESS;
+    }
+  }
+  return BY_ERROR;
+}
+
+by_error_status by_param_save_to_ee(uint16_t reg_addr)
+{
+  for (int i = 0; i < param_num; i++) {
+    if (param_index[i].reg_addr == reg_addr) {
+      flash_ee_data_write(reg_addr, *param_index[i].data_ptr);
+      return BY_SUCCESS;
+    }
+  }
+  return BY_ERROR;
+}

app/by_param.h

@@ -0,0 +1,19 @@
+#ifndef _BY_PARAM_H__
+#define _BY_PARAM_H__
+
+#include "at32f413.h"
+
+#include "by_utils.h"
+
+typedef struct by_param_type {
+  uint16_t reg_addr;
+  uint16_t *data_ptr;
+  char data_str[20];
+} by_param_type;
+
+#define BY_PARAM_NUM_MAX (20)
+
+by_error_status by_param_init(void);
+by_error_status by_param_reg(uint16_t reg_addr, uint16_t *data_ptr, const char *data_str);
+
+#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 "at32f413.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