feat: 增加距离控制

app/by_motion.c

@@ -8,11 +8,16 @@
 #define DRV_ENABLE()  gpio_bits_set(GPIOB, GPIO_PINS_15)
 #define DRV_DISABLE() gpio_bits_reset(GPIOB, GPIO_PINS_15)
 
+#define MU            (2.854f) // speed = \mu * speed_in_pulse
+
 by_motor_param param_m1;
 by_motor_param param_m2;
 PID_TypeDef pid_m1;
 PID_TypeDef pid_m2;
 uint8_t motion_enable_flag;
+uint8_t motion_busy_flag;
+int16_t time_via;
+float position_abs;
 
 void by_motion_set_pwm_m1(int32_t pwm_duty)
 {
@@ -23,14 +28,14 @@ void by_motion_set_pwm_m1(int32_t pwm_duty)
   tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, pwm_duty);
 }
 
-void by_motion_set_pwm_m2(int32_t pwm_duty)
+// void by_motion_set_pwm_m2(int32_t pwm_duty)
-{
+// {
-  pwm_duty = clip_s32(pwm_duty, -440, 440); // 不可以拉满哦
+//   pwm_duty = clip_s32(pwm_duty, -440, 440); // 不可以拉满哦
-  pwm_duty += 499;
+//   pwm_duty += 499;
 
-  // 互补 pwm 输出，499 为中值
+//   // 互补 pwm 输出，499 为中值
-  tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_duty);
+//   tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_duty);
-}
+// }
 
 int16_t by_motion_get_speed_m1(void)
 {
@@ -43,41 +48,96 @@ int16_t by_motion_get_speed_m1(void)
 #undef alpha
 }
 
-int16_t by_motion_get_speed_m2(void)
+// int16_t by_motion_get_speed_m2(void)
-{
+// {
-#define alpha (0.1f)
+// #define alpha (0.1f)
-  static float last_speed = 0.0f;
+//   static float last_speed = 0.0f;
-  param_m2.real_speed     = alpha * last_speed + (1.0f - alpha) * (float)(-1 * (int16_t)tmr_counter_value_get(TMR3));
+//   param_m2.real_speed     = alpha * last_speed + (1.0f - alpha) * (float)(-1 * (int16_t)tmr_counter_value_get(TMR3));
-  last_speed = param_m2.real_speed;
+//   last_speed              = param_m2.real_speed;
-  tmr_counter_value_set(TMR3, 0);
+//   tmr_counter_value_set(TMR3, 0);
-  return (int16_t)param_m2.real_speed;
+//   return (int16_t)param_m2.real_speed;
-#undef alpha
+// #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)
+// void by_motion_set_speed_m2(int16_t speed)
+// {
+//   param_m2.target_speed = speed;
+// }
+
+void by_motion_set_distance(float distance, int16_t speed)
 {
-  param_m2.target_speed = speed;
+  if (motion_busy_flag) {
+    return;
+  }
+
+  if (speed < 0) {
+    speed = -speed;
+  }
+
+  if (distance < 0.0f) {
+    by_motion_set_speed_m1(-1 * speed);
+    distance = -distance;
+  } else {
+    by_motion_set_speed_m1(speed);
+  }
+
+  motion_busy_flag = 1;
+  position_abs += distance;
+  time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s
+  // lwprintf("time_via = %d\r\n", time_via);
+}
+
+void by_motion_set_distance2(float distance, int16_t speed)
+{
+  if (motion_busy_flag) {
+    return;
+  }
+
+  if (speed < 0) {
+    speed = -speed;
+  }
+
+  if (distance < 0.0f) {
+    distance = -distance;
+  }
+
+  distance = position_abs - distance;
+
+  if (distance < 0.0f) {
+    distance = -distance;
+    by_motion_set_speed_m1(-1 * speed);
+  } else {
+    by_motion_set_speed_m1(speed);
+  }
+
+  motion_busy_flag = 1;
+  position_abs += distance;
+  time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s
+  // lwprintf("time_via = %d\r\n", time_via);
 }
 
 void by_motion_init(void)
 {
   motion_enable_flag = 0;
+  motion_busy_flag   = 0;
+  time_via           = 0;
+  position_abs       = 0.0f; // TODO 待添加复位操作
 
   by_motion_set_pwm_m1(0);
-  by_motion_set_pwm_m2(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.Kp = BY_MOTION_DEFAULT_KP_M2;
-  param_m2.Ki = BY_MOTION_DEFAULT_KI_M2;
+  // param_m2.Ki = BY_MOTION_DEFAULT_KI_M2;
-  param_m2.Kd = BY_MOTION_DEFAULT_KD_M2;
+  // param_m2.Kd = BY_MOTION_DEFAULT_KD_M2;
 
   /* load parameters form ee */
   // TODO
@@ -87,10 +147,10 @@ void by_motion_init(void)
   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(&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_SetMode(&pid_m2, _PID_MODE_AUTOMATIC);
-  PID_SetSampleTime(&pid_m2, 10);
+  // PID_SetSampleTime(&pid_m2, 10);
-  PID_SetOutputLimits(&pid_m2, -400, 400);
+  // PID_SetOutputLimits(&pid_m2, -400, 400);
 
   motion_enable_flag = 1;
   DRV_ENABLE();
@@ -112,22 +172,50 @@ void by_motion_run(void)
 
 void by_motion_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len)
 {
-#define BC2D_MODEL1
 
-  if (0x01 == stdd_id) {
+  if (0x08 == stdd_id) {
-#if defined(BC2D_MODEL1)
+    uint8_t mode  = data[0];
-    int16_t speed_m1_temp = (int16_t)(data[0] | (data[1] << 8));
+    int16_t speed = (int16_t)data[1];
-    int16_t speed_m2_temp = (int16_t)(data[2] | (data[3] << 8));
+    // float distance = (float)(data[2] | (data[3] << 8) | (data[4] << 16) | (data[5] << 24));
-    by_motion_set_speed_m1(speed_m1_temp);
+    float distance;
-    by_motion_set_speed_m2(speed_m2_temp);
+    memcpy(&distance, &data[2], 4);
-#elif defined(BC2D_MODEL2)
+    if (0 == mode) {
-    int16_t speed_m1_temp = (int16_t)(data[4] | (data[5] << 8));
+      by_motion_set_distance2(distance, speed);
-    int16_t speed_m2_temp = (int16_t)(data[6] | (data[7] << 8));
+    } else if (1 == mode) {
-    by_motion_set_speed_m1(speed_m1_temp);
+      by_motion_set_distance(distance, speed);
-    by_motion_set_speed_m2(speed_m2_temp);
-#endif
     }
+  }
+
+  // #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);
 }
+
+void by_motion_tmr_handle(void)
+{
+  if (!motion_busy_flag) {
+    return;
+  }
+
+  if (time_via > 0) {
+    time_via--;
+  } else {
+    by_motion_set_speed_m1(0);
+    motion_busy_flag = 0;
+  }
+}

app/by_motion.h

@@ -28,6 +28,7 @@ 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 void by_motion_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len);
+extern void by_motion_tmr_handle(void);
 
 extern by_motor_param param_m1;
 extern by_motor_param param_m2;

project/src/at32f413_int.c

@@ -237,6 +237,7 @@ void TMR4_GLOBAL_IRQHandler(void)
   /* add user code begin TMR4_GLOBAL_IRQ 0 */
   if (SET == tmr_interrupt_flag_get(TMR4, TMR_OVF_FLAG)) {
     by_motion_run();
+    by_motion_tmr_handle();
     tmr_flag_clear(TMR4, TMR_OVF_FLAG);
   }
   /* add user code end TMR4_GLOBAL_IRQ 0 */

project/src/main.c

@@ -129,7 +129,7 @@ int main(void)
     /* add user code begin 3 */
     // lwprintf("pwm value: %f\r\n", param_m1.out_pwm);
     // lwprintf("mot speed: %f\r\n", param_m1.real_speed);
-    lwprintf("%f,%f,%f\r\n", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm);
+    // lwprintf("%f,%f,%f\r\n", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm);
     /* add user code end 3 */
   }
 }