Commit c1b50087 authored by Wenxi Xu's avatar Wenxi Xu
Browse files

重构电机驱动API，合并获取和设置功能，更新电机控制模式，添加多环控制模式支持，优化电机状态获取逻辑，更新示例代码以反映新接口

parent c490efc1

Documents/motor/api.md

+24 −17
Original line number Diff line number Diff line 
# 电机驱动API

```c

motor_driver_api {

    float motor_get_speed(const struct device *dev);

    float motor_get_torque(const struct device *dev);

    float motor_get_angle(const struct device *dev);

    int motor_set_speed(const struct device *dev, float speed_rpm);

    int motor_set_torque(const struct device *dev, float torque);

    int motor_set_angle(const struct device *dev, float angle);

    int motor_set_mode(const struct device *dev, enum motor_mode mode);

    int motor_get(const struct device *dev, motor_status_t *status);

    int motor_set(const struct device *dev, motor_status_t *status);

    void motor_control(const struct device *dev, enum motor_cmd cmd);

};

```
@@ -18,22 +13,34 @@ motor_driver_api { 
 * MIT: MIT模式

 * PV: 位置-速度控制

 * VO: 速度控制  

 * MULTILOOP: 多环串联控制

 * ML_TORQUE: 多环扭矩控制

 * ML_ANGLE: 多环角度控制

 * ML_SPEED: 多环速度控制

 */

enum motor_mode { MIT, PV, VO, MULTILOOP };

enum motor_mode { MIT, PV, VO, ML_TORQUE, ML_ANGLE, ML_SPEED };

```

```c

/**

 * @brief 电机控制命令

 */

enum motor_cmd { ENABLE_MOTOR, DISABLE_MOTOR, SET_ZERO_OFFSET, CLEAR_PID, CLEAR_ERROR };

enum motor_cmd { ENABLE_MOTOR, DISABLE_MOTOR, SET_ZERO, CLEAR_PID, CLEAR_ERROR };

```

## 电机状态获取

`motor_get_speed(const struct device *dev)`: 获取电机当前速度，返回值为电机当前速度，单位为rpm。

`motor_get_torque(const struct device *dev)`: 获取电机当前扭矩，返回值为电机当前扭矩，单位为N*m。

`motor_get_angle(const struct device *dev)`: 获取电机当前角度，返回值为电机当前角度，单位为°。

`motor_set_speed(const struct device *dev, float speed_rpm)`: 设置电机目标速度，参数为目标速度，单位为rpm。

`motor_set_torque(const struct device *dev, float torque)`: 设置电机目标扭矩，参数为目标扭矩，单位为N*m。

`motor_set_angle(const struct device *dev, float angle)`: 设置电机目标角度，参数为目标角度，单位为°。

`motor_set_mode(const struct device *dev, enum motor_mode mode)`: 设置电机工作模式，参数为工作模式。

`motor_get(const struct device *dev, motor_status_t *status)`: 获取电机当前状态，参数为电机状态结构体。

`motor_set(const struct device *dev, motor_status_t *status)`: 设置电机目标状态，参数为电机状态结构体。

`motor_control(const struct device *dev, enum motor_cmd cmd)`: 控制电机，参数为控制命令。



## 示例

设置电机为多环扭矩控制模式，并设置目标扭矩为10N*m。

```c

motor_status_t status;

status.mode = ML_TORQUE;

status.torque = 10;

motor_set(motor, &status);

```

获取电机当前的扭矩

```c

motor_status_t status;

motor_get(motor, &status);

LOG_INF("Motor torque: %.2f", status.torque);

```
 
 No newline at end of file

drivers/motor/dji/motor_dji.c

+82 −52
Original line number Diff line number Diff line
@@ -135,7 +135,6 @@ void dji_torque_limit(const struct device *dev, float max_torque, float min_torq 
int dji_set_speed(const struct device *dev, float speed_rpm)

{

	struct dji_motor_data *data = dev->data;

	const struct dji_motor_config *cfg = dev->config;



	if (speed_rpm > data->common.speed_limit[1]) {

		speed_rpm = data->common.speed_limit[1];
@@ -144,56 +143,20 @@ int dji_set_speed(const struct device *dev, float speed_rpm) 
	}



	data->target_rpm = speed_rpm;

	data->current_mode_index = -1;

	for (int i = 0; i < sizeof(cfg->common.pid_datas) / sizeof(cfg->common.pid_datas[0]); i++) {

		if (cfg->common.pid_datas[i]->pid_dev == NULL) {

			if (data->current_mode_index == -1) {

				data->current_mode_index = i;

				data->target_torque = 0;

				return -ENOEXEC;

			}

			break;

		}

		if (strcmp(cfg->common.capabilities[i], "speed") == 0) {

			pid_calc(cfg->common.pid_datas[i]);

			data->current_mode_index = i;

		}

	}

	// ctrl_struct->current[id] = pid_calc(dev);



	return 0;

}



int dji_set_angle(const struct device *dev, float angle)

{

	struct dji_motor_data *data = dev->data;

	const struct dji_motor_config *cfg = dev->config;



	data->target_angle = angle;



	data->current_mode_index = -1;

	for (int i = 0; i < SIZE_OF_ARRAY(cfg->common.pid_datas); i++) {

		if (cfg->common.pid_datas[i]->pid_dev == NULL) {

			if (data->current_mode_index == -1) {

				data->current_mode_index = i;

				data->target_torque = 0;

				return -ENOEXEC;

			}

			break;

		}

		if (strcmp(cfg->common.capabilities[i], "angle") == 0) {

			pid_calc(cfg->common.pid_datas[i]);

			data->current_mode_index = i;

		}

	}



	// ctrl_struct->current[id] = pid_calc(dev);

	return 0;

}



int dji_set_torque(const struct device *dev, float torque)

{

	struct dji_motor_data *data = dev->data;

	const struct dji_motor_config *cfg = dev->config;



	if (torque > data->common.torque_limit[1]) {

		torque = data->common.torque_limit[1];
@@ -202,17 +165,74 @@ int dji_set_torque(const struct device *dev, float torque) 
	}



	data->target_torque = torque;



	return 0;

}



int dji_set_mode(const struct device *dev, enum motor_mode mode)

{

	struct dji_motor_data *data = dev->data;

	const struct dji_motor_config *cfg = dev->config;



	char mode_str[10];

	switch (mode) {

	case ML_TORQUE:

		strcpy(mode_str, "torque");

		break;

	case ML_ANGLE:

		strcpy(mode_str, "angle");

		break;

	case ML_SPEED:

		strcpy(mode_str, "speed");

		break;

	default:

		LOG_ERR("Unsupported motor mode: %d", mode);

		return -ENOSYS;

	}



	data->current_mode_index = -1;



	for (int i = 0; i < SIZE_OF_ARRAY(cfg->common.pid_datas); i++) {

		if (cfg->common.pid_datas[i]->pid_dev == NULL) {

			data->current_mode_index = i;

			break;

		}

		if (strcmp(cfg->common.capabilities[i], "torque") == 0) {

		if (strcmp(cfg->common.capabilities[i], mode_str) == 0) {

			pid_calc(cfg->common.pid_datas[i]);

			data->current_mode_index = i;

		}

	}

	// ctrl_struct->current[id] = pid_calc(dev);



	if (data->current_mode_index == -1) {

		LOG_ERR("No motor mode found for %s", mode_str);

		return -ENOSYS;

	}



	return 0;

}



int dji_set(const struct device *dev, motor_status_t *status)

{

	if (status->mode == ML_TORQUE) {

		dji_set_torque(dev, status->torque);

	} else if (status->mode == ML_ANGLE) {

		dji_set_angle(dev, status->angle + 360.0f * (float)status->round_cnt);

	} else if (status->mode == ML_SPEED) {

		dji_set_speed(dev, status->rpm);

	} else {

		LOG_ERR("Unsupported motor mode: %d", status->mode);

		return -ENOSYS;

	}



	dji_set_mode(dev, status->mode);



	if (status->speed_limit[0] > 0) {

		dji_speed_limit(dev, status->speed_limit[1], status->speed_limit[0]);

	}

	if (status->torque_limit[0] > 0) {

		dji_torque_limit(dev, status->torque_limit[1], status->torque_limit[0]);

	}



	return 0;

}
@@ -231,7 +251,7 @@ void dji_control(const struct device *dev, enum motor_cmd cmd) 
	case DISABLE_MOTOR:

		data->online = false;

		break;

	case SET_ZERO_OFFSET:

	case SET_ZERO:

		data->angle_add = 0;

		data->common.angle = 0;

		break;
@@ -244,22 +264,32 @@ void dji_control(const struct device *dev, enum motor_cmd cmd) 
	}

}



float dji_get_angle(const struct device *dev)

int dji_get(const struct device *dev, motor_status_t *status)

{

	struct dji_motor_data *data = dev->data;

	return fmodf(data->common.angle, 360.0f);

}

	const struct dji_motor_config *cfg = dev->config;



float dji_get_speed(const struct device *dev)

{

	struct dji_motor_data *data = dev->data;

	return data->common.rpm;

	if (strcmp(cfg->common.capabilities[data->current_mode_index], "torque") == 0) {

		status->mode = ML_TORQUE;

	} else if (strcmp(cfg->common.capabilities[data->current_mode_index], "angle") == 0) {

		status->mode = ML_ANGLE;

	} else if (strcmp(cfg->common.capabilities[data->current_mode_index], "speed") == 0) {

		status->mode = ML_SPEED;

	} else if (strcmp(cfg->common.capabilities[data->current_mode_index], "mit") == 0) {

		status->mode = MIT;

	}



float dji_get_torque(const struct device *dev)

{

	struct dji_motor_data *data = dev->data;

	return data->common.torque;

	status->angle = fmodf(data->common.angle, 360.0f);

	status->rpm = data->common.rpm;

	status->torque = data->common.torque;

	status->round_cnt = data->common.round_cnt;



	status->speed_limit[0] = data->common.speed_limit[0];

	status->speed_limit[1] = data->common.speed_limit[1];

	status->torque_limit[0] = data->common.torque_limit[0];

	status->torque_limit[1] = data->common.torque_limit[1];



	return 0;

}



int dji_init(const struct device *dev)

drivers/motor/dji/motor_dji.h

+6 −21
Original line number Diff line number Diff line
@@ -105,19 +105,10 @@ struct dji_motor_config { 
// 全局变量声明

extern struct motor_controller ctrl_structs[];



// 函数声明

static void can_rx_callback(const struct device *can_dev, struct can_frame *frame, void *user_data);



void dji_speed_limit(const struct device *dev, float max_speed, float min_speed);

void dji_torque_limit(const struct device *dev, float max_torque, float min_torque);

int dji_set_speed(const struct device *dev, float speed_rpm);

int dji_set_angle(const struct device *dev, float angle);

int dji_set_torque(const struct device *dev, float torque);

float dji_set_zero(const struct device *dev);



float dji_get_angle(const struct device *dev);

float dji_get_speed(const struct device *dev);

float dji_get_torque(const struct device *dev);

int dji_set_mode(const struct device *dev, enum motor_mode mode);



int dji_get(const struct device *dev, motor_status_t *status);

int dji_set(const struct device *dev, motor_status_t *status);

int dji_init(const struct device *dev);



void dji_control(const struct device *dev, enum motor_cmd cmd);
@@ -138,15 +129,9 @@ K_WORK_DEFINE(dji_init_handle, dji_init_handler); 
K_TIMER_DEFINE(dji_miss_handle_timer, NULL, NULL);



static const struct motor_driver_api motor_api_funcs = {

	.motor_get_speed = dji_get_speed,

	.motor_get_torque = dji_get_torque,

	.motor_get_angle = dji_get_angle,

	.motor_set_speed = dji_set_speed,

	.motor_set_torque = dji_set_torque,

	.motor_set_angle = dji_set_angle,

	.motor_get = dji_get,

	.motor_set = dji_set,

	.motor_control = dji_control,

	.motor_limit_speed = dji_speed_limit,

	.motor_limit_torque = dji_torque_limit,

};



extern const struct device *can_devices[];

motor/boards/robomaster_board_c.overlay

+42 −7
Original line number Diff line number Diff line
@@ -5,22 +5,57 @@ 
		id = <1>;

		rx_id = <0x201>;

		tx_id = <0x200>;

		gear_ratio = "19.20";

		gear_ratio = "15.76";

		status = "okay";

		can_channel = <&canbus1>;

		controllers = <&speed_pid>;

		capabilities = "speed";

		controllers = <&wheel_angle_pid &wheel_speed_pid>;

		capabilities = "angle", "speed";

	};

	motor_steer0: motor_steer0{

		compatible = "dji,motor";

		is_gm6020;

		id = <5>;

		rx_id = <0x209>;

		tx_id = <0x2FF>;

		gear_ratio = "1.0";

		status = "okay";

		can_channel = <&canbus1>;

		controllers = <&steer_angle_pid &steer_speed_pid>;

		capabilities = "angle", "speed";

	};



	pid {

		speed_pid: speed_pid {

		wheel_speed_pid: wheel_speed_pid {

			#controller-cells = <0>;

			compatible = "pid,single";

			k_p = "2.65";

			k_d = "60";

			detri_lpf = "0.985";

		};



		wheel_angle_pid: wheel_angle_pid {

			#controller-cells = <0>;

			compatible = "pid,single";

			k_p = "1.8";

			k_i = "2.0";

			k_d = "2";

			k_p = "3";

			k_d = "320";

			detri_lpf = "0.985";

		};



		steer_speed_pid: steer_speed_pid {

			#controller-cells = <0>;

			compatible = "pid,single";

			k_p = "0.0195";

			k_d = "4.85";

			detri_lpf = "0.95";

		};



		steer_angle_pid: steer_angle_pid {

			#controller-cells = <0>;

			compatible = "pid,single";

			k_p = "2.0";

			k_d = "0";

			detri_lpf = "0.9985";

		};

	};

	sbus0: sbus0 {

		compatible = "ares,sbus";

motor/prj.conf

+1 −1
Original line number Diff line number Diff line
@@ -37,7 +37,7 @@ CONFIG_LOG_BACKEND_UART=y 
CONFIG_SERIAL=y



# 禁用优化

CONFIG_NO_OPTIMIZATIONS=y

CONFIG_NO_OPTIMIZATIONS=n

CONFIG_COMPILER_OPT=""



# 启用驱动

CRA Git | Maintained and supported by SUSTech CRA and CCSE