project_bycmd/by_cmd.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;

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");
  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)
{
  LOCKAPI();

  uint8_t buff[4] = {0};
  memcpy(buff, &speed, 4);
  by_frame_send(0x31, buff, 4);
  UNLOCKAPI();
}

/**
 * @brief 发送 y 方向速度指令（无应答）
 *
 * @param 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();
}

/**
 * @brief 发送角速度指令（无应答）
 *
 * @param 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();
}

/**
 * @brief 发送 x 方向移动距离指令（等待应答）
 *
 * @param speed
 * @param time
 * @return 0 发送成功
 * @return -1 应答超时
 */
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);
  ret = by_cmd_reg_listerning(0x34, 1000);
  UNLOCKAPI();
  return (ret);
}

/**
 * @brief 发送 y 方向移动距离指令（等待应答）
 *
 * @param speed
 * @param time
 * @return true 发送成功
 * @return false 应答超时
 */
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);
  ret = by_cmd_reg_listerning(0x35, 1000);
  UNLOCKAPI();
  return (ret);
}

/**
 * @brief 发送 omega 方向旋转角度指令（等待应答）
 *
 * @param speed
 * @param time
 * @return true 发送成功
 * @return false 应答超时
 */
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);
  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);
  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);
  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);
  ret = by_cmd_reg_listerning(0x43, 1000);
  UNLOCKAPI();
  return (ret);
}

/**
 * @brief 设置 x 轴增量位置
 *
 * @param speed
 * @param distance
 * @return int
 */
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[0], &speed, 1);
  memcpy(&buff[4], &distance, 4);
  by_frame_send(0x44, buff, 8);
  ret = by_cmd_reg_listerning(0x44, 1000);
  UNLOCKAPI();
  return (ret);
}

/**
 * @brief 设置 z 轴增量位置
 *
 * @param speed
 * @param distance
 * @return int
 */
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[0], &speed, 1);
  memcpy(&buff[4], &distance, 4);
  by_frame_send(0x46, buff, 8);
  ret = by_cmd_reg_listerning(0x46, 1000);
  UNLOCKAPI();
  return (ret);
}

/**
 * @brief 发送 x 轴绝对位置
 *
 * @param speed
 * @param position
 * @return int
 */
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[0], &speed, 1);
  memcpy(&buff[4], &position, 4);
  by_frame_send(0x47, buff, 8);
  ret = by_cmd_reg_listerning(0x47, 1000);
  UNLOCKAPI();
  return (ret);
}

/**
 * @brief 发送 z 轴绝对位置
 *
 * @param speed
 * @param position
 * @return int
 */
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[0], &speed, 1);
  memcpy(&buff[4], &position, 4);
  by_frame_send(0x49, buff, 8);
  ret = by_cmd_reg_listerning(0x49, 1000);
  UNLOCKAPI();
  return (ret);
}

/**
 * @brief 设置夹爪摇臂角度
 *
 * @param angle
 * @return int
 */
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);
  ret = by_cmd_reg_listerning(0x50, 1000);
  UNLOCKAPI();
  return (ret);
}

/**
 * @brief 设置夹爪角度
 *
 * @param angle
 * @return int
 */
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);
  ret = by_cmd_reg_listerning(0x51, 1000);
  UNLOCKAPI();
  return (ret);
}

/**
 * @brief 设置摄像头角度
 *
 * @param angle
 * @return int
 */
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);
  ret = by_cmd_reg_listerning(0x52, 1000);
  UNLOCKAPI();
  return (ret);
}

/**
 * @brief 设置顶端抓取机构角度
 *
 * @param angle
 * @return int
 */
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);
  ret = by_cmd_reg_listerning(0x53, 1000);
  UNLOCKAPI();
  return (ret);
}

/**
 * @brief 设置托盘角度
 *
 * @param angle
 * @return int
 */
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);
  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);
  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);
  ret = by_cmd_reg_listerning(0x56, 1000);

  if (!ret) {
    memcpy(distance, &received_data[0], 4);
  }

  UNLOCKAPI();

  if (!ret) {
    return 0;
  } else {
    return -1;
  }
}