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

pref: 为三个方向设置独立的标志位

Browse Source 
保证移动距离完整，防止下发某一方向速度指令时，导致其他方向无法正常退出距离控制模式的问题
This commit is contained in:
bmy
2024-07-28 16:24:31 +08:00
parent 73b2e1a7e9
commit f2caaae8c8
4 changed files with 99 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
app/by_messy.c
View File

@@ -25,37 +25,37 @@ void by_messy_loop(void)
switch (cmd) {
case 0x31: // 设置速度 x
memcpy(&motion_speed_struct.v_x, buff, sizeof(motion_speed_struct.v_x));
by_motion_set_mode(0);
by_motion_set_mode_x(0);
break;
case 0x32: // 设置速度 y
memcpy(&motion_speed_struct.v_y, buff, sizeof(motion_speed_struct.v_y));
by_motion_set_mode(0);
by_motion_set_mode_y(0);
break;
case 0x33: // 设置速度 omega
memcpy(&motion_speed_struct.v_w, buff, sizeof(motion_speed_struct.v_w));
by_motion_set_mode(0);
by_motion_set_mode_w(0);
break;
case 0x34: // 设置移动距离 x
by_frame_send(cmd, buff); // 正确接收后直接返回原文
memcpy(&motion_speed_struct.v_x, buff, sizeof(motion_speed_struct.v_x));
by_motion_set_mode(1);
by_motion_set_mode_x(1);
motion_time_struct.t_x += buff[1];
break;
case 0x35: // 设置移动距离 y
by_frame_send(cmd, buff); // 正确接收后直接返回原文
memcpy(&motion_speed_struct.v_y, buff, sizeof(motion_speed_struct.v_y));
by_motion_set_mode(1);
by_motion_set_mode_y(1);
motion_time_struct.t_y += buff[1];
break;
case 0x36: // 设置旋转角度 omega
by_frame_send(cmd, buff); // 正确接收后直接返回原文
memcpy(&motion_speed_struct.v_w, buff, sizeof(motion_speed_struct.v_w));
by_motion_set_mode(1);
by_motion_set_mode_w(1);
motion_time_struct.t_w += buff[1];
break;

119
app/by_motion.c
View File

@@ -19,6 +19,12 @@
/*** 控制模式 ***/
uint8_t control_mode = 0; // 0-速度模式 1-位置模式
struct by_motion_mode_struct {
uint8_t x;
uint8_t y;
uint8_t w;
} by_motion_mode = {0,0,0};
/*** 位置控制 ***/
uint8_t control_timer = 0; // 位置控制定时器状态
uint32_t control_timer_cnt = 0; // 位置控制计数
@@ -74,20 +80,38 @@ void by_motion_update_speed(void)
*/
void by_motion_loop(void)
{
if (control_mode == 0) { // 速度控制模式
memset(&motion_time_struct, 0, sizeof(motion_time_struct));
} else { // 位置控制模式
if (0 == motion_time_struct.t_x) {
motion_speed_struct.v_x = 0;
}
if (0 == motion_time_struct.t_y) {
motion_speed_struct.v_y = 0;
}
if (0 == motion_time_struct.t_w) {
motion_speed_struct.v_w = 0;
}
if (by_motion_mode.x == 0) {
motion_time_struct.t_x = 0;
} else if (by_motion_mode.x == 1 && motion_time_struct.t_x == 0) {
motion_speed_struct.v_x = 0;
}
if (by_motion_mode.y == 0) {
motion_time_struct.t_y = 0;
} else if (by_motion_mode.y == 1 && motion_time_struct.t_y == 0) {
motion_speed_struct.v_y = 0;
}
if (by_motion_mode.w == 0) {
motion_time_struct.t_w = 0;
} else if (by_motion_mode.w == 1 && motion_time_struct.t_w == 0) {
motion_speed_struct.v_w = 0;
}
// if (control_mode == 0) { // 速度控制模式
// memset(&motion_time_struct, 0, sizeof(motion_time_struct));
// } else { // 位置控制模式
// if (0 == motion_time_struct.t_x) {
// motion_speed_struct.v_x = 0;
// }
// if (0 == motion_time_struct.t_y) {
// motion_speed_struct.v_y = 0;
// }
// if (0 == motion_time_struct.t_w) {
// motion_speed_struct.v_w = 0;
// }
// }
if ((motion_speed_struct.v_x != motion_speed_struct_last.v_x) || (motion_speed_struct.v_y != motion_speed_struct_last.v_y) || (motion_speed_struct.v_w != motion_speed_struct_last.v_w)) {
by_motion_update_speed();
memcpy(&motion_speed_struct_last, &motion_speed_struct, sizeof(motion_speed_type));
@@ -100,29 +124,62 @@ void by_motion_loop(void)
*/
void by_motion_timer_handle(void)
{
if (control_mode == 0) {
// if (control_mode == 0) {
// motion_time_struct.t_x = 0;
// motion_time_struct.t_y = 0;
// motion_time_struct.t_w = 0;
// } else {
// if (motion_time_struct.t_x > 0) {
// motion_time_struct.t_x--;
// }
// if (motion_time_struct.t_y > 0) {
// motion_time_struct.t_y--;
// }
// if (motion_time_struct.t_w > 0) {
// motion_time_struct.t_w--;
// }
// }
if (by_motion_mode.x == 0) {
motion_time_struct.t_x = 0;
} else if(motion_time_struct.t_x > 0) {
motion_time_struct.t_x--;
}
if(by_motion_mode.y == 0) {
motion_time_struct.t_y = 0;
} else if(motion_time_struct.t_y > 0) {
motion_time_struct.t_y--;
}
if(by_motion_mode.w == 0) {
motion_time_struct.t_w = 0;
} else {
if (motion_time_struct.t_x > 0) {
motion_time_struct.t_x--;
}
if (motion_time_struct.t_y > 0) {
motion_time_struct.t_y--;
}
if (motion_time_struct.t_w > 0) {
motion_time_struct.t_w--;
}
} else if(motion_time_struct.t_w > 0) {
motion_time_struct.t_w--;
}
}
/**
* @brief 设置电机控制模式
*
* @param mode 0-速度模式 1-位置模式
*/
void by_motion_set_mode(uint8_t mode)
// /**
// * @brief 设置电机控制模式
// *
// * @param mode 0-速度模式 1-位置模式
// */
// void by_motion_set_mode(uint8_t mode)
// {
// control_mode = mode;
// }
void by_motion_set_mode_x(uint8_t mode)
{
control_mode = mode;
}
by_motion_mode.x = mode;
}
void by_motion_set_mode_y(uint8_t mode)
{
by_motion_mode.y = mode;
}
void by_motion_set_mode_w(uint8_t mode)
{
by_motion_mode.w = mode;
}

5
app/by_motion.h
View File

@@ -20,7 +20,10 @@ extern void by_motion_update_speed(void);
extern void by_motion_loop(void);
extern void by_motion_set_mode(uint8_t mode);
extern void by_motion_timer_handle(void);
extern void by_motion_set_mode(uint8_t mode);
// extern void by_motion_set_mode(uint8_t mode);
extern void by_motion_set_mode_x(uint8_t mode);
extern void by_motion_set_mode_y(uint8_t mode);
extern void by_motion_set_mode_w(uint8_t mode);
extern motion_speed_type motion_speed_struct;
extern motion_time_type motion_time_struct;