#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, ¶m); } // 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); // }