Added robot move commands its own message buffer

2026-10-06 13:36:36 +03:00
parent bb1e17b27a
commit 7a7e7d9c68
9 changed files with 296 additions and 32 deletions
@@ -0,0 +1,99 @@
+/*
+
+     (1)               (6)
+       \               /
+        \             /
+         \           /
+          *---(F)---*
+         /           \
+        /             \
+       /               \
+(2)---*        o        *---(5)
+       \               /
+        \             /
+         \           /
+          *---(B)---*
+         /           \
+        /             \
+       /               \
+     (3)               (4)
+
+*/
+
+#include "robot.h"
+#include "led.h"
+
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(robot, LOG_LEVEL_INF);
+
+
+// THREAD
+static struct k_thread robot_thread_data;
+static k_tid_t robot_thread_id = NULL;
+#define ROBOT_THREAD_STACK_SIZE 2048
+#define ROBOT_THREAD_PRIORITY 5
+K_THREAD_STACK_DEFINE(robot_thread_stack, ROBOT_THREAD_STACK_SIZE);
+
+// BUFFER
+#define ROBOT_MSG_BUFFER_SIZE 10
+struct command_message_t robot_msg_buffer[ROBOT_MSG_BUFFER_SIZE + 2];
+struct command_message_t *robot_msg_buffer_ptr;
+char robot_msgq_buffer[ROBOT_MSG_BUFFER_SIZE * sizeof(uint32_t)];
+struct k_msgq robot_msgq;
+
+
+static void robot_thread(void *p1, void *p2, void *p3) {
+	ARG_UNUSED(p1);
+	ARG_UNUSED(p2);
+	ARG_UNUSED(p3);
+	struct command_message_t *data;
+
+	while (1) {
+        k_msgq_get(&robot_msgq, &data, K_FOREVER);
+
+		// TODO: handle commands
+		led1_toggle();
+	}
+}
+
+int robot_init() {
+
+	// BUFFER
+	memset(robot_msg_buffer, 0, sizeof(robot_msg_buffer));
+	robot_msg_buffer_ptr = robot_msg_buffer;
+	k_msgq_init(&robot_msgq, robot_msgq_buffer, sizeof(struct command_message_t *), ROBOT_MSG_BUFFER_SIZE);
+
+	// THREAD
+	robot_thread_id = k_thread_create(
+		&robot_thread_data,
+		robot_thread_stack,
+		K_THREAD_STACK_SIZEOF(robot_thread_stack),
+		robot_thread,
+		NULL, NULL, NULL,
+		ROBOT_THREAD_PRIORITY,
+		0,
+		K_NO_WAIT
+	);
+
+	return 0;
+}
+
+struct command_message_t* robot_get_next_tx_buf() {
+	struct command_message_t *buf = robot_msg_buffer_ptr;
+
+	// Increment the buffer pointer
+	robot_msg_buffer_ptr++;
+
+	if (robot_msg_buffer_ptr > &robot_msg_buffer[ROBOT_MSG_BUFFER_SIZE-1]) {
+		robot_msg_buffer_ptr = robot_msg_buffer;
+	}
+
+	return buf;
+}
+
+int robot_notify_msg(struct command_message_t* msg) {
+	k_msgq_put(&robot_msgq, &msg, K_NO_WAIT);
+
+	return 0;
+}