#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_operete(id, cmd, &param);
}