182 lines
3.4 KiB
C
182 lines
3.4 KiB
C
#include "by_cmd.h"
|
|
|
|
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
#include <signal.h>
|
|
#include <unistd.h>
|
|
#include <pthread.h>
|
|
#include "by_frame.h"
|
|
#include "logc/log.h"
|
|
|
|
uint8_t cmd;
|
|
uint32_t received_data[2];
|
|
|
|
uint8_t listerning_cmd;
|
|
int received_flag;
|
|
|
|
int by_cmd_init(void)
|
|
{
|
|
log_debug("by_cmd init");
|
|
return (by_frame_init());
|
|
}
|
|
|
|
void *by_cmd_loop(void *timeout)
|
|
{
|
|
while (*(int *)timeout > 0) {
|
|
int ret = by_frame_parse(&cmd, received_data);
|
|
|
|
if (!ret && cmd == listerning_cmd) {
|
|
// log_debug("got frame cmd %2X", cmd);
|
|
break;
|
|
}
|
|
usleep(10);
|
|
}
|
|
|
|
received_flag = 1;
|
|
listerning_cmd = 0;
|
|
|
|
return NULL;
|
|
}
|
|
|
|
/**
|
|
* @brief 注册监听的
|
|
*s
|
|
* @param cmd
|
|
*/
|
|
int by_cmd_reg_listerning(uint8_t cmd, int timeout)
|
|
{
|
|
listerning_cmd = cmd;
|
|
received_flag = 0;
|
|
|
|
pthread_t thread_listerning;
|
|
pthread_create(&thread_listerning, NULL, by_cmd_loop, &timeout);
|
|
|
|
while (timeout--) {
|
|
if (received_flag) {
|
|
// log_debug("received done! using %fSEC", (float)timeout_cnt * 0.00005);
|
|
listerning_cmd = 0;
|
|
received_flag = 0;
|
|
return 0;
|
|
}
|
|
usleep(10);
|
|
}
|
|
|
|
listerning_cmd = 0;
|
|
received_flag = 0;
|
|
|
|
log_warn("get cmd time out");
|
|
|
|
return -1;
|
|
}
|
|
|
|
/**
|
|
* @brief 发送 x 方向速度指令（无应答）
|
|
*
|
|
* @param speed
|
|
*/
|
|
void by_cmd_send_speed_x(float speed)
|
|
{
|
|
uint8_t buff[4] = {0};
|
|
memcpy(buff, &speed, 4);
|
|
|
|
by_frame_send(0x31, buff, 4);
|
|
}
|
|
|
|
/**
|
|
* @brief 发送 y 方向速度指令（无应答）
|
|
*
|
|
* @param speed
|
|
*/
|
|
void by_cmd_send_speed_y(float speed)
|
|
{
|
|
uint8_t buff[4] = {0};
|
|
memcpy(buff, &speed, 4);
|
|
|
|
by_frame_send(0x32, buff, 4);
|
|
}
|
|
|
|
/**
|
|
* @brief 发送角速度指令（无应答）
|
|
*
|
|
* @param speed
|
|
*/
|
|
void by_cmd_send_speed_omega(float speed)
|
|
{
|
|
uint8_t buff[4] = {0};
|
|
memcpy(buff, &speed, 4);
|
|
|
|
by_frame_send(0x33, buff, 4);
|
|
}
|
|
|
|
/**
|
|
* @brief 发送 x 方向移动距离指令（等待应答）
|
|
*
|
|
* @param speed
|
|
* @param time
|
|
* @return true 发送成功
|
|
* @return false 应答超时
|
|
*/
|
|
int by_cmd_send_distance_x(float speed, uint32_t time)
|
|
{
|
|
uint8_t buff[4] = {0};
|
|
memcpy(buff, &speed, 4);
|
|
memcpy(buff + 4, &time, 4);
|
|
by_frame_send(0x34, buff, 8);
|
|
return (by_cmd_reg_listerning(0x34, 1000));
|
|
}
|
|
|
|
/**
|
|
* @brief 发送 y 方向移动距离指令（等待应答）
|
|
*
|
|
* @param speed
|
|
* @param time
|
|
* @return true 发送成功
|
|
* @return false 应答超时
|
|
*/
|
|
int by_cmd_send_distance_y(float speed, uint32_t time)
|
|
{
|
|
uint8_t buff[4] = {0};
|
|
memcpy(buff, &speed, 4);
|
|
memcpy(buff + 4, &time, 4);
|
|
by_frame_send(0x35, buff, 8);
|
|
return (by_cmd_reg_listerning(0x35, 1000));
|
|
}
|
|
|
|
/**
|
|
* @brief 发送 omega 方向旋转角度指令（等待应答）
|
|
*
|
|
* @param speed
|
|
* @param time
|
|
* @return true 发送成功
|
|
* @return false 应答超时
|
|
*/
|
|
int by_cmd_send_angle_omega(float speed, uint32_t time)
|
|
{
|
|
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));
|
|
}
|
|
|
|
int by_cmd_send_reset_axis_x(void)
|
|
{
|
|
uint8_t buff[4] = {0};
|
|
by_frame_send(0x41, buff, 4);
|
|
return (by_cmd_reg_listerning(0x41, 1000));
|
|
}
|
|
|
|
int by_cmd_send_reset_axis_z(void)
|
|
{
|
|
uint8_t buff[4] = {0};
|
|
by_frame_send(0x42, buff, 4);
|
|
return (by_cmd_reg_listerning(0x42, 1000));
|
|
}
|
|
|
|
int by_cmd_send_reset_end_effector(void)
|
|
{
|
|
uint8_t buff[4] = {0};
|
|
by_frame_send(0x43, buff, 4);
|
|
return (by_cmd_reg_listerning(0x43, 1000));
|
|
} |