btl143/BC2D-POS-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: 修复位置设置指令错误

Browse Source
This commit is contained in:
bmy
2024-05-20 12:10:23 +08:00
parent 5831580e55
commit e38a025201
1 changed files with 11 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
app/by_motion.c
View File

@@ -71,6 +71,7 @@ void by_motion_set_speed_m1(int16_t speed)
void by_motion_set_distance(float distance, int16_t speed) void by_motion_set_distance(float distance, int16_t speed)
{ {
// TODO 验证距离是否超限
if (motion_busy_flag) { if (motion_busy_flag) {
return; return;
} }
@@ -79,6 +80,8 @@ void by_motion_set_distance(float distance, int16_t speed)
speed = -speed; speed = -speed;
} }
position_abs += distance;
if (distance < 0.0f) { if (distance < 0.0f) {
by_motion_set_speed_m1(-1 * speed); by_motion_set_speed_m1(-1 * speed);
distance = -distance; distance = -distance;
@@ -87,13 +90,14 @@ void by_motion_set_distance(float distance, int16_t speed)
} }
motion_busy_flag = 1; 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); // 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) void by_motion_set_distance2(float distance, int16_t speed)
{ {
// TODO 验证距离是否超限
if (motion_busy_flag) { if (motion_busy_flag) {
return; return;
} }
@@ -106,7 +110,8 @@ void by_motion_set_distance2(float distance, int16_t speed)
distance = -distance; distance = -distance;
} }
distance = position_abs - distance; distance = distance - position_abs;
position_abs += distance;
if (distance < 0.0f) { if (distance < 0.0f) {
distance = -distance; distance = -distance;
@@ -116,8 +121,8 @@ void by_motion_set_distance2(float distance, int16_t speed)
} }
motion_busy_flag = 1; 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); // lwprintf("time_via = %d\r\n", time_via);
} }
@@ -156,7 +161,7 @@ void by_motion_init(void)
DRV_ENABLE(); DRV_ENABLE();
// 上电回零 // 上电回零
DWT_Delay(3000000); DWT_Delay(1000000);
by_motion_set_speed_m1(-4); by_motion_set_speed_m1(-4);
while (SET == gpio_input_data_bit_read(GPIOB, GPIO_PINS_3)) { while (SET == gpio_input_data_bit_read(GPIOB, GPIO_PINS_3)) {
// 等待复位 // 等待复位