From 7e3ece64cd5138081263413461dc6856a668bfae Mon Sep 17 00:00:00 2001 From: lonkaars Date: Tue, 24 May 2022 21:04:12 +0200 Subject: sim reading serial in from stdin working --- robot/tests/info.bin | 1 + robot/tests/ping.bin | Bin 0 -> 3 bytes robot/tests/readme.md | 11 +++++++++++ 3 files changed, 12 insertions(+) create mode 100644 robot/tests/info.bin create mode 100644 robot/tests/ping.bin create mode 100644 robot/tests/readme.md (limited to 'robot/tests') diff --git a/robot/tests/info.bin b/robot/tests/info.bin new file mode 100644 index 0000000..e1ce36c --- /dev/null +++ b/robot/tests/info.bin @@ -0,0 +1 @@ +ÿ \ No newline at end of file diff --git a/robot/tests/ping.bin b/robot/tests/ping.bin new file mode 100644 index 0000000..456804e Binary files /dev/null and b/robot/tests/ping.bin differ diff --git a/robot/tests/readme.md b/robot/tests/readme.md new file mode 100644 index 0000000..7f4cc9a --- /dev/null +++ b/robot/tests/readme.md @@ -0,0 +1,11 @@ +this is a subdirectory that contains binary example commands to send to the +simultation robot code + +to use: + +``` +./a.out < file +``` + +where `file` is one of the .bin files in this folder + -- cgit v1.2.3 From 9cd83d662849fe72c83038a018107d9bbf5c2398 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 16:30:21 +0200 Subject: WIP serial parsing --- robot/hypervisor.c | 8 ++++- robot/hypervisor.h | 4 +++ robot/readme.md | 2 ++ robot/sercomm.c | 12 +++----- robot/sim.c | 6 ++++ robot/sim.h | 3 +- robot/tests/padded_info.bin | 1 + shared/bin.c | 8 +++++ shared/bin.h | 3 ++ shared/protocol.c | 10 ++++++ shared/protocol.h | 74 +++++++++++++++++++++++++++++++++++++++++++-- shared/serial_parse.c | 54 +++++++++++++++++++++++++++++++++ shared/serial_parse.h | 9 ++++++ 13 files changed, 182 insertions(+), 12 deletions(-) create mode 100644 robot/tests/padded_info.bin create mode 100644 shared/protocol.c create mode 100644 shared/serial_parse.c create mode 100644 shared/serial_parse.h (limited to 'robot/tests') diff --git a/robot/hypervisor.c b/robot/hypervisor.c index 4d46a12..0f754a3 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -5,6 +5,8 @@ #include "orangutan_shim.h" #include "sercomm.h" +uint64_t g_w2_hypervisor_cycles = 0; + void w2_hypervisor_main() { #ifdef W2_SIM w2_sim_cycle_begin(); @@ -26,6 +28,10 @@ void w2_hypervisor_main() { #ifdef W2_SIM if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n"); - usleep(100e3); + if (!g_w2_sim_headless) usleep(100e3); + + if (g_w2_sim_headless && DBG_MAX_CYCLES > -1 && g_w2_hypervisor_cycles > DBG_MAX_CYCLES) exit(0); #endif + + g_w2_hypervisor_cycles++; } diff --git a/robot/hypervisor.h b/robot/hypervisor.h index 0e73259..35fa64c 100644 --- a/robot/hypervisor.h +++ b/robot/hypervisor.h @@ -1,5 +1,9 @@ #pragma once +#include + +extern uint64_t g_w2_hypervisor_cycles; + /** * backbone of all other modules * diff --git a/robot/readme.md b/robot/readme.md index 4a7aca3..2de14db 100644 --- a/robot/readme.md +++ b/robot/readme.md @@ -110,6 +110,8 @@ global todo: module (for last ping time measurement) - [ ] calibrate (line-detecting) light sensors in setup.c, or manually by placing the robot and pressing a button (maybe make this a seperate mode) +- [ ] create labeled timer functions like nodejs `console.time()` and + `console.timeEnd()` (use for serial read timeout constraint) ### hypervisor diff --git a/robot/sercomm.c b/robot/sercomm.c index 7072f9e..f4b7eb5 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -2,6 +2,7 @@ #include #include "../shared/bin.h" +#include "../shared/serial_parse.h" #include "orangutan_shim.h" #include "sercomm.h" @@ -27,11 +28,9 @@ void w2_sercomm_main() { g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE; } + // read and parse data while (serial_get_received_bytes() != g_w2_serial_buffer_index) { - uint8_t byte = g_w2_serial_buffer[g_w2_serial_buffer_index]; -#ifdef W2_SIM - simprintf("serial byte: %02x\n", byte); -#endif + w2_serial_parse(g_w2_serial_buffer[g_w2_serial_buffer_index]); g_w2_serial_buffer_index = (g_w2_serial_buffer_index + 1) % W2_SERIAL_READ_BUFFER_SIZE; } } @@ -43,10 +42,7 @@ void w2_sercomm_append_msg(w2_s_bin *data) { uint8_t next_index = (g_w2_sercomm_index + 1) % W2_SERCOMM_BUFFER_SIZE; g_w2_sercomm_buffer_full = next_index == g_w2_sercomm_offset; free(g_w2_sercomm_buffer[g_w2_sercomm_index]); - w2_s_bin *data_copy = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * data->bytes); - memcpy(&data_copy->data, data->data, data->bytes); - data_copy->bytes = data->bytes; - g_w2_sercomm_buffer[g_w2_sercomm_index] = data_copy; + g_w2_sercomm_buffer[g_w2_sercomm_index] = w2_bin_s_alloc(data->bytes, data->data); if (g_w2_sercomm_buffer_full) return; g_w2_sercomm_index = next_index; } diff --git a/robot/sim.c b/robot/sim.c index b061c9a..8af672e 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -52,6 +52,12 @@ void serial_set_baud_rate(unsigned int rate) { } void serial_send(char* message, unsigned int length) { + if (g_w2_sim_headless) { + for (unsigned int byte = 0; byte < length; byte++) + putc(message[byte] & 0xff, stdout); + return; + } + if (!DBG_ENABLE_PRINTFUNC) return; simprintfunc("serial_send", ", %u", length); unsigned int bytes = 0; simprintf(""); diff --git a/robot/sim.h b/robot/sim.h index 501552e..336592b 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -8,10 +8,11 @@ extern bool g_w2_sim_headless; // debug fine-tuning -#define DBG_ENABLE_PRINTFUNC (0) +#define DBG_ENABLE_PRINTFUNC (1) #define DBG_ENABLE_SIMWARN (1) #define DBG_ENABLE_SIMINFO (1) #define DBG_ENABLE_CYCLEINFO (0) +#define DBG_MAX_CYCLES (10) // debug print options #define DBG_BYTES_PER_LINE 16 diff --git a/robot/tests/padded_info.bin b/robot/tests/padded_info.bin new file mode 100644 index 0000000..2ff1d8f --- /dev/null +++ b/robot/tests/padded_info.bin @@ -0,0 +1 @@ +=ãÿ=íÿÿI89ÿ \ No newline at end of file diff --git a/shared/bin.c b/shared/bin.c index a2c91a4..fc6e14b 100644 --- a/shared/bin.c +++ b/shared/bin.c @@ -1,4 +1,5 @@ #include +#include #include "bin.h" @@ -62,3 +63,10 @@ uint16_t w2_bin_hton16(uint16_t h16) { uint32_t w2_bin_ntoh32(uint32_t n32) { return w2_bin_hton32(n32); } uint16_t w2_bin_ntoh16(uint16_t n16) { return w2_bin_hton16(n16); } + +w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data) { + w2_s_bin* temp = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * bytes); + temp->bytes = bytes; + memcpy(&temp->data, data, bytes); + return temp; +} diff --git a/shared/bin.h b/shared/bin.h index 1c9b951..d9eacd7 100644 --- a/shared/bin.h +++ b/shared/bin.h @@ -27,6 +27,9 @@ typedef struct { uint8_t data[]; } w2_s_bin; +/** allocate new w2_s_bin struct and fill with `*data` for `bytes` bytes */ +w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data); + typedef struct { enum w2_e_struct_types type; uint16_t length; diff --git a/shared/protocol.c b/shared/protocol.c new file mode 100644 index 0000000..2707725 --- /dev/null +++ b/shared/protocol.c @@ -0,0 +1,10 @@ +#include "protocol.h" + +W2_PROTOCOL_DEFINE(W2_PROTOCOL_CMD_PING_RX) + +w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data) { + w2_s_cmd_ping_rx *parsed = malloc(data->bytes); + memcpy(parsed, data->data, data->bytes); + return parsed; +} + diff --git a/shared/protocol.h b/shared/protocol.h index b400252..b0602cf 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -1,7 +1,18 @@ #pragma once -#define W2_CMDDIR_RX (0) -#define W2_CMDDIR_TX (1) +#include +#include +#include + +#include "bin.h" + +#define W2_SERIAL_START_BYTE 0xff + +#define W2_CMDDIR_RX 0 +#define W2_CMDDIR_TX 1 + +#define W2_CMD_CODE_MASK (~1) +#define W2_CMD_DIRECTION_MASK (1) enum w2_e_serial_commands { /** ping command */ @@ -33,3 +44,62 @@ enum w2_e_serial_commands { /** control leds command */ W2_CMD_CLED = 0x1a, }; + +// TODO +// array met indicies die structs opslaan met eigenschappen over de protocol bericht +// +// belangrijke eigenschappen: +// lengte!!! +// generic struct (voor parsen) +// parse functie +// dump functie +// +// +// als het kan deze allemaal met macro's op deze manier definieren: +// +#define W2_PROTOCOL_CMD(name, direction, ...) +#define W2_PROTOCOL_DEFINE(a) +#define W2_PROTOCOL_DECLARE(a) +#define W2_PROTOCOL_PROP(type, name) + +#define W2_CMDDIR_NAME_0 rx +#define W2_CMDDIR_NAME_1 tx + +#define W2_CMDDIR(dir) W2_CMDDIR_NAME_##dir + +#define W2_PROTOCOL_UINT8_T +#define W2_PROTOCOL_UINT16_T +#define W2_PROTOCOL_UINT32_T +#define W2_PROTOCOL_INT8_T +#define W2_PROTOCOL_INT16_T +#define W2_PROTOCOL_INT32_T + +#define W2_PROTOCOL_UINT8_T_TYPE uint8_t +#define W2_PROTOCOL_UINT16_T_TYPE uint16_t +#define W2_PROTOCOL_UINT32_T_TYPE uint32_t +#define W2_PROTOCOL_INT8_T_TYPE int8_t +#define W2_PROTOCOL_INT16_T_TYPE int16_t +#define W2_PROTOCOL_INT32_T_TYPE int32_t + +#define W2_PROTOCOL_UINT8_T_SIZE 1 +#define W2_PROTOCOL_UINT16_T_SIZE 2 +#define W2_PROTOCOL_UINT32_T_SIZE 4 +#define W2_PROTOCOL_INT8_T_SIZE 1 +#define W2_PROTOCOL_INT16_T_SIZE 2 +#define W2_PROTOCOL_INT32_T_SIZE 4 + +#define W2_PROTOCOL_CMD_PING_RX \ +W2_PROTOCOL_CMD(ping, W2_CMDDIR_RX, \ + W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, opcode) \ + W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, id) \ +) + +W2_PROTOCOL_DECLARE(W2_PROTOCOL_CMD_PING_RX) + +typedef struct { + uint8_t opcode; + uint8_t id; +} w2_s_cmd_ping_rx; + +w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data); + diff --git a/shared/serial_parse.c b/shared/serial_parse.c new file mode 100644 index 0000000..c8a3fae --- /dev/null +++ b/shared/serial_parse.c @@ -0,0 +1,54 @@ +#include + +#include "consts.h" +#include "serial_parse.h" +#include "bin.h" +#ifdef W2_SIM +#include "../robot/orangutan_shim.h" +#endif + +void w2_serial_handle(w2_s_bin *code) { +#ifdef W2_SIM + serial_send((char*)code->data, code->bytes); + // simprintf("yeah: %02x\n", code); +#endif +} + +void w2_serial_parse(uint8_t byte) { + static uint8_t current_message[W2_SERIAL_READ_BUFFER_SIZE] = {0}; + static uint8_t current_message_index = 0; + static uint8_t complete_message_length = 2; + + static bool attentive = false; + static bool listening = false; + + if (byte == W2_SERIAL_START_BYTE) { + attentive = !attentive; + // if (attentive && listening) { + // current_message[current_message_index++] = byte; + // } + } else { + // activate listen after non-0xff byte after 0xff + if (attentive && !listening) { + attentive = false; + listening = true; + } + } + + if (!listening) return; + current_message[current_message_index++] = byte; + + if (current_message_index == complete_message_length) { + w2_s_bin *copy = w2_bin_s_alloc(current_message_index, current_message); + w2_serial_handle(copy); + free(copy); + + memset(¤t_message, 0, W2_SERIAL_READ_BUFFER_SIZE); + current_message_index = 0; + complete_message_length = 1; + attentive = false; + listening = false; + return; + } +} + diff --git a/shared/serial_parse.h b/shared/serial_parse.h new file mode 100644 index 0000000..516bf8a --- /dev/null +++ b/shared/serial_parse.h @@ -0,0 +1,9 @@ +#pragma once + +#include + +#include "protocol.h" + +/** parse serial data byte by byte */ +void w2_serial_parse(uint8_t byte); + -- cgit v1.2.3 From e63abbdff3408c17b45c9032e7f7ba0199b43bd0 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 23:36:43 +0200 Subject: struct packing and dynamic size calculation --- robot/sercomm.c | 42 +++++++++++------------ robot/sim.c | 29 +++------------- robot/tests/mcfg.bin | Bin 0 -> 11 bytes shared/protocol.c | 90 ++++++++++++++++++++++++++------------------------ shared/protocol.h | 52 +++++++++++++++-------------- shared/serial_parse.c | 4 +-- 6 files changed, 103 insertions(+), 114 deletions(-) create mode 100644 robot/tests/mcfg.bin (limited to 'robot/tests') diff --git a/robot/sercomm.c b/robot/sercomm.c index 9e40b0e..293e3c6 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -47,8 +47,8 @@ void w2_sercomm_append_msg(w2_s_bin *data) { g_w2_sercomm_index = next_index; } -void w2_scmd_ping_rx(w2_s_bin *data) { - w2_s_cmd_ping_rx *message = malloc(w2_scmd_length(data->data, data->bytes)); +void w2_cmd_ping_rx(w2_s_bin *data) { + w2_s_cmd_ping_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); memcpy(message, data->data, data->bytes); size_t return_size = sizeof(w2_s_cmd_ping_tx); @@ -65,34 +65,34 @@ void w2_scmd_ping_rx(w2_s_bin *data) { free(return_message_bin); } -void w2_scmd_mode_rx(w2_s_bin *data) { return; } +void w2_cmd_mode_rx(w2_s_bin *data) { return; } -void w2_scmd_sped_rx(w2_s_bin *data) { return; } +void w2_cmd_sped_rx(w2_s_bin *data) { return; } -void w2_scmd_dirc_rx(w2_s_bin *data) { return; } +void w2_cmd_dirc_rx(w2_s_bin *data) { return; } -void w2_scmd_cord_rx(w2_s_bin *data) { return; } +void w2_cmd_cord_rx(w2_s_bin *data) { return; } -void w2_scmd_bomd_rx(w2_s_bin *data) { return; } +void w2_cmd_bomd_rx(w2_s_bin *data) { return; } -void w2_scmd_sres_rx(w2_s_bin *data) { return; } +void w2_cmd_sres_rx(w2_s_bin *data) { return; } -void w2_scmd_mcfg_rx(w2_s_bin *data) { return; } +void w2_cmd_mcfg_rx(w2_s_bin *data) { return; } -void w2_scmd_sens_rx(w2_s_bin *data) { return; } +void w2_cmd_sens_rx(w2_s_bin *data) { return; } -void w2_scmd_info_rx(w2_s_bin *data) { return; } +void w2_cmd_info_rx(w2_s_bin *data) { return; } -void w2_scmd_disp_rx(w2_s_bin *data) { return; } +void w2_cmd_disp_rx(w2_s_bin *data) { return; } -void w2_scmd_play_rx(w2_s_bin *data) { return; } +void w2_cmd_play_rx(w2_s_bin *data) { return; } -void w2_scmd_cled_rx(w2_s_bin *data) { return; } +void w2_cmd_cled_rx(w2_s_bin *data) { return; } -void w2_scmd_ping_tx(w2_s_bin *data) {} -void w2_scmd_expt_tx(w2_s_bin *data) {} -void w2_scmd_mode_tx(w2_s_bin *data) {} -void w2_scmd_cord_tx(w2_s_bin *data) {} -void w2_scmd_bomd_tx(w2_s_bin *data) {} -void w2_scmd_sens_tx(w2_s_bin *data) {} -void w2_scmd_info_tx(w2_s_bin *data) {} +void w2_cmd_ping_tx(w2_s_bin *data) {} +void w2_cmd_expt_tx(w2_s_bin *data) {} +void w2_cmd_mode_tx(w2_s_bin *data) {} +void w2_cmd_cord_tx(w2_s_bin *data) {} +void w2_cmd_bomd_tx(w2_s_bin *data) {} +void w2_cmd_sens_tx(w2_s_bin *data) {} +void w2_cmd_info_tx(w2_s_bin *data) {} diff --git a/robot/sim.c b/robot/sim.c index 57e0ef5..0cde6a0 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -38,7 +38,6 @@ static const char* const W2_CMD_DIRECTIONS[] = { void time_reset() { simprintfunc("time_reset", ""); clock_gettime(CLOCK_MONOTONIC, &reference_time); - return; } unsigned long get_ms() { @@ -51,27 +50,22 @@ unsigned long get_ms() { void red_led(unsigned char on) { simprintfunc("red_led", "%i", on); - return; } void green_led(unsigned char on) { simprintfunc("green_led", "%i", on); - return; } void clear() { simprintfunc("clear", ""); - return; } void play(const char* melody) { simprintfunc("play", "\"%s\"", melody); - return; } void serial_set_baud_rate(unsigned int rate) { simprintfunc("serial_set_baud_rate", "%u", rate); - return; } void serial_send(char* message, unsigned int length) { @@ -81,25 +75,16 @@ void serial_send(char* message, unsigned int length) { return; } if (!DBG_ENABLE_PRINTFUNC) return; + simprintfunc("serial_send", ", %u", length); - unsigned int bytes = 0; - simprintf(""); - for (unsigned int byte = 0; byte < length; byte++) { - if (bytes > DBG_BYTES_PER_LINE) { - bytes = 0; - printf("\n"); - simprintf(""); - } - printf("%02x ", message[byte] & 0xff); - bytes++; - } - printf("\n"); - return; + + w2_s_bin *bin = w2_bin_s_alloc(length, (uint8_t*) message); + w2_sim_print_serial(bin); + free(bin); } void serial_receive_ring(char* buffer, unsigned char size) { simprintfunc("serial_receive_ring", "0x%016lx, %u", (unsigned long) buffer, size); - return; } unsigned char serial_get_received_bytes() { @@ -118,16 +103,12 @@ void w2_sim_setup(int argc, char **argv) { term.c_cc[VTIME] = 0; term.c_cc[VMIN] = 0; tcsetattr(STDIN_FILENO, 0, &term); - - return; } void w2_sim_cycle_begin() { // read bytes from stdin while(read(STDIN_FILENO, (g_w2_serial_buffer + sizeof(char) * g_w2_serial_buffer_head), 1) > 0) g_w2_serial_buffer_head = (g_w2_serial_buffer_head + 1) % W2_SERIAL_READ_BUFFER_SIZE; - - return; } void w2_sim_print_serial(w2_s_bin *data) { diff --git a/robot/tests/mcfg.bin b/robot/tests/mcfg.bin new file mode 100644 index 0000000..a43ecf8 Binary files /dev/null and b/robot/tests/mcfg.bin differ diff --git a/shared/protocol.c b/shared/protocol.c index 18ee227..73063f3 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -5,133 +5,137 @@ #include "../robot/orangutan_shim.h" #endif -uint8_t w2_scmd_length(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { - if ((data[0] & W2_CMD_CODE_MASK) == W2_CMD_PING) return 2; +size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { + if (data[0] == (W2_CMD_PING | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_ping_rx); + if (data[0] == (W2_CMD_PING | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_ping_tx); - if (data[0] == (W2_CMD_MODE | W2_CMDDIR_RX)) return 2; - if (data[0] == (W2_CMD_MODE | W2_CMDDIR_TX)) return 2; + if (data[0] == (W2_CMD_MODE | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_mode_rx); + if (data[0] == (W2_CMD_MODE | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_mode_tx); - if (data[0] == (W2_CMD_SPED | W2_CMDDIR_RX)) return 2; + if (data[0] == (W2_CMD_SPED | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sped_rx); - if (data[0] == (W2_CMD_DIRC | W2_CMDDIR_RX)) return 5; + if (data[0] == (W2_CMD_DIRC | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_dirc_rx); - if (data[0] == (W2_CMD_CORD | W2_CMDDIR_RX)) return 6; - if (data[0] == (W2_CMD_CORD | W2_CMDDIR_TX)) return 6; + if (data[0] == (W2_CMD_CORD | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_cord_rx); + if (data[0] == (W2_CMD_CORD | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_cord_tx); - if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_RX)) return 9; - if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_TX)) return 6; + if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_bomd_rx); + if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_bomd_tx); - if (data[0] == (W2_CMD_SRES | W2_CMDDIR_RX)) return 2; + if (data[0] == (W2_CMD_SRES | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sres_rx); - if (data[0] == (W2_CMD_SENS | W2_CMDDIR_RX)) return 1; - if (data[0] == (W2_CMD_SENS | W2_CMDDIR_TX)) return 0; + if (data[0] == (W2_CMD_SENS | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sens_rx); + if (data[0] == (W2_CMD_SENS | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_sens_tx); - if (data[0] == (W2_CMD_INFO | W2_CMDDIR_RX)) return 1; - if (data[0] == (W2_CMD_INFO | W2_CMDDIR_TX)) return 41; + if (data[0] == (W2_CMD_INFO | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_info_rx); + if (data[0] == (W2_CMD_INFO | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_info_tx); w2_s_bin *copy = w2_bin_s_alloc(data_length, data); uint8_t length = 1; - if (data[0] == (W2_CMD_EXPT | W2_CMDDIR_TX)) length = w2_scmd_expt_tx_length(copy); - if (data[0] == (W2_CMD_MCFG | W2_CMDDIR_RX)) length = w2_scmd_mcfg_rx_length(copy); + if (data[0] == (W2_CMD_EXPT | W2_CMDDIR_TX)) length = w2_cmd_expt_tx_sizeof(copy); + if (data[0] == (W2_CMD_MCFG | W2_CMDDIR_RX)) length = w2_cmd_mcfg_rx_sizeof(copy); free(copy); return length; } -uint8_t w2_scmd_expt_tx_length(w2_s_bin *data) { - return 3 + (data->bytes > 2 ? (sizeof(uint8_t) * data->data[2]) : 0); +#define W2_DYN_MEMBER_SIZEOF(struct_t, length_byte, trailing_type) \ + sizeof(struct_t) + \ + (data->bytes > length_byte ? (sizeof(trailing_type) * data->data[length_byte]) : 0) + +size_t w2_cmd_expt_tx_sizeof(w2_s_bin *data) { + return W2_DYN_MEMBER_SIZEOF(w2_s_cmd_expt_tx, 2, uint8_t); } -uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data) { - // TODO 3 = sizeof(feature) -> protocol.md - return 4 + (data->bytes > 3 ? (3 * data->data[3]) : 0); +size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data) { + return W2_DYN_MEMBER_SIZEOF(w2_s_cmd_mcfg_rx, 3, w2_s_cmd_mcfg_feature); } -void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { +void w2_cmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { w2_s_bin *copy = w2_bin_s_alloc(data_length, data); bool unknown = false; switch (data[0]) { case W2_CMD_PING | W2_CMDDIR_RX: { - w2_scmd_ping_rx(copy); + w2_cmd_ping_rx(copy); break; } case W2_CMD_PING | W2_CMDDIR_TX: { - w2_scmd_ping_tx(copy); + w2_cmd_ping_tx(copy); break; } case W2_CMD_EXPT | W2_CMDDIR_TX: { - w2_scmd_expt_tx(copy); + w2_cmd_expt_tx(copy); break; } case W2_CMD_MODE | W2_CMDDIR_RX: { - w2_scmd_mode_rx(copy); + w2_cmd_mode_rx(copy); break; } case W2_CMD_MODE | W2_CMDDIR_TX: { - w2_scmd_mode_tx(copy); + w2_cmd_mode_tx(copy); break; } case W2_CMD_SPED | W2_CMDDIR_RX: { - w2_scmd_sped_rx(copy); + w2_cmd_sped_rx(copy); break; } case W2_CMD_DIRC | W2_CMDDIR_RX: { - w2_scmd_dirc_rx(copy); + w2_cmd_dirc_rx(copy); break; } case W2_CMD_CORD | W2_CMDDIR_RX: { - w2_scmd_cord_rx(copy); + w2_cmd_cord_rx(copy); break; } case W2_CMD_CORD | W2_CMDDIR_TX: { - w2_scmd_cord_tx(copy); + w2_cmd_cord_tx(copy); break; } case W2_CMD_BOMD | W2_CMDDIR_RX: { - w2_scmd_bomd_rx(copy); + w2_cmd_bomd_rx(copy); break; } case W2_CMD_BOMD | W2_CMDDIR_TX: { - w2_scmd_bomd_tx(copy); + w2_cmd_bomd_tx(copy); break; } case W2_CMD_SRES | W2_CMDDIR_RX: { - w2_scmd_sres_rx(copy); + w2_cmd_sres_rx(copy); break; } case W2_CMD_MCFG | W2_CMDDIR_RX: { - w2_scmd_mcfg_rx(copy); + w2_cmd_mcfg_rx(copy); break; } case W2_CMD_SENS | W2_CMDDIR_RX: { - w2_scmd_sens_rx(copy); + w2_cmd_sens_rx(copy); break; } case W2_CMD_SENS | W2_CMDDIR_TX: { - w2_scmd_sens_tx(copy); + w2_cmd_sens_tx(copy); break; } case W2_CMD_INFO | W2_CMDDIR_RX: { - w2_scmd_info_rx(copy); + w2_cmd_info_rx(copy); break; } case W2_CMD_INFO | W2_CMDDIR_TX: { - w2_scmd_info_tx(copy); + w2_cmd_info_tx(copy); break; } case W2_CMD_DISP | W2_CMDDIR_RX: { - w2_scmd_disp_rx(copy); + w2_cmd_disp_rx(copy); break; } case W2_CMD_PLAY | W2_CMDDIR_RX: { - w2_scmd_play_rx(copy); + w2_cmd_play_rx(copy); break; } case W2_CMD_CLED | W2_CMDDIR_RX: { - w2_scmd_cled_rx(copy); + w2_cmd_cled_rx(copy); break; } default: { diff --git a/shared/protocol.h b/shared/protocol.h index 52ba2bd..adec614 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -46,6 +46,8 @@ enum w2_e_scmds { W2_CMD_CLED = 0x1a, }; +#pragma pack(push, 1) + typedef struct { uint8_t opcode; uint8_t id; @@ -158,53 +160,55 @@ typedef struct { typedef struct { } w2_s_cmd_cled_rx; +#pragma pack(pop) + /** global handler for complete messages */ -void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); +void w2_cmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); /** calculate message length */ -uint8_t w2_scmd_length(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); +size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); /** handler for ping_rx (on complete message) */ -void w2_scmd_ping_rx(w2_s_bin *data); +void w2_cmd_ping_rx(w2_s_bin *data); /** handler for ping_tx (on complete message) */ -void w2_scmd_ping_tx(w2_s_bin *data); +void w2_cmd_ping_tx(w2_s_bin *data); /** handler for expt_tx (on complete message) */ -void w2_scmd_expt_tx(w2_s_bin *data); +void w2_cmd_expt_tx(w2_s_bin *data); /** handler for mode_rx (on complete message) */ -void w2_scmd_mode_rx(w2_s_bin *data); +void w2_cmd_mode_rx(w2_s_bin *data); /** handler for mode_tx (on complete message) */ -void w2_scmd_mode_tx(w2_s_bin *data); +void w2_cmd_mode_tx(w2_s_bin *data); /** handler for sped_rx (on complete message) */ -void w2_scmd_sped_rx(w2_s_bin *data); +void w2_cmd_sped_rx(w2_s_bin *data); /** handler for dirc_rx (on complete message) */ -void w2_scmd_dirc_rx(w2_s_bin *data); +void w2_cmd_dirc_rx(w2_s_bin *data); /** handler for cord_rx (on complete message) */ -void w2_scmd_cord_rx(w2_s_bin *data); +void w2_cmd_cord_rx(w2_s_bin *data); /** handler for cord_tx (on complete message) */ -void w2_scmd_cord_tx(w2_s_bin *data); +void w2_cmd_cord_tx(w2_s_bin *data); /** handler for bomd_rx (on complete message) */ -void w2_scmd_bomd_rx(w2_s_bin *data); +void w2_cmd_bomd_rx(w2_s_bin *data); /** handler for bomd_tx (on complete message) */ -void w2_scmd_bomd_tx(w2_s_bin *data); +void w2_cmd_bomd_tx(w2_s_bin *data); /** handler for sres_rx (on complete message) */ -void w2_scmd_sres_rx(w2_s_bin *data); +void w2_cmd_sres_rx(w2_s_bin *data); /** handler for mcfg_rx (on complete message) */ -void w2_scmd_mcfg_rx(w2_s_bin *data); +void w2_cmd_mcfg_rx(w2_s_bin *data); /** handler for sens_rx (on complete message) */ -void w2_scmd_sens_rx(w2_s_bin *data); +void w2_cmd_sens_rx(w2_s_bin *data); /** handler for sens_tx (on complete message) */ -void w2_scmd_sens_tx(w2_s_bin *data); +void w2_cmd_sens_tx(w2_s_bin *data); /** handler for info_rx (on complete message) */ -void w2_scmd_info_rx(w2_s_bin *data); +void w2_cmd_info_rx(w2_s_bin *data); /** handler for info_tx (on complete message) */ -void w2_scmd_info_tx(w2_s_bin *data); +void w2_cmd_info_tx(w2_s_bin *data); /** handler for disp_rx (on complete message) */ -void w2_scmd_disp_rx(w2_s_bin *data); +void w2_cmd_disp_rx(w2_s_bin *data); /** handler for play_rx (on complete message) */ -void w2_scmd_play_rx(w2_s_bin *data); +void w2_cmd_play_rx(w2_s_bin *data); /** handler for cled_rx (on complete message) */ -void w2_scmd_cled_rx(w2_s_bin *data); +void w2_cmd_cled_rx(w2_s_bin *data); /** calculate message length for expt_tx (incomplete message) */ -uint8_t w2_scmd_expt_tx_length(w2_s_bin *data); +size_t w2_cmd_expt_tx_sizeof(w2_s_bin *data); /** calculate message length for mcfg_rx (incomplete message) */ -uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data); +size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data); diff --git a/shared/serial_parse.c b/shared/serial_parse.c index 0ebe377..3bf9c14 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -27,10 +27,10 @@ void w2_serial_parse(uint8_t byte) { if (!listening) return; current_message[current_message_index++] = byte; - complete_message_length = w2_scmd_length(current_message, current_message_index); + complete_message_length = w2_cmd_sizeof(current_message, current_message_index); if (current_message_index == complete_message_length) { - w2_scmd_handler(current_message, current_message_index); + w2_cmd_handler(current_message, current_message_index); memset(¤t_message, 0, W2_SERIAL_READ_BUFFER_SIZE); current_message_index = 0; -- cgit v1.2.3 From 54b69efe150e1a102faafb4e214159c92abbb841 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 12:14:04 +0200 Subject: clean up ugly switch case in favor of array lookup --- robot/setup.c | 1 + robot/tests/undefined.bin | 1 + shared/protocol.c | 125 +++++++++++++--------------------------------- shared/protocol.h | 6 +++ shared/serial_parse.c | 1 + 5 files changed, 43 insertions(+), 91 deletions(-) create mode 100644 robot/tests/undefined.bin (limited to 'robot/tests') diff --git a/robot/setup.c b/robot/setup.c index 944ea68..f8468bf 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -25,6 +25,7 @@ void w2_setup_main() { clear(); // start serial i/o + w2_cmd_setup_handlers(); serial_set_baud_rate(W2_SERIAL_BAUD); serial_receive_ring(g_w2_serial_buffer, W2_SERIAL_READ_BUFFER_SIZE); diff --git a/robot/tests/undefined.bin b/robot/tests/undefined.bin new file mode 100644 index 0000000..d44f518 --- /dev/null +++ b/robot/tests/undefined.bin @@ -0,0 +1 @@ +ÿ€9 4­ \ No newline at end of file diff --git a/shared/protocol.c b/shared/protocol.c index 73063f3..77ec466 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -5,6 +5,30 @@ #include "../robot/orangutan_shim.h" #endif +void (*g_w2_cmd_handlers[W2_CMD_COUNT])(w2_s_bin *) = {0}; +void w2_cmd_setup_handlers() { + g_w2_cmd_handlers[W2_CMD_PING | W2_CMDDIR_RX] = w2_cmd_ping_rx; + g_w2_cmd_handlers[W2_CMD_PING | W2_CMDDIR_TX] = w2_cmd_ping_tx; + g_w2_cmd_handlers[W2_CMD_EXPT | W2_CMDDIR_TX] = w2_cmd_expt_tx; + g_w2_cmd_handlers[W2_CMD_MODE | W2_CMDDIR_RX] = w2_cmd_mode_rx; + g_w2_cmd_handlers[W2_CMD_MODE | W2_CMDDIR_TX] = w2_cmd_mode_tx; + g_w2_cmd_handlers[W2_CMD_SPED | W2_CMDDIR_RX] = w2_cmd_sped_rx; + g_w2_cmd_handlers[W2_CMD_DIRC | W2_CMDDIR_RX] = w2_cmd_dirc_rx; + g_w2_cmd_handlers[W2_CMD_CORD | W2_CMDDIR_RX] = w2_cmd_cord_rx; + g_w2_cmd_handlers[W2_CMD_CORD | W2_CMDDIR_TX] = w2_cmd_cord_tx; + g_w2_cmd_handlers[W2_CMD_BOMD | W2_CMDDIR_RX] = w2_cmd_bomd_rx; + g_w2_cmd_handlers[W2_CMD_BOMD | W2_CMDDIR_TX] = w2_cmd_bomd_tx; + g_w2_cmd_handlers[W2_CMD_SRES | W2_CMDDIR_RX] = w2_cmd_sres_rx; + g_w2_cmd_handlers[W2_CMD_MCFG | W2_CMDDIR_RX] = w2_cmd_mcfg_rx; + g_w2_cmd_handlers[W2_CMD_SENS | W2_CMDDIR_RX] = w2_cmd_sens_rx; + g_w2_cmd_handlers[W2_CMD_SENS | W2_CMDDIR_TX] = w2_cmd_sens_tx; + g_w2_cmd_handlers[W2_CMD_INFO | W2_CMDDIR_RX] = w2_cmd_info_rx; + g_w2_cmd_handlers[W2_CMD_INFO | W2_CMDDIR_TX] = w2_cmd_info_tx; + g_w2_cmd_handlers[W2_CMD_DISP | W2_CMDDIR_RX] = w2_cmd_disp_rx; + g_w2_cmd_handlers[W2_CMD_PLAY | W2_CMDDIR_RX] = w2_cmd_play_rx; + g_w2_cmd_handlers[W2_CMD_CLED | W2_CMDDIR_RX] = w2_cmd_cled_rx; +} + size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { if (data[0] == (W2_CMD_PING | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_ping_rx); if (data[0] == (W2_CMD_PING | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_ping_tx); @@ -54,101 +78,20 @@ size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data) { } void w2_cmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { - w2_s_bin *copy = w2_bin_s_alloc(data_length, data); - bool unknown = false; - - switch (data[0]) { - case W2_CMD_PING | W2_CMDDIR_RX: { - w2_cmd_ping_rx(copy); - break; - } - case W2_CMD_PING | W2_CMDDIR_TX: { - w2_cmd_ping_tx(copy); - break; - } - case W2_CMD_EXPT | W2_CMDDIR_TX: { - w2_cmd_expt_tx(copy); - break; - } - case W2_CMD_MODE | W2_CMDDIR_RX: { - w2_cmd_mode_rx(copy); - break; - } - case W2_CMD_MODE | W2_CMDDIR_TX: { - w2_cmd_mode_tx(copy); - break; - } - case W2_CMD_SPED | W2_CMDDIR_RX: { - w2_cmd_sped_rx(copy); - break; - } - case W2_CMD_DIRC | W2_CMDDIR_RX: { - w2_cmd_dirc_rx(copy); - break; - } - case W2_CMD_CORD | W2_CMDDIR_RX: { - w2_cmd_cord_rx(copy); - break; - } - case W2_CMD_CORD | W2_CMDDIR_TX: { - w2_cmd_cord_tx(copy); - break; - } - case W2_CMD_BOMD | W2_CMDDIR_RX: { - w2_cmd_bomd_rx(copy); - break; - } - case W2_CMD_BOMD | W2_CMDDIR_TX: { - w2_cmd_bomd_tx(copy); - break; - } - case W2_CMD_SRES | W2_CMDDIR_RX: { - w2_cmd_sres_rx(copy); - break; - } - case W2_CMD_MCFG | W2_CMDDIR_RX: { - w2_cmd_mcfg_rx(copy); - break; - } - case W2_CMD_SENS | W2_CMDDIR_RX: { - w2_cmd_sens_rx(copy); - break; - } - case W2_CMD_SENS | W2_CMDDIR_TX: { - w2_cmd_sens_tx(copy); - break; - } - case W2_CMD_INFO | W2_CMDDIR_RX: { - w2_cmd_info_rx(copy); - break; - } - case W2_CMD_INFO | W2_CMDDIR_TX: { - w2_cmd_info_tx(copy); - break; - } - case W2_CMD_DISP | W2_CMDDIR_RX: { - w2_cmd_disp_rx(copy); - break; - } - case W2_CMD_PLAY | W2_CMDDIR_RX: { - w2_cmd_play_rx(copy); - break; - } - case W2_CMD_CLED | W2_CMDDIR_RX: { - w2_cmd_cled_rx(copy); - break; - } - default: { + w2_s_bin *copy = w2_bin_s_alloc(data_length, data); + void (*handler)(w2_s_bin *) = g_w2_cmd_handlers[data[0]]; + + if (handler == NULL) { #ifdef W2_SIM - simwarn("unknown serial message with code 0x%02x\n", data[0]); + // TODO throw warning + simwarn("unknown serial message with code 0x%02x\n", data[0]); #endif - unknown = true; - } - } - + } else { #ifdef W2_SIM - if (!unknown) w2_sim_print_serial(copy); + w2_sim_print_serial(copy); #endif + handler(copy); + } free(copy); } diff --git a/shared/protocol.h b/shared/protocol.h index adec614..5bdfcbe 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -15,6 +15,7 @@ #define W2_CMD_CODE_MASK (~1) #define W2_CMD_DIRECTION_MASK (1) +#define W2_CMD_COUNT 28 enum w2_e_scmds { /** ping command */ W2_CMD_PING = 0x00, @@ -162,6 +163,11 @@ typedef struct { #pragma pack(pop) +/** stores message handlers in array with opcode as index */ +extern void (*g_w2_cmd_handlers[W2_CMD_COUNT])(w2_s_bin *); +/** fills g_w2_cmd_handlers with functions */ +void w2_cmd_setup_handlers(); + /** global handler for complete messages */ void w2_cmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); /** calculate message length */ diff --git a/shared/serial_parse.c b/shared/serial_parse.c index 3bf9c14..d755dc8 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -3,6 +3,7 @@ #include "consts.h" #include "serial_parse.h" +// TODO: give this function last time of byte, and measure if >5ms, throw warning void w2_serial_parse(uint8_t byte) { static uint8_t current_message[W2_SERIAL_READ_BUFFER_SIZE] = {0}; static uint8_t current_message_index = 0; -- cgit v1.2.3 From d70f4f44f927281d6c9bfff64264bd754d682dc8 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 14:05:53 +0200 Subject: implement mode command --- protocol.md | 13 +++++++++++-- robot/calibration.c | 1 - robot/calibration.h | 9 --------- robot/errcatch.c | 3 +-- robot/grid.c | 1 - robot/grid.h | 8 -------- robot/halt.c | 5 ----- robot/halt.h | 8 -------- robot/maze.c | 1 - robot/maze.h | 8 -------- robot/mode_grid.c | 3 +++ robot/mode_grid.h | 8 ++++++++ robot/mode_halt.c | 3 +++ robot/mode_halt.h | 8 ++++++++ robot/mode_lcal.c | 1 + robot/mode_lcal.h | 9 +++++++++ robot/mode_maze.c | 3 +++ robot/mode_maze.h | 8 ++++++++ robot/modes.c | 20 +++++++++++++++++++- robot/modes.h | 30 ++++++++++++++++++++++++++++++ robot/readme.md | 2 +- robot/sercomm.c | 8 +++++++- robot/setup.c | 1 - robot/tests/mode.bin | 1 + 24 files changed, 113 insertions(+), 49 deletions(-) delete mode 100644 robot/calibration.c delete mode 100644 robot/calibration.h delete mode 100644 robot/grid.c delete mode 100644 robot/grid.h delete mode 100644 robot/halt.c delete mode 100644 robot/halt.h delete mode 100644 robot/maze.c delete mode 100644 robot/maze.h create mode 100644 robot/mode_grid.c create mode 100644 robot/mode_grid.h create mode 100644 robot/mode_halt.c create mode 100644 robot/mode_halt.h create mode 100644 robot/mode_lcal.c create mode 100644 robot/mode_lcal.h create mode 100644 robot/mode_maze.c create mode 100644 robot/mode_maze.h create mode 100644 robot/tests/mode.bin (limited to 'robot/tests') diff --git a/protocol.md b/protocol.md index 7084d14..345fe50 100644 --- a/protocol.md +++ b/protocol.md @@ -34,7 +34,7 @@ readability. |--:|---|:-:|:-:|---| |`0x00`|[PING](#ping)|yes|`r <=> c`|ping |`0x02`|[EXPT](#expt)|yes|`r --> c`|exception -|`0x04`|[MODE](#mode)|no|`r <=> c`|mode +|`0x04`|[MODE](#mode)|yes|`r <=> c`|mode |`0x06`|[SPED](#sped)|no|`r <-- c`|speed |`0x08`|[DIRC](#dirc)|no|`r <-- c`|direct control |`0x0a`|[CORD](#cord)|no|`r <=> c`|coordinate @@ -96,7 +96,16 @@ message, and can be 0 in case of no message. |`uint8_t`|mode code| when initiated from the client, the **mode** command forces the robot to change -execution mode. the mode codes are undetermined as of now. +execution mode. **mode** can be one of: + +- 0: mode_maze +- 1: mode_grid +- 2: mode_halt +- 3: mode_lcal +- 4: mode_chrg +- 5: mode_dirc +- 6: mode_spin +- 7: mode_scal #### get mode (`r --> c`) (2 bytes) diff --git a/robot/calibration.c b/robot/calibration.c deleted file mode 100644 index 7788532..0000000 --- a/robot/calibration.c +++ /dev/null @@ -1 +0,0 @@ -#include "calibration.h" diff --git a/robot/calibration.h b/robot/calibration.h deleted file mode 100644 index 5c1af38..0000000 --- a/robot/calibration.h +++ /dev/null @@ -1,9 +0,0 @@ -#pragma once - -/** - * calibration mode - * - * turns robot on its own axis 360 degress, and aligns the front sensors with - * the line if found, else triggers halt mode (emergency) - */ -void w2_mode_calb(); diff --git a/robot/errcatch.c b/robot/errcatch.c index 99f1063..cf06f5d 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -2,7 +2,6 @@ #include #include "errcatch.h" -#include "halt.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" @@ -52,7 +51,7 @@ void w2_errcatch_handle_error(w2_s_error *error) { uint8_t severity = error->code & W2_E_TYPE_MASK; // trigger emergency mode for critical errors - if ((severity ^ W2_E_TYPE_CRIT) == 0) g_w2_current_mode = &w2_mode_halt; + if ((severity ^ W2_E_TYPE_CRIT) == 0) w2_modes_switch(W2_M_HALT); // TODO: handle more error types switch (error->code) { diff --git a/robot/grid.c b/robot/grid.c deleted file mode 100644 index 0c83272..0000000 --- a/robot/grid.c +++ /dev/null @@ -1 +0,0 @@ -#include "grid.h" diff --git a/robot/grid.h b/robot/grid.h deleted file mode 100644 index fcf9100..0000000 --- a/robot/grid.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -/** - * warehouse mode - * - * processes orders from the order buffer - */ -void w2_mode_grid(); diff --git a/robot/halt.c b/robot/halt.c deleted file mode 100644 index 2f159f0..0000000 --- a/robot/halt.c +++ /dev/null @@ -1,5 +0,0 @@ -#include - -#include "halt.h" - -void w2_mode_halt() { return; } diff --git a/robot/halt.h b/robot/halt.h deleted file mode 100644 index d92905e..0000000 --- a/robot/halt.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -/** - * halt (emergency) mode - * - * stops all execution until emergency status is manually cleared by the user - */ -void w2_mode_halt(); diff --git a/robot/maze.c b/robot/maze.c deleted file mode 100644 index a27414f..0000000 --- a/robot/maze.c +++ /dev/null @@ -1 +0,0 @@ -#include "maze.h" diff --git a/robot/maze.h b/robot/maze.h deleted file mode 100644 index 9fbeb8c..0000000 --- a/robot/maze.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -/** - * maze mode - * - * finds route out of maze - */ -void w2_mode_maze(); diff --git a/robot/mode_grid.c b/robot/mode_grid.c new file mode 100644 index 0000000..8526499 --- /dev/null +++ b/robot/mode_grid.c @@ -0,0 +1,3 @@ +#include "mode_grid.h" + +void w2_mode_grid() {} diff --git a/robot/mode_grid.h b/robot/mode_grid.h new file mode 100644 index 0000000..fcf9100 --- /dev/null +++ b/robot/mode_grid.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * warehouse mode + * + * processes orders from the order buffer + */ +void w2_mode_grid(); diff --git a/robot/mode_halt.c b/robot/mode_halt.c new file mode 100644 index 0000000..88d6183 --- /dev/null +++ b/robot/mode_halt.c @@ -0,0 +1,3 @@ +#include "mode_halt.h" + +void w2_mode_halt() { return; } diff --git a/robot/mode_halt.h b/robot/mode_halt.h new file mode 100644 index 0000000..d92905e --- /dev/null +++ b/robot/mode_halt.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * halt (emergency) mode + * + * stops all execution until emergency status is manually cleared by the user + */ +void w2_mode_halt(); diff --git a/robot/mode_lcal.c b/robot/mode_lcal.c new file mode 100644 index 0000000..6b4e736 --- /dev/null +++ b/robot/mode_lcal.c @@ -0,0 +1 @@ +#include "mode_lcal.h" diff --git a/robot/mode_lcal.h b/robot/mode_lcal.h new file mode 100644 index 0000000..dd373f0 --- /dev/null +++ b/robot/mode_lcal.h @@ -0,0 +1,9 @@ +#pragma once + +/** + * calibration mode + * + * turns robot on its own axis 360 degress, and aligns the front sensors with + * the line if found, else triggers halt mode (emergency) + */ +void w2_mode_lcal(); diff --git a/robot/mode_maze.c b/robot/mode_maze.c new file mode 100644 index 0000000..6ae62be --- /dev/null +++ b/robot/mode_maze.c @@ -0,0 +1,3 @@ +#include "mode_maze.h" + +void w2_mode_maze() {} diff --git a/robot/mode_maze.h b/robot/mode_maze.h new file mode 100644 index 0000000..9fbeb8c --- /dev/null +++ b/robot/mode_maze.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * maze mode + * + * finds route out of maze + */ +void w2_mode_maze(); diff --git a/robot/modes.c b/robot/modes.c index c22875c..1f36703 100644 --- a/robot/modes.c +++ b/robot/modes.c @@ -1,6 +1,24 @@ #include "modes.h" -#include "halt.h" +#include "sercomm.h" void (*g_w2_current_mode)() = &w2_mode_halt; void w2_modes_main() { (*g_w2_current_mode)(); } + +void w2_modes_switch(w2_e_mode new_mode) { + if (new_mode == W2_M_PREV) { + // TODO implement previous mode buffer + } else { + g_w2_current_mode = W2_MODES[new_mode]; + } + + // forward mode change to sercomm + size_t msg_size = sizeof(w2_s_cmd_mode_tx); + w2_s_cmd_mode_tx *msg = malloc(msg_size); + msg->opcode = W2_CMD_MODE | W2_CMDDIR_TX; + msg->mode = (uint8_t)new_mode; + w2_s_bin *msg_bin = w2_bin_s_alloc(msg_size, (uint8_t *)msg); + w2_sercomm_append_msg(msg_bin); + free(msg); + free(msg_bin); +} diff --git a/robot/modes.h b/robot/modes.h index dd34690..fe81043 100644 --- a/robot/modes.h +++ b/robot/modes.h @@ -1,5 +1,11 @@ #pragma once +#include "mode_grid.h" +#include "mode_halt.h" +#include "mode_lcal.h" +#include "mode_maze.h" + +/** function pointer to current mode */ extern void (*g_w2_current_mode)(); /** @@ -8,3 +14,27 @@ extern void (*g_w2_current_mode)(); * executes mode in g_w2_current_mode */ void w2_modes_main(); + +/** mode constants */ +typedef enum { + W2_M_PREV = -1, + W2_M_MAZE = 0, + W2_M_GRID = 1, + W2_M_HALT = 2, + W2_M_LCAL = 3, + W2_M_CHRG = 4, + W2_M_DIRC = 5, + W2_M_SPIN = 6, + W2_M_SCAL = 7, +} w2_e_mode; + +/** array that maps w2_e_mode to mode function pointers */ +static const void(*const W2_MODES[]) = { + &w2_mode_maze, + &w2_mode_grid, + &w2_mode_grid, + &w2_mode_halt, +}; + +/** switch the current mode */ +void w2_modes_switch(w2_e_mode new_mode); diff --git a/robot/readme.md b/robot/readme.md index 2de14db..31acafc 100644 --- a/robot/readme.md +++ b/robot/readme.md @@ -62,7 +62,7 @@ what they're supposed to do: |maze |`mode_maze `|Jorn & Abdullaahi| controls robot during maze portion of map; hands off control to warehouse module| |warehouse |`mode_grid `|Loek| controls robot during warehouse portion of map; hands off control to maze module| |emergency stop |`mode_halt `|Fiona| stops all execution until emergency mode is reset by software or user| -|calibration |`mode_calb `|Fiona| find line by turning on own axis if lost| +|calibration |`mode_lcal `|Fiona| find line by turning on own axis if lost| ## some standards diff --git a/robot/sercomm.c b/robot/sercomm.c index a3ccdb0..e972f28 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -4,6 +4,7 @@ #include "../shared/bin.h" #include "../shared/serial_parse.h" #include "hypervisor.h" +#include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" @@ -66,7 +67,12 @@ void w2_cmd_ping_rx(w2_s_bin *data) { free(return_message_bin); } -void w2_cmd_mode_rx(w2_s_bin *data) { return; } +void w2_cmd_mode_rx(w2_s_bin *data) { + w2_s_cmd_mode_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); + memcpy(message, data->data, data->bytes); + + w2_modes_switch(message->mode); +} void w2_cmd_sped_rx(w2_s_bin *data) { return; } diff --git a/robot/setup.c b/robot/setup.c index d075c41..1ecda43 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -2,7 +2,6 @@ #include "../shared/bin.h" #include "../shared/consts.h" -#include "halt.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" diff --git a/robot/tests/mode.bin b/robot/tests/mode.bin new file mode 100644 index 0000000..35ebd35 --- /dev/null +++ b/robot/tests/mode.bin @@ -0,0 +1 @@ +ÿ \ No newline at end of file -- cgit v1.2.3 From f073c9d3848dab915bed4844e9d13684aa5e23eb Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 15:34:58 +0200 Subject: implement direct control --- protocol.md | 2 +- robot/mode_dirc.c | 6 +++++- robot/mode_dirc.h | 5 +++++ robot/sercomm.c | 9 ++++++++- robot/sim.c | 3 +++ robot/sim.h | 4 +++- robot/tests/dirc.bin | Bin 0 -> 6 bytes robot/tests/mode.bin | 2 +- 8 files changed, 26 insertions(+), 5 deletions(-) create mode 100644 robot/tests/dirc.bin (limited to 'robot/tests') diff --git a/protocol.md b/protocol.md index 345fe50..c6cb3fb 100644 --- a/protocol.md +++ b/protocol.md @@ -36,7 +36,7 @@ readability. |`0x02`|[EXPT](#expt)|yes|`r --> c`|exception |`0x04`|[MODE](#mode)|yes|`r <=> c`|mode |`0x06`|[SPED](#sped)|no|`r <-- c`|speed -|`0x08`|[DIRC](#dirc)|no|`r <-- c`|direct control +|`0x08`|[DIRC](#dirc)|yes|`r <-- c`|direct control |`0x0a`|[CORD](#cord)|no|`r <=> c`|coordinate |`0x0c`|[BOMD](#bomd)|no|`r <=> c`|backorder modify |`0x0e`|[SRES](#sres)|no|`r <-- c`|soft reset diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c index 6c5d2bb..0bbf3cb 100644 --- a/robot/mode_dirc.c +++ b/robot/mode_dirc.c @@ -1,3 +1,7 @@ #include "mode_dirc.h" +#include "orangutan_shim.h" -void w2_mode_dirc() {} +int16_t g_w2_mode_dirc_motor_l = 0; +int16_t g_w2_mode_dirc_motor_r = 0; + +void w2_mode_dirc() { set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r); } diff --git a/robot/mode_dirc.h b/robot/mode_dirc.h index 25a664a..5b9bbf4 100644 --- a/robot/mode_dirc.h +++ b/robot/mode_dirc.h @@ -1,5 +1,10 @@ #pragma once +#include + +extern int16_t g_w2_mode_dirc_motor_l; +extern int16_t g_w2_mode_dirc_motor_r; + /** * direct control mode * diff --git a/robot/sercomm.c b/robot/sercomm.c index c9c6194..2786b85 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -4,6 +4,7 @@ #include "../shared/bin.h" #include "../shared/serial_parse.h" #include "hypervisor.h" +#include "mode_dirc.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" @@ -76,7 +77,13 @@ void w2_cmd_mode_rx(w2_s_bin *data) { void w2_cmd_sped_rx(w2_s_bin *data) { return; } -void w2_cmd_dirc_rx(w2_s_bin *data) { return; } +void w2_cmd_dirc_rx(w2_s_bin *data) { + w2_s_cmd_dirc_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); + memcpy(message, data->data, data->bytes); + + g_w2_mode_dirc_motor_l = w2_bin_ntoh16(message->left); + g_w2_mode_dirc_motor_r = w2_bin_ntoh16(message->right); +} void w2_cmd_cord_rx(w2_s_bin *data) { return; } diff --git a/robot/sim.c b/robot/sim.c index baf8a8a..ddc208a 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -123,3 +123,6 @@ void w2_sim_print_serial(w2_s_bin *data) { printf("\n"); } +void set_motors(int left, int right) { + simprintfunc("set_motors", "%i, %i", left, right); +} diff --git a/robot/sim.h b/robot/sim.h index 25cc713..a595042 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -10,7 +10,7 @@ extern bool g_w2_sim_headless; // debug fine-tuning -#define DBG_ENABLE_PRINTFUNC (0) +#define DBG_ENABLE_PRINTFUNC (1) #define DBG_ENABLE_SIMWARN (1) #define DBG_ENABLE_SIMINFO (1) #define DBG_ENABLE_CYCLEINFO (0) @@ -54,6 +54,8 @@ void serial_set_baud_rate(unsigned int rate); // NOLINT void serial_send(char *message, unsigned int length); // NOLINT void serial_receive_ring(char *buffer, unsigned char size); // NOLINT unsigned char serial_get_received_bytes(); // NOLINT +void set_motors(int left, int right); // NOLINT + void w2_sim_setup(int argc, char **argv); void w2_sim_cycle_begin(); void w2_sim_print_serial(w2_s_bin *data); diff --git a/robot/tests/dirc.bin b/robot/tests/dirc.bin new file mode 100644 index 0000000..1aea35c Binary files /dev/null and b/robot/tests/dirc.bin differ diff --git a/robot/tests/mode.bin b/robot/tests/mode.bin index 35ebd35..9cd9175 100644 --- a/robot/tests/mode.bin +++ b/robot/tests/mode.bin @@ -1 +1 @@ -ÿ \ No newline at end of file +ÿ \ No newline at end of file -- cgit v1.2.3