Damiao driver working now (8fdbeacd) · Commits · Wenxi XU / Mambo

.vscode/settings.json

+1 −1

Original line number	Diff line number	Diff line
		@@ -32,7 +32,7 @@
		},
		"C_Cpp.formatting": "clangFormat",
		"C_Cpp.errorSquiggles": "disabled",
		"cortex-debug.variableUseNaturalFormat": false,
		"cortex-debug.variableUseNaturalFormat": true,
		"C_Cpp.default.compileCommands": "${workspaceFolder}/.vscode/compile_commands.json",
		"cmake.configureOnOpen": false,

drivers/motor/motor_dm.c

+20 −9

Original line number	Diff line number	Diff line
		@@ -93,7 +93,7 @@ int dm_send_queued(struct tx_frame frame, struct k_msgq msgq) {
		LOG_ERR("TX queue full, will be put into msgq: %d", err);
		}
		} else if (err < 0) {
		LOG_ERR("CAN hardware TX queue is full. (err %d)", err);
		// LOG_ERR("CAN hardware TX queue is full. (err %d)", err);
		err = k_msgq_put(msgq, frame, K_NO_WAIT);
		if (err) {
		LOG_ERR("Failed to put CAN frame into TX queue: %d", err);
		@@ -105,13 +105,23 @@ int dm_send_queued(struct tx_frame frame, struct k_msgq msgq) {
		int dm_queue_proceed(struct k_msgq *msgq) {
		struct tx_frame frame;
		int err = 0;
		bool give_up = false;
		while (!k_msgq_get(msgq, &frame, K_NO_WAIT)) {
		err = k_sem_take(frame.sem, K_NO_WAIT);
		if (err == 0) {
		err = can_send(frame.can_dev, &(frame.frame), K_MSEC(1), can_tx_callback, frame.sem);
		if (err) {
		LOG_ERR("Failed to send CAN frame: %d", err);
		}
		k_msgq_purge(msgq);
		} else {
		if (give_up) {
		k_msgq_purge(msgq);
		break;
		}
		k_sleep(K_USEC(300));
		give_up = true;
		continue;
		}
		}
		return err;
		@@ -319,7 +329,7 @@ int get_motor_id(int id) {
		static struct can_filter filters[CAN_COUNT];

		static void dm_motor_ctrl_entry(void arg1, void arg2, void *arg3) {
		LOG_ERR("DM motor control thread started");
		LOG_DBG("DM motor control thread started");
		struct can_frame tx_frame;

		for (int i = 0; i < MOTOR_COUNT; i++) {
		@@ -354,7 +364,7 @@ static void dm_motor_ctrl_entry(void arg1, void arg2, void *arg3) {
		// can ignore that.
		}

		k_sleep(K_MSEC(60));
		k_sleep(K_MSEC(1000));

		for (int i = 0; i < MOTOR_COUNT; i++) {
		dm_motor_control(motor_devices[i], ENABLE_MOTOR);
		@@ -362,7 +372,7 @@ static void dm_motor_ctrl_entry(void arg1, void arg2, void *arg3) {

		for (;;) {
		struct can_frame rx_frame;
		while (!k_msgq_get(&dm_can_rx_msgq, &rx_frame, K_MSEC(1))) {
		while (!k_msgq_get(&dm_can_rx_msgq, &rx_frame, K_NO_WAIT)) {
		int id = get_motor_id(rx_frame.id);
		if (id == -1) {
		LOG_ERR("Unknown motor ID: %d", rx_frame.id);
		@@ -373,7 +383,9 @@ static void dm_motor_ctrl_entry(void arg1, void arg2, void *arg3) {
		const struct dm_motor_config *cfg =
		(const struct dm_motor_config *)(motor_devices[id]->config);

		if (data->missed_times > 0) {
		data->missed_times--;
		}
		data->err = rx_frame.data[0] >> 4;

		float prev_angle = data->common.angle;
		@@ -411,21 +423,20 @@ static void dm_motor_ctrl_entry(void arg1, void arg2, void *arg3) {
		};
		dm_send_queued(&queued_frame, &dm_can_tx_msgq);
		}
		if (++data->missed_times > 0) {
		LOG_ERR("Motor %d is not responding, trying to recover...", i);
		if (++(data->missed_times) > 3) {
		dm_motor_control(motor_devices[i], CLEAR_ERROR);
		} else if (data->missed_times > 3) {
		} else if (data->missed_times > 5) {
		LOG_ERR("Motor %d is not responding, setting it to offline...", i);
		data->online = false;
		}
		} else if (data->missed_times < 3) {
		} else if (data->missed_times <= 5) {
		dm_motor_control(motor_devices[i], CLEAR_ERROR);
		data->online = true;
		LOG_ERR("Motor %d is responding again, resuming...", i);
		}
		}
		dm_queue_proceed(&dm_can_tx_msgq);
		k_sleep(K_USEC(800));
		k_sleep(K_USEC(750));
		}
		}

motor/boards/robomaster_board_c.overlay

+2 −2

Original line number	Diff line number	Diff line
		@@ -18,9 +18,9 @@
		mit_pid_0: mit_pid_0 {
		#controller-cells = <0>;
		compatible = "pid,mit";
		k_p = <700>;
		k_p = <50>;
		k_i = <3>;
		k_d = <800>;
		k_d = <8>;
		};
		};
		sbus0: sbus0 {

motor/src/main.c

+21 −7

Original line number	Diff line number	Diff line
		@@ -29,6 +29,16 @@ LOG_MODULE_REGISTER(main, LOG_LEVEL_DBG);
		const struct device *can_dev = DEVICE_DT_GET(CANBUS_NODE);
		const struct device *motor1 = DEVICE_DT_GET(MOTOR1_NODE);

		/* CAN Feedback to console*/
		K_THREAD_STACK_DEFINE(feedback_stack_area, 4096); // 定义线程栈
		void console_feedback(void arg1, void arg2, void *arg3) {
		while (1) {

		LOG_INF("rpm: motor1: %.2f\n", (double)motor_get_speed(motor1));
		k_msleep(500);
		}
		}

		int main(void) {
		// motor_control(motor1, ENABLE_MOTOR);
		// motor_set_mode(motor1, MIT);
		@@ -36,15 +46,19 @@ int main(void) {
		// motor_set_speed(motor1, 10);
		motor_set_mode(motor1, MIT);
		motor_control(motor1, ENABLE_MOTOR);
		motor_set_speed(motor1, 2);

		/* Start Feedback thread*/
		struct k_thread feedback_thread_data;
		k_thread_create(&feedback_thread_data,
		feedback_stack_area, // 修改为 can_send_stack_area
		K_THREAD_STACK_SIZEOF(feedback_stack_area), console_feedback, (void *)motor1,
		NULL, NULL, 0, 0, K_MSEC(300));

		while (1) {
		// float motor1_rpm = 0;
		// while (1) {
		// motor1_rpm = motor_get_speed(motor1);
		// LOG_INF("rpm: motor1: %.2f %.2f\n", (double)motor1_rpm,
		// (double)motor_get_speed(motor1));
		// k_msleep(500);
		// }
		k_sleep(K_FOREVER);
		// LOG_INF("rpm: motor1: %.2f %.2f\n", (double)motor1_rpm, (double)motor_get_speed(motor1));
		k_msleep(500);
		// k_sleep(K_FOREVER);
		}
		}

Admin message