btl143/project_bycmd
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: 增加命令执行日志输出

Browse Source
This commit is contained in:
bmy
2024-05-29 21:49:14 +08:00
parent 6b0b279835
commit 07a32a247e
5 changed files with 167 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

155
by_cmd.c
View File

@@ -15,6 +15,10 @@ uint32_t received_data[2];
uint8_t listerning_cmd;
int received_flag;
pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER;
#define LOCKAPI() pthread_mutex_lock(&lock)
#define UNLOCKAPI() pthread_mutex_unlock(&lock)
int by_cmd_init(void)
{
log_debug("by_cmd init");
@@ -77,9 +81,12 @@ int by_cmd_reg_listerning(uint8_t cmd, int timeout)
*/
void by_cmd_send_speed_x(float speed)
{
LOCKAPI();
uint8_t buff[4] = {0};
memcpy(buff, &speed, 4);
by_frame_send(0x31, buff, 4);
UNLOCKAPI();
}
/**
@@ -89,9 +96,13 @@ void by_cmd_send_speed_x(float speed)
*/
void by_cmd_send_speed_y(float speed)
{
LOCKAPI();
uint8_t buff[4] = {0};
memcpy(buff, &speed, 4);
by_frame_send(0x32, buff, 4);
UNLOCKAPI();
}
/**
@@ -101,9 +112,13 @@ void by_cmd_send_speed_y(float speed)
*/
void by_cmd_send_speed_omega(float speed)
{
LOCKAPI();
uint8_t buff[4] = {0};
memcpy(buff, &speed, 4);
by_frame_send(0x33, buff, 4);
UNLOCKAPI();
}
/**
@@ -116,11 +131,16 @@ void by_cmd_send_speed_omega(float speed)
*/
int by_cmd_send_distance_x(float speed, uint32_t time)
{
LOCKAPI();
log_info("send distance_x speed:%.1f, time:%dms", speed, time * 5);
int ret = 0;
uint8_t buff[8] = {0};
memcpy(buff, &speed, 4);
memcpy(buff + 4, &time, 4);
by_frame_send(0x34, buff, 8);
return (by_cmd_reg_listerning(0x34, 1000));
ret = by_cmd_reg_listerning(0x34, 1000);
UNLOCKAPI();
return (ret);
}
/**
@@ -133,11 +153,16 @@ int by_cmd_send_distance_x(float speed, uint32_t time)
*/
int by_cmd_send_distance_y(float speed, uint32_t time)
{
LOCKAPI();
log_info("send distance_y speed:%.1f, time:%dms", speed, time * 5);
int ret = 0;
uint8_t buff[8] = {0};
memcpy(buff, &speed, 4);
memcpy(buff + 4, &time, 4);
by_frame_send(0x35, buff, 8);
return (by_cmd_reg_listerning(0x35, 1000));
ret = by_cmd_reg_listerning(0x35, 1000);
UNLOCKAPI();
return (ret);
}
/**
@@ -150,32 +175,52 @@ int by_cmd_send_distance_y(float speed, uint32_t time)
*/
int by_cmd_send_angle_omega(float speed, uint32_t time)
{
LOCKAPI();
log_info("send angle_omega:%.1f, time:%dms", speed, time * 5);
int ret = 0;
uint8_t buff[4] = {0};
memcpy(buff, &speed, 4);
memcpy(buff + 4, &time, 4);
by_frame_send(0x36, buff, 8);
return (by_cmd_reg_listerning(0x36, 1000));
ret = by_cmd_reg_listerning(0x36, 1000);
UNLOCKAPI();
return (ret);
}
int by_cmd_send_reset_axis_x(void)
{
LOCKAPI();
log_info("send reset_axis_x");
int ret = 0;
uint8_t buff[4] = {0};
by_frame_send(0x41, buff, 4);
return (by_cmd_reg_listerning(0x41, 1000));
ret = by_cmd_reg_listerning(0x41, 1000);
UNLOCKAPI();
return (ret);
}
int by_cmd_send_reset_axis_z(void)
{
LOCKAPI();
log_info("send reset_axis_z");
int ret = 0;
uint8_t buff[4] = {0};
by_frame_send(0x42, buff, 4);
return (by_cmd_reg_listerning(0x42, 1000));
ret = by_cmd_reg_listerning(0x42, 1000);
UNLOCKAPI();
return (ret);
}
int by_cmd_send_reset_end_effector(void)
{
LOCKAPI();
log_info("send end_effector");
int ret = 0;
uint8_t buff[4] = {0};
by_frame_send(0x43, buff, 4);
return (by_cmd_reg_listerning(0x43, 1000));
ret = by_cmd_reg_listerning(0x43, 1000);
UNLOCKAPI();
return (ret);
}
/**
@@ -187,11 +232,16 @@ int by_cmd_send_reset_end_effector(void)
*/
int by_cmd_send_distance_axis_x(uint8_t speed, float distance)
{
LOCKAPI();
log_info("send distance_axis_x speed:%d, distance:%.2f", speed, distance);
int ret = 0;
uint8_t buff[8] = {0};
memcpy(buff, &speed, 1);
memcpy(buff + 1, &distance, 4);
memcpy(&buff[0], &speed, 1);
memcpy(&buff[4], &distance, 4);
by_frame_send(0x44, buff, 8);
return (by_cmd_reg_listerning(0x44, 1000));
ret = by_cmd_reg_listerning(0x44, 1000);
UNLOCKAPI();
return (ret);
}
/**
@@ -203,11 +253,16 @@ int by_cmd_send_distance_axis_x(uint8_t speed, float distance)
*/
int by_cmd_send_distance_axis_z(uint8_t speed, float distance)
{
LOCKAPI();
log_info("send distance_axis_z speed:%d, distance:%.2f", speed, distance);
int ret = 0;
uint8_t buff[8] = {0};
memcpy(buff, &speed, 1);
memcpy(buff + 1, &distance, 4);
memcpy(&buff[0], &speed, 1);
memcpy(&buff[4], &distance, 4);
by_frame_send(0x46, buff, 8);
return (by_cmd_reg_listerning(0x46, 1000));
ret = by_cmd_reg_listerning(0x46, 1000);
UNLOCKAPI();
return (ret);
}
/**
@@ -219,11 +274,16 @@ int by_cmd_send_distance_axis_z(uint8_t speed, float distance)
*/
int by_cmd_send_position_axis_x(uint8_t speed, float position)
{
LOCKAPI();
log_info("send position_axis_x speed:%d, distance:%.2f", speed, position);
int ret = 0;
uint8_t buff[8] = {0};
memcpy(buff, &speed, 1);
memcpy(buff + 1, &position, 4);
memcpy(&buff[0], &speed, 1);
memcpy(&buff[4], &position, 4);
by_frame_send(0x47, buff, 8);
return (by_cmd_reg_listerning(0x47, 1000));
ret = by_cmd_reg_listerning(0x47, 1000);
UNLOCKAPI();
return (ret);
}
/**
@@ -235,11 +295,16 @@ int by_cmd_send_position_axis_x(uint8_t speed, float position)
*/
int by_cmd_send_position_axis_z(uint8_t speed, float position)
{
LOCKAPI();
log_info("send position_axis_z speed:%d, distance:%.2f", speed, position);
int ret = 0;
uint8_t buff[8] = {0};
memcpy(buff, &speed, 1);
memcpy(buff + 1, &position, 4);
memcpy(&buff[0], &speed, 1);
memcpy(&buff[4], &position, 4);
by_frame_send(0x49, buff, 8);
return (by_cmd_reg_listerning(0x49, 1000));
ret = by_cmd_reg_listerning(0x49, 1000);
UNLOCKAPI();
return (ret);
}
/**
@@ -250,10 +315,15 @@ int by_cmd_send_position_axis_z(uint8_t speed, float position)
*/
int by_cmd_send_angle_claw_arm(float angle)
{
LOCKAPI();
log_info("send angle_claw_arm angle:%.2f", angle);
int ret = 0;
uint8_t buff[4] = {0};
memcpy(buff, &angle, 4);
by_frame_send(0x50, buff, 4);
return (by_cmd_reg_listerning(0x50, 1000));
ret = by_cmd_reg_listerning(0x50, 1000);
UNLOCKAPI();
return (ret);
}
/**
@@ -264,10 +334,15 @@ int by_cmd_send_angle_claw_arm(float angle)
*/
int by_cmd_send_angle_claw(float angle)
{
LOCKAPI();
log_info("send angle_claw angle:%.2f", angle);
int ret = 0;
uint8_t buff[4] = {0};
memcpy(buff, &angle, 4);
by_frame_send(0x51, buff, 4);
return (by_cmd_reg_listerning(0x51, 1000));
ret = by_cmd_reg_listerning(0x51, 1000);
UNLOCKAPI();
return (ret);
}
/**
@@ -278,10 +353,15 @@ int by_cmd_send_angle_claw(float angle)
*/
int by_cmd_send_angle_camera(float angle)
{
LOCKAPI();
log_info("send angle_camera angle:%.2f", angle);
int ret = 0;
uint8_t buff[4] = {0};
memcpy(buff, &angle, 4);
by_frame_send(0x52, buff, 4);
return (by_cmd_reg_listerning(0x52, 1000));
ret = by_cmd_reg_listerning(0x52, 1000);
UNLOCKAPI();
return (ret);
}
/**
@@ -292,10 +372,15 @@ int by_cmd_send_angle_camera(float angle)
*/
int by_cmd_send_angle_scoop(float angle)
{
LOCKAPI();
log_info("send angle_scoop angle:%.2f", angle);
int ret = 0;
uint8_t buff[4] = {0};
memcpy(buff, &angle, 4);
by_frame_send(0x53, buff, 4);
return (by_cmd_reg_listerning(0x53, 1000));
ret = by_cmd_reg_listerning(0x53, 1000);
UNLOCKAPI();
return (ret);
}
/**
@@ -306,25 +391,45 @@ int by_cmd_send_angle_scoop(float angle)
*/
int by_cmd_send_angle_storage(float angle)
{
LOCKAPI();
log_info("send angle_storage angle:%.2f", angle);
int ret = 0;
uint8_t buff[4] = {0};
memcpy(buff, &angle, 4);
by_frame_send(0x54, buff, 4);
return (by_cmd_reg_listerning(0x54, 1000));
ret = by_cmd_reg_listerning(0x54, 1000);
UNLOCKAPI();
return (ret);
}
int by_cmd_send_ranging_start(void)
{
LOCKAPI();
int ret = 0;
uint8_t buff[4] = {0};
by_frame_send(0x55, buff, 4);
return (by_cmd_reg_listerning(0x55, 1000));
ret = by_cmd_reg_listerning(0x55, 1000);
UNLOCKAPI();
return (ret);
}
int by_cmd_recv_ranging_data(float *distance)
{
LOCKAPI();
int ret = 0;
uint8_t buff[4] = {0};
by_frame_send(0x56, buff, 4);
if (!by_cmd_reg_listerning(0x56, 1000)) {
ret = by_cmd_reg_listerning(0x56, 1000);
if (!ret) {
memcpy(distance, &received_data[0], 4);
}
UNLOCKAPI();
if (!ret) {
return 0;
} else {
return -1;