initial commit

2024-06-10 18:04:53 +08:00
commit 3c525b4cfd
115 changed files with 54092 additions and 0 deletions
--- a/app/by_motion.c
+++ b/app/by_motion.c
@@ -0,0 +1,140 @@
+#include "by_motion.h"
+
+#include "pid.h"
+#include "by_utils.h"
+#include "by_debug.h"
+
+#define DRV_ENABLE()  gpio_bits_set(GPIOA, GPIO_PINS_12)
+#define DRV_DISABLE() gpio_bits_reset(GPIOA, GPIO_PINS_12)
+
+by_motor_param param_m1;
+by_motor_param param_m2;
+PID_TypeDef pid_m1;
+PID_TypeDef pid_m2;
+uint8_t motion_enable_flag;
+
+void by_motion_set_pwm_m1(int32_t pwm_duty)
+{
+  pwm_duty = clip_s32(pwm_duty, -900, 900); // 不可以拉满哦
+
+  if (pwm_duty < 0) {
+    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_3, -pwm_duty);
+    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_4, 0);
+  } else {
+    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_3, 0);
+    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_4, pwm_duty);
+  }
+}
+
+void by_motion_set_pwm_m2(int32_t pwm_duty)
+{
+  pwm_duty = clip_s32(pwm_duty, -900, 900); // 不可以拉满哦
+
+  if (pwm_duty < 0) {
+    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, -pwm_duty);
+    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, 0);
+  } else {
+    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, 0);
+    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_duty);
+  }
+}
+
+int16_t by_motion_get_speed_m1(void)
+{
+#define alpha (0.1f)
+  static float last_speed = 0.0f;
+  param_m1.real_speed     = alpha * last_speed + (1.0f - alpha) * (float)(-1 * (int16_t)tmr_counter_value_get(TMR3));
+  last_speed              = param_m1.real_speed;
+  tmr_counter_value_set(TMR3, 0);
+  return (int16_t)param_m1.real_speed;
+#undef alpha
+}
+
+int16_t by_motion_get_speed_m2(void)
+{
+#define alpha (0.1f)
+  static float last_speed = 0.0f;
+  param_m2.real_speed     = alpha * last_speed + (1.0f - alpha) * (float)(-1 * (int16_t)tmr_counter_value_get(TMR2));
+  last_speed              = param_m2.real_speed;
+  tmr_counter_value_set(TMR2, 0);
+  return (int16_t)param_m2.real_speed;
+#undef alpha
+}
+
+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, -900, 900);
+
+  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, -900, 900);
+
+  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);
+
+    by_motion_set_pwm_m1((int32_t)param_m1.out_pwm);
+    by_motion_set_pwm_m2((int32_t)param_m2.out_pwm);
+  }
+}
+
+void by_motion_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len)
+{
+#define BC2D_MODEL1
+
+  if (0x01 == stdd_id) {
+#if defined(BC2D_MODEL1)
+    int16_t speed_m1_temp = (int16_t)(data[0] | (data[1] << 8));
+    int16_t speed_m2_temp = (int16_t)(data[2] | (data[3] << 8));
+    by_motion_set_speed_m1(speed_m1_temp);
+    by_motion_set_speed_m2(speed_m2_temp);
+#elif defined(BC2D_MODEL2)
+    int16_t speed_m1_temp = (int16_t)(data[4] | (data[5] << 8));
+    int16_t speed_m2_temp = (int16_t)(data[6] | (data[7] << 8));
+    by_motion_set_speed_m1(speed_m1_temp);
+    by_motion_set_speed_m2(speed_m2_temp);
+#endif
+  }
+
+  LOGD("m1:%f,%f,%f", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm);
+  LOGD("m2:%f,%f,%f", param_m2.real_speed, param_m2.target_speed, param_m2.out_pwm);
+}