74 lines
2.2 KiB
C
74 lines
2.2 KiB
C
|
#include "by_messy.h"
|
||
|
|
|
||
|
|
#include "lwprintf.h"
|
||
|
|
#include "by_frame.h"
|
||
|
|
#include "by_motion.h"
|
||
|
|
|
||
|
|
void by_messy_init(void)
|
||
|
|
{
|
||
|
|
by_frame_init();
|
||
|
|
}
|
||
|
|
|
||
|
|
void by_messy_loop(void)
|
||
|
|
{
|
||
|
|
uint8_t cmd = 0;
|
||
|
|
uint32_t buff[BY_FRAME_DATA_NUM] = {0};
|
||
|
|
|
||
|
|
if (!by_frame_parse(&cmd, buff)) {
|
||
|
|
|
||
|
|
lwprintf("get cmd: %X\r\n", cmd);
|
||
|
|
|
||
|
|
switch (cmd) {
|
||
|
|
case 0x31: // 设置速度 x
|
||
|
|
memcpy(&motion_speed_struct.v_x, buff, sizeof(motion_speed_struct.v_x));
|
||
|
|
by_motion_set_mode(0);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case 0x32: // 设置速度 y
|
||
|
|
memcpy(&motion_speed_struct.v_y, buff, sizeof(motion_speed_struct.v_y));
|
||
|
|
by_motion_set_mode(0);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case 0x33: // 设置速度 omega
|
||
|
|
memcpy(&motion_speed_struct.v_w, buff, sizeof(motion_speed_struct.v_w));
|
||
|
|
by_motion_set_mode(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);
|
||
|
|
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);
|
||
|
|
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);
|
||
|
|
motion_time_struct.t_w += buff[1];
|
||
|
|
break;
|
||
|
|
|
||
|
|
case 0x41: // 设置转台 x 轴复位
|
||
|
|
by_frame_send(cmd, buff); // 正确接收后直接返回原文
|
||
|
|
break;
|
||
|
|
|
||
|
|
case 0x42: // 设置转台 z 轴复位
|
||
|
|
by_frame_send(cmd, buff); // 正确接收后直接返回原文
|
||
|
|
break;
|
||
|
|
|
||
|
|
case 0x43: // 设置转台末端执行器复位
|
||
|
|
by_frame_send(cmd, buff); // 正确接收后直接返回原文
|
||
|
|
break;
|
||
|
|
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|