You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
53 lines
1.2 KiB
53 lines
1.2 KiB
|
|
#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);
|
|
// }
|