144 lines
5.5 KiB
C
144 lines
5.5 KiB
C
#include "by_messy.h"
|
|
|
|
#include "lwprintf.h"
|
|
#include "by_frame.h"
|
|
#include "by_motion.h"
|
|
#include "by_can.h"
|
|
|
|
void by_messy_init(void)
|
|
{
|
|
by_frame_init();
|
|
}
|
|
|
|
void by_messy_loop(void)
|
|
{
|
|
uint8_t cmd = 0;
|
|
uint8_t buff_t_u8[8] = {0};
|
|
uint16_t buff_t_u16[4] = {0};
|
|
float buff_t_f[2] = {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); // 正确接收后直接返回原文
|
|
buff_t_u8[0] = 0;
|
|
buff_t_u8[1] = 1; // 复位模式下速度设置无效
|
|
buff_t_f[0] = 0.0;
|
|
memcpy(&buff_t_u8[2], &buff_t_f[0], sizeof(buff_t_f[0]));
|
|
by_can_send_stdd(0x008, buff_t_u8, 1, 100);
|
|
break;
|
|
|
|
case 0x42: // 设置转台 z 轴复位
|
|
by_frame_send(cmd, buff); // 正确接收后直接返回原文
|
|
buff_t_u8[0] = 0;
|
|
buff_t_u8[1] = 5; // 设置复位速度 5
|
|
by_can_send_stdd(0x008, buff_t_u8, 1, 100);
|
|
break;
|
|
case 0x43: // 设置转台末端执行器复位
|
|
by_frame_send(cmd, buff); // 正确接收后直接返回原文
|
|
buff_t_u8[0] = 0;
|
|
by_can_send_stdd(0x009, buff_t_u8, 1, 100);
|
|
break;
|
|
case 0x44: // 设置x轴位置 (增量)
|
|
by_frame_send(cmd, buff);
|
|
buff_t_u8[0] = 1; // 增量模式
|
|
buff_t_u8[1] = buff[0]; // 拷贝设置速度
|
|
memcpy(&buff_t_u8[2], &buff[1], sizeof(float)); // 拷贝设置距离
|
|
by_can_send_stdd(0x006, buff_t_u8, 8, 100);
|
|
break;
|
|
case 0x46: // 设置z轴位置 (增量)
|
|
by_frame_send(cmd, buff);
|
|
buff_t_u8[0] = 1; // 增量模式
|
|
buff_t_u8[1] = buff[0]; // 拷贝设置速度
|
|
memcpy(&buff_t_u8[2], &buff[1], sizeof(float)); // 拷贝设置距离
|
|
by_can_send_stdd(0x008, buff_t_u8, 8, 100);
|
|
break;
|
|
case 0x47: // 设置x轴位置 (绝对)
|
|
by_frame_send(cmd, buff);
|
|
buff_t_u8[0] = 0; // 位置模式
|
|
buff_t_u8[1] = buff[0]; // 拷贝设置速度
|
|
memcpy(&buff_t_u8[2], &buff[1], sizeof(float)); // 拷贝设置距离
|
|
by_can_send_stdd(0x006, buff_t_u8, 8, 100);
|
|
break;
|
|
case 0x49: // 设置z轴位置 (绝对)
|
|
by_frame_send(cmd, buff);
|
|
buff_t_u8[0] = 0; // 增量模式
|
|
buff_t_u8[1] = buff[0]; // 拷贝设置速度
|
|
memcpy(&buff_t_u8[2], &buff[1], sizeof(float)); // 拷贝设置距离
|
|
by_can_send_stdd(0x008, buff_t_u8, 8, 100);
|
|
break;
|
|
case 0x50: // 设置夹爪摇臂角度
|
|
by_frame_send(cmd, buff);
|
|
memcpy(buff_t_f, buff, sizeof(float));
|
|
buff_t_u16[0] = (int16_t)(buff_t_f[0]);
|
|
by_can_send_stdd(0x009, (uint8_t *)&buff_t_u16, 2, 100);
|
|
break;
|
|
case 0x51: // 设置夹爪角度
|
|
by_frame_send(cmd, buff);
|
|
memcpy(buff_t_f, buff, sizeof(float));
|
|
buff_t_u16[0] = (int16_t)(buff_t_f[0]);
|
|
by_can_send_stdd(0x00A, (uint8_t *)&buff_t_u16, 2, 100);
|
|
break;
|
|
case 0x52: // 设置摄像头角度
|
|
by_frame_send(cmd, buff);
|
|
memcpy(buff_t_f, buff, sizeof(float));
|
|
buff_t_u16[0] = (int16_t)(buff_t_f[0]);
|
|
by_can_send_stdd(0x00B, (uint8_t *)&buff_t_u16, 2, 100);
|
|
break;
|
|
case 0x53: // 设置顶端抓取机构角度
|
|
by_frame_send(cmd, buff);
|
|
memcpy(buff_t_f, buff, sizeof(float));
|
|
buff_t_u16[0] = (int16_t)(buff_t_f[0]);
|
|
by_can_send_stdd(0x00C, (uint8_t *)&buff_t_u16, 2, 100);
|
|
break;
|
|
case 0x54: // 设置托盘角度
|
|
by_frame_send(cmd, buff);
|
|
memcpy(buff_t_f, buff, sizeof(float));
|
|
buff_t_u16[0] = (int16_t)(buff_t_f[0]);
|
|
by_can_send_stdd(0x00D, (uint8_t *)&buff_t_u16, 2, 100);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
} |