auxiliary_sample_addition_new/Usr/module/feite_servo/servo_driver.c

#include "servo_driver.h"
#include "servo_package_process.h"

static servo_obj_t servo[servo_amount] = {
    {STS_small_L}, {STS_small_M}, {STS_small_R}, {STS_large_R}, {STS_large_Y},
};
static param_t param;

/**
 * @description: ��ȡ��������
 * @param {uint8_t} servo_id ����id
 * @return {*}
 */
servo_obj_t* Get_Servo(uint8_t servo_id) {
  if (servo_id > STS_large_Y || servo_id < STS_small_L) {
    
    while(1){
      printf("Servo_id dose not exist:%d\n", servo_id);
    }
  }
printf("Servo id : %d\n", servo_id);
  //return &servo[servo_id];
  
}

/**
 * @description: ������������
 * @param {servo_obj_t*} servo ��������
 * @param {uint8_t} function   ʵ�ֹ���
 * @param {int32_t} param      ��������
 * @return {*}
 */
void servo_drive_single(uint8_t id, uint8_t addr, uint8_t cmd, int16_t *param_data, uint8_t param_count) {
  
  param.addr = addr;
  param.data = param_data;
  param.count = param_count;

  if (!(cmd == W_DATA || cmd == REG_W_DATA)) {
    printf("write cmd error :%02X", cmd);
    return;
  }

  Write_oprete(id, cmd, &param);
}

// void servo_drive_multi(servo_obj_t* servo, uint8_t function, int32_t param) {
//   servo->reg  = function;
//   servo->data = param;
//   servo->cmd  = SYC_W_DATA;
//   Write_oprete(servo);
// }