diff --git a/app/by_motion.c b/app/by_motion.c index 45e5826..d808986 100644 --- a/app/by_motion.c +++ b/app/by_motion.c @@ -71,6 +71,7 @@ void by_motion_set_speed_m1(int16_t speed) void by_motion_set_distance(float distance, int16_t speed) { + // TODO 验证距离是否超限 if (motion_busy_flag) { return; } @@ -79,6 +80,8 @@ void by_motion_set_distance(float distance, int16_t speed) speed = -speed; } + position_abs += distance; + if (distance < 0.0f) { by_motion_set_speed_m1(-1 * speed); distance = -distance; @@ -87,13 +90,14 @@ void by_motion_set_distance(float distance, int16_t speed) } motion_busy_flag = 1; - position_abs += distance; - time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz,控制值单位为 s + time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz,控制值单位为 s // lwprintf("time_via = %d\r\n", time_via); + LOGD("position_abs = %f\r\n", position_abs); } void by_motion_set_distance2(float distance, int16_t speed) { + // TODO 验证距离是否超限 if (motion_busy_flag) { return; } @@ -106,7 +110,8 @@ void by_motion_set_distance2(float distance, int16_t speed) distance = -distance; } - distance = position_abs - distance; + distance = distance - position_abs; + position_abs += distance; if (distance < 0.0f) { distance = -distance; @@ -116,8 +121,8 @@ void by_motion_set_distance2(float distance, int16_t speed) } motion_busy_flag = 1; - position_abs += distance; - time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz,控制值单位为 s + time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz,控制值单位为 s + LOGD("position_abs = %f\r\n", position_abs); // lwprintf("time_via = %d\r\n", time_via); } @@ -156,7 +161,7 @@ void by_motion_init(void) DRV_ENABLE(); // 上电回零 - DWT_Delay(3000000); + DWT_Delay(1000000); by_motion_set_speed_m1(-4); while (SET == gpio_input_data_bit_read(GPIOB, GPIO_PINS_3)) { // 等待复位