diff --git a/pins.drawio b/pins.drawio index b0698d8..6126078 100644 --- a/pins.drawio +++ b/pins.drawio @@ -1,6 +1,6 @@ - + - + @@ -1049,6 +1049,9 @@ + + + @@ -1145,11 +1148,8 @@ - - - - + @@ -1193,6 +1193,15 @@ + + + + + + + + + @@ -2107,4 +2116,101 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/servo_controller/scripts/send_cmd.py b/servo_controller/scripts/send_cmd.py index 60c590b..14865e1 100644 --- a/servo_controller/scripts/send_cmd.py +++ b/servo_controller/scripts/send_cmd.py @@ -48,6 +48,9 @@ COMMANDS = { "DIGITAL_LEG_OUT": 12, "POWER_MONITOR_SET_READ": 13, "POWER_MONITOR_READ": 14, + "MOVE_COMMAND_START": 15, + "TEST": 16, + "MOVE_COMMAND_END": 17, } COMMAND_NAMES = {v: k for k, v in COMMANDS.items()} @@ -60,6 +63,7 @@ COMMANDS_DATA_OPTIONAL = { "LED_TOGGLE", "ADC_READ_RAW_ALL", "ADC_READ_ALL", + "TEST", } # struct formats for typed data tokens (all little-endian) diff --git a/servo_controller/src/command_handler.c b/servo_controller/src/command_handler.c index 88217c1..93838c7 100644 --- a/servo_controller/src/command_handler.c +++ b/servo_controller/src/command_handler.c @@ -12,20 +12,21 @@ LOG_MODULE_REGISTER(command_handler, LOG_LEVEL_INF); -// BUFFER (add ack and nack at the and as static) -#define CMD_MSG_BUFFER_SIZE 10 -struct command_message_t cmd_msg_buffer[CMD_MSG_BUFFER_SIZE + 2]; -struct command_message_t *cmd_msg_buffer_ptr; +// BUFFER +#define CMD_TX_MSG_BUFFER_SIZE 10 +struct command_message_t cmd_tx_msg_buffer[CMD_TX_MSG_BUFFER_SIZE]; +struct command_message_t *cmd_tx_msg_buffer_ptr; +#define CMD_RX_MSG_BUFFER_SIZE 10 +struct command_message_t cmd_rx_msg_buffer[CMD_RX_MSG_BUFFER_SIZE]; +struct command_message_t *cmd_rx_msg_buffer_ptr; int command_handler_init() { // BUFFER - memset(cmd_msg_buffer, 0, sizeof(cmd_msg_buffer)); - cmd_msg_buffer_ptr = cmd_msg_buffer; - - // CREATE ACK/NACK - command_create_ack(&cmd_msg_buffer[CMD_MSG_BUFFER_SIZE]); - command_create_nack(&cmd_msg_buffer[CMD_MSG_BUFFER_SIZE + 1]); + memset(cmd_tx_msg_buffer, 0, sizeof(cmd_tx_msg_buffer)); + cmd_tx_msg_buffer_ptr = cmd_tx_msg_buffer; + memset(cmd_rx_msg_buffer, 0, sizeof(cmd_rx_msg_buffer)); + cmd_rx_msg_buffer_ptr = cmd_rx_msg_buffer; return 0; } @@ -162,13 +163,26 @@ int command_handler_rx(struct command_message_t *msg) { } struct command_message_t* cmd_get_next_tx_buf() { - struct command_message_t *buf = cmd_msg_buffer_ptr; + struct command_message_t *buf = cmd_tx_msg_buffer_ptr; // Increment the buffer pointer - cmd_msg_buffer_ptr++; + cmd_tx_msg_buffer_ptr++; - if (cmd_msg_buffer_ptr > &cmd_msg_buffer[CMD_MSG_BUFFER_SIZE-1]) { - cmd_msg_buffer_ptr = cmd_msg_buffer; + if (cmd_tx_msg_buffer_ptr > &cmd_tx_msg_buffer[CMD_TX_MSG_BUFFER_SIZE-1]) { + cmd_tx_msg_buffer_ptr = cmd_tx_msg_buffer; + } + + return buf; +} + +struct command_message_t* cmd_get_next_rx_buf() { + struct command_message_t *buf = cmd_rx_msg_buffer_ptr; + + // Increment the buffer pointer + cmd_rx_msg_buffer_ptr++; + + if (cmd_rx_msg_buffer_ptr > &cmd_rx_msg_buffer[CMD_RX_MSG_BUFFER_SIZE-1]) { + cmd_rx_msg_buffer_ptr = cmd_rx_msg_buffer; } return buf; diff --git a/servo_controller/src/command_handler.h b/servo_controller/src/command_handler.h index 2f450e6..e5f076c 100644 --- a/servo_controller/src/command_handler.h +++ b/servo_controller/src/command_handler.h @@ -7,6 +7,7 @@ int command_handler_init(); int command_handler_rx(struct command_message_t *msg); struct command_message_t* cmd_get_next_tx_buf(); +struct command_message_t* cmd_get_next_rx_buf(); int command_handler_tx(struct command_message_t *msg); diff --git a/servo_controller/src/command_message.h b/servo_controller/src/command_message.h index 9016d9c..a9e4184 100644 --- a/servo_controller/src/command_message.h +++ b/servo_controller/src/command_message.h @@ -27,8 +27,12 @@ typedef enum { POWER_MONITOR_SET_READ, POWER_MONITOR_READ, - // Keep last - NUM_COMMANDS, + MOVE_COMMAND_START, + // All the commands going to robot thread go here + TEST_COMMAND, // 16 + + MOVE_COMMAND_END + } commands_e; struct command_message_t { diff --git a/servo_controller/src/main.c b/servo_controller/src/main.c index a7cb106..fcbacd3 100644 --- a/servo_controller/src/main.c +++ b/servo_controller/src/main.c @@ -6,6 +6,7 @@ #include "digital_out.h" #include "power_monitor.h" #include "command_handler.h" +#include "robot.h" #include LOG_MODULE_REGISTER(main, LOG_LEVEL_INF); @@ -71,5 +72,12 @@ int main(void) { return 0; } + // ROBOT init + ret = robot_init(); + if (ret != 0) { + LOG_ERR("Failed to enable ROBOT"); + return 0; + } + return 0; } \ No newline at end of file diff --git a/servo_controller/src/robot.c b/servo_controller/src/robot.c new file mode 100644 index 0000000..2504786 --- /dev/null +++ b/servo_controller/src/robot.c @@ -0,0 +1,99 @@ +/* + + (1) (6) + \ / + \ / + \ / + *---(F)---* + / \ + / \ + / \ +(2)---* o *---(5) + \ / + \ / + \ / + *---(B)---* + / \ + / \ + / \ + (3) (4) + +*/ + +#include "robot.h" +#include "led.h" + +#include + +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; +} \ No newline at end of file diff --git a/servo_controller/src/robot.h b/servo_controller/src/robot.h new file mode 100644 index 0000000..fc3e481 --- /dev/null +++ b/servo_controller/src/robot.h @@ -0,0 +1,13 @@ +#ifndef ROBOT_H +#define ROBOT_H + + +#include "command_message.h" + + +int robot_init(); +struct command_message_t* robot_get_next_tx_buf(); +int robot_notify_msg(struct command_message_t* msg); + + +#endif // ROBOT_H \ No newline at end of file diff --git a/servo_controller/src/usb.c b/servo_controller/src/usb.c index f1bce71..ba87628 100644 --- a/servo_controller/src/usb.c +++ b/servo_controller/src/usb.c @@ -1,6 +1,7 @@ #include "usb.h" #include "usb_conf.h" #include "command_handler.h" +#include "robot.h" #include #include @@ -90,8 +91,7 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) { ARG_UNUSED(p1); ARG_UNUSED(p2); ARG_UNUSED(p3); - struct command_message_t msg; - command_message_init(&msg); + struct command_message_t *msg; LOG_INF("USB command processing thread started"); @@ -110,17 +110,26 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) { len = ring_buf_get(&ringbuf, buf_header, (COMMAND_HEADER_SIZE - 1)); if ((len == (COMMAND_HEADER_SIZE - 1)) && (buf_header[1] == COMMAND_ID) && (buf_header[0] <= COMMAND_DATA_SIZE)) { - msg.length = buf_header[0]; - msg.command = buf_header[2]; + uint8_t is_move = 0; + if (buf_header[2] > MOVE_COMMAND_START && buf_header[2] < MOVE_COMMAND_END) { + msg = robot_get_next_tx_buf(); + is_move = 1; + } + else { + msg = cmd_get_next_rx_buf(); + } + command_message_init(msg); + msg->length = buf_header[0]; + msg->command = buf_header[2]; // Ignore the tick for now - msg.crc = buf_header[COMMAND_HEADER_SIZE - 2]; + msg->crc = buf_header[COMMAND_HEADER_SIZE - 2]; - if (msg.length) { - len = ring_buf_get(&ringbuf, msg.data, msg.length); + if (msg->length) { + len = ring_buf_get(&ringbuf, msg->data, msg->length); } - uint8_t calculated_crc = command_calculate_crc(&msg); - if (calculated_crc != msg.crc) { + uint8_t calculated_crc = command_calculate_crc(msg); + if (calculated_crc != msg->crc) { if (RETURN_ACK) { // Send NACK nack.tick = k_uptime_get(); @@ -130,7 +139,13 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) { continue; } - int ret = command_handler_rx(&msg); + int ret; + if (is_move) { + ret = robot_notify_msg(msg); + } + else { + ret = command_handler_rx(msg); + } if (ret == 0) { if (RETURN_ACK) { // Send ACK