diff options
Diffstat (limited to 'robot')
| -rw-r--r-- | robot/- | 0 | ||||
| -rw-r--r-- | robot/errcatch.c | 65 | ||||
| -rw-r--r-- | robot/errcatch.h | 34 | ||||
| -rw-r--r-- | robot/hypervisor.c | 15 | ||||
| -rw-r--r-- | robot/hypervisor.h | 12 | ||||
| -rw-r--r-- | robot/main.c | 4 | ||||
| -rw-r--r-- | robot/makefile | 4 | ||||
| -rw-r--r-- | robot/mode_dirc.c | 19 | ||||
| -rw-r--r-- | robot/mode_lcal.c | 2 | ||||
| -rw-r--r-- | robot/modes.c | 44 | ||||
| -rw-r--r-- | robot/modes.h | 35 | ||||
| -rw-r--r-- | robot/readme.md | 9 | ||||
| -rw-r--r-- | robot/sercomm.c | 163 | ||||
| -rw-r--r-- | robot/setup.c | 3 | ||||
| -rw-r--r-- | robot/sim.c | 32 | ||||
| -rw-r--r-- | robot/sim.h | 27 |
16 files changed, 212 insertions, 256 deletions
diff --git a/robot/- b/robot/- deleted file mode 100644 index e69de29..0000000 --- a/robot/- +++ /dev/null diff --git a/robot/errcatch.c b/robot/errcatch.c index 8df90b8..17c96fa 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -1,52 +1,10 @@ -#include <stdlib.h> #include <string.h> -#include "errcatch.h" +#include "../shared/errcatch.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" -w2_s_error *g_w2_error_buffer[W2_ERROR_BUFFER_SIZE] = {}; -uint8_t g_w2_error_index = 0; -uint8_t g_w2_error_offset = 0; -uint8_t g_w2_error_buffer_full = 0; -uint8_t g_w2_error_uncaught = 0; - -void w2_errcatch_main() { - while (g_w2_error_offset != g_w2_error_index) { - w2_s_error *error = g_w2_error_buffer[g_w2_error_offset]; - w2_errcatch_handle_error(error); - g_w2_error_offset = (g_w2_error_offset + 1) % W2_ERROR_BUFFER_SIZE; - } - if (g_w2_error_buffer_full) { - w2_errcatch_throw(W2_E_WARN_ERR_BUFFER_FULL); - g_w2_error_buffer_full = 0; - } - if (g_w2_error_uncaught) { - w2_errcatch_throw(W2_E_WARN_UNCAUGHT_ERROR); - g_w2_error_uncaught = 0; - } -} - -w2_s_error *w2_alloc_error(w2_e_errorcode code, uint16_t length, const char *message) { - w2_s_error *error = malloc(sizeof(w2_s_error) + length); - memcpy(error, &(w2_s_error const){.code = code, .message_length = length}, sizeof(w2_s_error)); - strncpy(error->message, message, length); - - return error; -} - -void w2_errcatch_throw(w2_e_errorcode code) { w2_errcatch_throw_msg(code, 0, ""); } -void w2_errcatch_throw_msg(w2_e_errorcode code, uint16_t length, const char *message) { - uint8_t next_index = (g_w2_error_index + 1) % W2_ERROR_BUFFER_SIZE; - g_w2_error_buffer_full = next_index == g_w2_error_offset; - free(g_w2_error_buffer[g_w2_error_index]); - w2_s_error *error = w2_alloc_error(code, length, message); - g_w2_error_buffer[g_w2_error_index] = error; - if (g_w2_error_buffer_full) return; - g_w2_error_index = next_index; -} - void w2_errcatch_handle_error(w2_s_error *error) { uint8_t severity = error->code & W2_E_TYPE_MASK; @@ -59,23 +17,26 @@ void w2_errcatch_handle_error(w2_s_error *error) { break; } default: { - g_w2_error_uncaught = 1; + g_w2_error_uncaught = true; #ifdef W2_SIM - simwarn("Uncaught/unhandled error found with code 0x%02x\n", error->code); + simwarn("Uncaught/unhandled error found with code 0x%02x", error->code); + if (error->message_length > 0) + fprintf(stderr, " and message \"%.*s\"", error->message_length, error->message); + fprintf(stderr, "\n"); #endif } } // forward error to sercomm - size_t msg_size = sizeof(w2_s_cmd_expt_tx) + sizeof(uint8_t) * error->message_length; - w2_s_cmd_expt_tx *msg = malloc(msg_size); - msg->opcode = W2_CMD_EXPT | W2_CMDDIR_TX; - msg->error = error->code; - msg->length = error->message_length; + W2_CREATE_MSG_SIZE_BIN(w2_s_cmd_expt_tx, + sizeof(w2_s_cmd_expt_tx) + sizeof(uint8_t) * error->message_length, msg, + msg_bin); + msg->opcode = W2_CMD_EXPT | W2_CMDDIR_TX; + msg->error = error->code; + msg->length = error->message_length; memcpy(msg->message, error->message, error->message_length); - 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); return; diff --git a/robot/errcatch.h b/robot/errcatch.h deleted file mode 100644 index add4ece..0000000 --- a/robot/errcatch.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -/** @file errcatch.h */ - -#include <stdint.h> - -#include "../shared/consts.h" -#include "../shared/errors.h" - -/** error ring buffer */ -extern w2_s_error *g_w2_error_buffer[W2_ERROR_BUFFER_SIZE]; -/** stores head of ring buffer */ -extern uint8_t g_w2_error_index; -/** stores start of ring buffer */ -extern uint8_t g_w2_error_offset; - -/** error-handler module main */ -void w2_errcatch_main(); - -/** handle error */ -void w2_errcatch_handle_error(w2_s_error *error); - -/** append error to error buffer */ -void w2_errcatch_throw(w2_e_errorcode code); - -/** append error to error buffer (with debug message) */ -void w2_errcatch_throw_msg(w2_e_errorcode code, uint16_t length, const char *message); - -/** - * allocate and initialize error struct - * - * TODO: doesn't handle null pointers from malloc - */ -w2_s_error *w2_alloc_error(w2_e_errorcode code, uint16_t length, const char *message); diff --git a/robot/hypervisor.c b/robot/hypervisor.c index 1fd3ac2..2a6120b 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -1,6 +1,6 @@ #include "hypervisor.h" +#include "../shared/errcatch.h" #include "../shared/util.h" -#include "errcatch.h" #include "io.h" #include "modes.h" #include "orangutan_shim.h" @@ -14,6 +14,12 @@ unsigned long g_w2_hypervisor_ema_io_ms = 0; unsigned long g_w2_hypervisor_ema_mode_ms = 0; uint64_t g_w2_hypervisor_timers[W2_HYPERVISOR_TIMER_COUNT] = {0}; +unsigned int g_w2_ping_ms = 0; +uint8_t g_w2_ping_id = 0; +bool g_w2_ping_received = true; +bool g_w2_ping_timeout = false; +bool g_w2_connected = false; + void w2_hypervisor_main() { #ifdef W2_SIM w2_sim_cycle_begin(); @@ -27,7 +33,7 @@ void w2_hypervisor_main() { unsigned long sercomm_time = get_ms(); w2_errcatch_main(); unsigned long errcatch_time = get_ms() - sercomm_time; - w2_io_main(); + // w2_io_main(); unsigned long io_time = get_ms() - errcatch_time; w2_modes_main(); unsigned long mode_time = get_ms() - io_time; @@ -44,10 +50,9 @@ void w2_hypervisor_main() { #ifdef W2_SIM if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n"); - if (!g_w2_sim_headless) usleep(100e3); + if (DBG_CYCLE_DELAY > 0) usleep(DBG_CYCLE_DELAY); - if (g_w2_sim_headless && DBG_MAX_CYCLES > -1 && g_w2_hypervisor_cycles > DBG_MAX_CYCLES) - exit(0); + if (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 589d324..4b1ed9b 100644 --- a/robot/hypervisor.h +++ b/robot/hypervisor.h @@ -4,9 +4,13 @@ #include <stdint.h> +#include "../shared/bool.h" + /** amount of parallel timers */ #define W2_HYPERVISOR_TIMER_COUNT (1) +#define W2_TIMER_PING (0) + extern uint64_t g_w2_hypervisor_cycles; extern uint64_t g_w2_hypervisor_uptime_ms; @@ -15,10 +19,16 @@ extern unsigned long g_w2_hypervisor_ema_errcatch_ms; extern unsigned long g_w2_hypervisor_ema_io_ms; extern unsigned long g_w2_hypervisor_ema_mode_ms; +extern unsigned int g_w2_ping_ms; +extern uint8_t g_w2_ping_id; +extern bool g_w2_ping_received; +extern bool g_w2_ping_timeout; +extern bool g_w2_connected; + /** * backbone of all other modules * - * stores global variables and controls when other modules run + * stores global state and controls when other modules run */ void w2_hypervisor_main(); diff --git a/robot/main.c b/robot/main.c index d76dbaf..2f15e97 100644 --- a/robot/main.c +++ b/robot/main.c @@ -5,9 +5,9 @@ #include "sim.h" #endif -int main(int argc, char **argv) { +int main() { #ifdef W2_SIM - w2_sim_setup(argc, argv); + w2_sim_setup(); #endif w2_setup_main(); diff --git a/robot/makefile b/robot/makefile index f65552a..5f05872 100644 --- a/robot/makefile +++ b/robot/makefile @@ -9,7 +9,7 @@ PORT ?= /dev/ttyACM0 # SIM = true CFLAGS=-g -Wall $(DEVICE_SPECIFIC_CFLAGS) -Os -LDFLAGS=-Wl,-gc-sections -Wl,-relax +LDFLAGS=-Wl,-u,vfprintf -lprintf_flt -lm -Wl,-relax -Wl,-gc-sections include ../shared/os.mk all: $(if $(SIM), $(TARGET), out.hex) @@ -19,7 +19,7 @@ HEADERS := $(filter-out sim.h, $(wildcard *.h)) include ../shared/makefile # simulation -CFLAGS += $(if $(SIM), -DW2_SIM, -mcall-prologues -mmcu=$(MCU)) +CFLAGS += $(if $(SIM), -DW2_SIM -DDBG_ENABLE_COLOR, -mcall-prologues -mmcu=$(MCU)) LDFLAGS += $(if $(SIM), , -lpololu_$(DEVICE)) PREFIX := $(if $(SIM), , avr-) SOURCES += $(if $(SIM), sim.c, ) diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c index 9ed9fef..7021721 100644 --- a/robot/mode_dirc.c +++ b/robot/mode_dirc.c @@ -1,10 +1,25 @@ #include "mode_dirc.h" +#include "../shared/util.h" +#include "hypervisor.h" #include "io.h" +#include "modes.h" +#include "orangutan_shim.h" + +#include "../shared/errcatch.h" +#include <string.h> int16_t g_w2_mode_dirc_motor_l = 0; int16_t g_w2_mode_dirc_motor_r = 0; +uint8_t g_w2_mode_dirc_power = 100; void w2_mode_dirc() { - g_w2_io.motor_left.speed = g_w2_mode_dirc_motor_l; - g_w2_io.motor_right.speed = g_w2_mode_dirc_motor_r; + if (g_w2_connected == 1) + g_w2_mode_dirc_power = 100; + else + g_w2_mode_dirc_power = W2_MAX(0, g_w2_mode_dirc_power - 1); + + if (g_w2_mode_dirc_power == 0) w2_modes_call(W2_M_HALT); + + set_motors(g_w2_mode_dirc_motor_l * g_w2_mode_dirc_power / 100, + g_w2_mode_dirc_motor_r * g_w2_mode_dirc_power / 100); } diff --git a/robot/mode_lcal.c b/robot/mode_lcal.c index 6b4e736..896d0f0 100644 --- a/robot/mode_lcal.c +++ b/robot/mode_lcal.c @@ -1 +1,3 @@ #include "mode_lcal.h" + +void w2_mode_lcal() {} diff --git a/robot/modes.c b/robot/modes.c index 9877f6e..7decf47 100644 --- a/robot/modes.c +++ b/robot/modes.c @@ -1,16 +1,34 @@ #include "modes.h" +#include "../shared/errcatch.h" #include "../shared/protocol.h" #include "../shared/util.h" -#include "errcatch.h" #include "sercomm.h" -/** function pointer to current mode */ -// static void (*g_w2_current_mode)() = &w2_mode_halt; - -static void (*g_w2_mode_history[W2_MODE_HISTORY_BUFFER_SIZE])(); -static uint8_t g_w2_mode_history_index = 0; +#include "mode_chrg.h" +#include "mode_dirc.h" +#include "mode_grid.h" +#include "mode_halt.h" +#include "mode_lcal.h" +#include "mode_maze.h" +#include "mode_scal.h" +#include "mode_spin.h" + +w2_e_mode g_w2_mode_history[W2_MODE_HISTORY_BUFFER_SIZE]; +uint8_t g_w2_mode_history_index = 0; +void (*g_w2_modes[W2_MODE_COUNT])(); + +void w2_modes_init() { + g_w2_modes[W2_M_CHRG] = &w2_mode_chrg; + g_w2_modes[W2_M_DIRC] = &w2_mode_dirc; + g_w2_modes[W2_M_GRID] = &w2_mode_grid; + g_w2_modes[W2_M_HALT] = &w2_mode_halt; + g_w2_modes[W2_M_LCAL] = &w2_mode_lcal; + g_w2_modes[W2_M_MAZE] = &w2_mode_maze; + g_w2_modes[W2_M_SCAL] = &w2_mode_scal; + g_w2_modes[W2_M_SPIN] = &w2_mode_spin; +} -void w2_modes_main() { (*g_w2_mode_history[g_w2_mode_history_index])(); } +void w2_modes_main() { (*g_w2_modes[g_w2_mode_history[g_w2_mode_history_index]])(); } void w2_modes_switch(w2_e_mode new_mode, bool replace) { int16_t next_history_index = @@ -24,17 +42,15 @@ void w2_modes_switch(w2_e_mode new_mode, bool replace) { g_w2_mode_history_index = next_history_index; } else { g_w2_mode_history_index = next_history_index; - g_w2_mode_history[g_w2_mode_history_index] = W2_MODES[new_mode]; + g_w2_mode_history[g_w2_mode_history_index] = 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_CREATE_MSG_BIN(w2_s_cmd_mode_tx, msg, msg_bin); + msg->opcode = W2_CMD_MODE | W2_CMDDIR_TX; + msg->mode = new_mode; + w2_sercomm_append_msg(msg_bin); - free(msg); free(msg_bin); } diff --git a/robot/modes.h b/robot/modes.h index 122be4a..8a53560 100644 --- a/robot/modes.h +++ b/robot/modes.h @@ -2,16 +2,16 @@ /** @file modes.h */ +#include <stdint.h> + #include "../shared/consts.h" +#include "../shared/modes.h" + +extern w2_e_mode g_w2_mode_history[W2_MODE_HISTORY_BUFFER_SIZE]; +extern uint8_t g_w2_mode_history_index; -#include "mode_chrg.h" -#include "mode_dirc.h" -#include "mode_grid.h" -#include "mode_halt.h" -#include "mode_lcal.h" -#include "mode_maze.h" -#include "mode_scal.h" -#include "mode_spin.h" +/** setup g_w2_modes array */ +void w2_modes_init(); /** * mode logic @@ -20,25 +20,6 @@ */ 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, - &w2_mode_chrg, &w2_mode_dirc, &w2_mode_spin, &w2_mode_scal, -}; - /** switch current mode (allow switching back to previous mode) */ void w2_modes_call(w2_e_mode mode); /** switch current mode (replace current mode keeping history index) */ diff --git a/robot/readme.md b/robot/readme.md index e6ab294..8995dfb 100644 --- a/robot/readme.md +++ b/robot/readme.md @@ -22,7 +22,14 @@ SIM = true`, the robot code can be compiled for desktop debugging instead. all used pololu functions must be manually implemented in sim.c for this to work, but it allows easier debugging. *it's important that the `orangutan_shim.h` header is used instead of including `<pololu/orangutan.h>` directly for this to -keep working!* +keep working!* if you want to use the simulation robot code with the client, +compile the sim like normal, and use `socat` to create a pseudo-tty and foward +stdio. this pseudo-tty can be used as the com port argument for the client. +here's an example command that creates a tty device in this folder: + +``` +./a.out headless | socat pty,raw,echo=0,link=tty - +``` ## module hierarchy diff --git a/robot/sercomm.c b/robot/sercomm.c index 07c4bd8..c50dd15 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -2,8 +2,8 @@ #include <string.h> #include "../shared/bin.h" +#include "../shared/errcatch.h" #include "../shared/serial_parse.h" -#include "errcatch.h" #include "hypervisor.h" #include "io.h" #include "mode_dirc.h" @@ -31,16 +31,40 @@ void w2_sercomm_main() { g_w2_serial_buffer_index = (g_w2_serial_buffer_index + 1) % W2_SERIAL_READ_BUFFER_SIZE; } + // check time-out + if (!g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > W2_PING_TIMEOUT) { + g_w2_ping_timeout = true; + g_w2_connected = false; + w2_errcatch_throw(W2_E_WARN_PING_TIMEOUT); + } + // send ping every W2_TIMER_PING ms + if ((g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > W2_PING_FREQUENCY) || + g_w2_ping_timeout) { + g_w2_ping_timeout = false; + g_w2_ping_received = false; + g_w2_ping_id = (uint8_t)rand(); + + W2_CREATE_MSG_BIN(w2_s_cmd_ping_tx, msg, bin); + msg->opcode = W2_CMD_PING | W2_CMDDIR_TX; + msg->id = g_w2_ping_id; + + w2_sercomm_append_msg(bin); + free(bin); + + w2_hypervisor_time_start(W2_TIMER_PING); + } + // send data while (g_w2_sercomm_offset != g_w2_sercomm_index) { w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_offset]; #ifdef W2_SIM - w2_sim_print_serial(data); + if (DBG_ENABLE_SERIAL) w2_sim_print_serial(data); #endif - serial_send("\xff", 1); + serial_send_blocking("\xff", 1); for (uint8_t i = 0; i < data->bytes; i++) { uint8_t byte = data->data[i]; - byte == 0xff ? serial_send("\xff\xff", 2) : serial_send((char *)&byte, 1); + byte == 0xff ? serial_send_blocking("\xff\xff", 2) + : serial_send_blocking((char *)&byte, 1); } g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE; } @@ -58,59 +82,33 @@ void w2_sercomm_append_msg(w2_s_bin *data) { g_w2_sercomm_index = next_index; } -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); - void (*handler)(w2_s_bin *) = g_w2_cmd_handlers[data[0]]; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" - if (handler == NULL) { -#ifdef W2_SIM - // TODO throw warning - simwarn("unknown serial message with code 0x%02x\n", data[0]); -#endif - w2_errcatch_throw(W2_E_WARN_SERIAL_NOISY); - } else { -#ifdef W2_SIM - w2_sim_print_serial(copy); -#endif - handler(copy); - } +#include <stdlib.h> +#include <string.h> - free(copy); +void w2_cmd_ping_tx(w2_s_bin *data) { + g_w2_ping_ms = w2_hypervisor_time_end(W2_TIMER_PING); + g_w2_ping_received = true; + g_w2_ping_timeout = false; + g_w2_connected = true; } -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); - w2_s_cmd_ping_tx *return_message = malloc(return_size); - return_message->opcode = W2_CMD_PING | W2_CMDDIR_TX; - return_message->id = message->id; - - w2_s_bin *return_message_bin = w2_bin_s_alloc(return_size, (uint8_t *)return_message); - - w2_sercomm_append_msg(return_message_bin); - - free(message); - free(return_message); - free(return_message_bin); -} +void w2_cmd_ping_rx(w2_s_bin *data) { w2_sercomm_append_msg(data); } 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_swap(message->mode); + W2_CAST_BIN(w2_s_cmd_mode_rx, data, req); + w2_modes_swap(req->mode); } void w2_cmd_sped_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); + W2_CAST_BIN(w2_s_cmd_dirc_rx, data, req); - g_w2_mode_dirc_motor_l = w2_bin_ntoh16(message->left); - g_w2_mode_dirc_motor_r = w2_bin_ntoh16(message->right); + g_w2_mode_dirc_motor_l = w2_bin_ntoh16(req->left); + g_w2_mode_dirc_motor_r = w2_bin_ntoh16(req->right); } void w2_cmd_cord_rx(w2_s_bin *data) { return; } @@ -118,10 +116,9 @@ void w2_cmd_cord_rx(w2_s_bin *data) { return; } void w2_cmd_bomd_rx(w2_s_bin *data) { return; } void w2_cmd_sres_rx(w2_s_bin *data) { - w2_s_cmd_sres_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); - memcpy(message, data->data, data->bytes); + W2_CAST_BIN(w2_s_cmd_sres_rx, data, req); - switch (message->type) { + switch (req->type) { case W2_CMD_SRES_RX_TYPE_REINITGS: { // TODO: soft-reset break; @@ -139,51 +136,34 @@ void w2_cmd_sres_rx(w2_s_bin *data) { void w2_cmd_mcfg_rx(w2_s_bin *data) { return; } void w2_cmd_sens_rx(w2_s_bin *data) { - w2_s_cmd_sens_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); - memcpy(message, data->data, data->bytes); - - size_t return_size = sizeof(w2_s_cmd_sens_tx); - w2_s_cmd_sens_tx *return_message = malloc(return_size); - return_message->opcode = W2_CMD_SENS | W2_CMDDIR_TX; - memcpy((uint8_t *)&return_message->io, (uint8_t *)&g_w2_io, sizeof(w2_s_io_all)); - - for (int i = 0; i < 5; i++) w2_bin_repl_hton16(&return_message->io.qtr[i].range); - w2_bin_repl_hton16(&return_message->io.front_distance.detection); - w2_bin_repl_hton16(&return_message->io.side_distance.detection); - w2_bin_repl_hton16(&return_message->io.battery.charge_level); - w2_bin_repl_hton16((uint16_t *)&return_message->io.motor_left.speed); - w2_bin_repl_hton16((uint16_t *)&return_message->io.motor_right.speed); - - w2_s_bin *return_message_bin = w2_bin_s_alloc(return_size, (uint8_t *)return_message); - - w2_sercomm_append_msg(return_message_bin); - - free(message); - free(return_message); - free(return_message_bin); + W2_CREATE_MSG_BIN(w2_s_cmd_sens_tx, res_msg, res_bin); + res_msg->opcode = W2_CMD_SENS | W2_CMDDIR_TX; + memcpy((uint8_t *)&res_msg->io, (uint8_t *)&g_w2_io, sizeof(w2_s_io_all)); + + for (int i = 0; i < 5; i++) w2_bin_repl_hton16(&res_msg->io.qtr[i].range); + w2_bin_repl_hton16(&res_msg->io.front_distance.detection); + w2_bin_repl_hton16(&res_msg->io.side_distance.detection); + w2_bin_repl_hton16(&res_msg->io.battery.charge_level); + w2_bin_repl_hton16((uint16_t *)&res_msg->io.motor_left.speed); + w2_bin_repl_hton16((uint16_t *)&res_msg->io.motor_right.speed); + + w2_sercomm_append_msg(res_bin); + free(res_bin); } void w2_cmd_info_rx(w2_s_bin *data) { - w2_s_cmd_info_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); - memcpy(message, data->data, data->bytes); - - size_t return_size = sizeof(w2_s_cmd_info_tx); - w2_s_cmd_info_tx *return_message = malloc(return_size); - return_message->opcode = W2_CMD_INFO | W2_CMDDIR_TX; - strncpy((char *)return_message->build_str, W2_BUILD_STR, sizeof(return_message->build_str)); - return_message->errcatch_ms = (uint8_t)g_w2_hypervisor_ema_errcatch_ms; - return_message->io_ms = (uint8_t)g_w2_hypervisor_ema_io_ms; - return_message->sercomm_ms = (uint8_t)g_w2_hypervisor_ema_sercomm_ms; - return_message->mode_ms = (uint8_t)g_w2_hypervisor_ema_mode_ms; - return_message->uptime_s = w2_bin_hton32((uint32_t)(g_w2_hypervisor_uptime_ms / 1e3)); - - w2_s_bin *return_message_bin = w2_bin_s_alloc(return_size, (uint8_t *)return_message); - - w2_sercomm_append_msg(return_message_bin); - - free(message); - free(return_message); - free(return_message_bin); + W2_CREATE_MSG_BIN(w2_s_cmd_info_tx, res_msg, res_bin); + res_msg->opcode = W2_CMD_INFO | W2_CMDDIR_TX; + strncpy((char *)res_msg->build_str, W2_BUILD_STR, sizeof(res_msg->build_str)); + res_msg->errcatch_ms = (uint8_t)g_w2_hypervisor_ema_errcatch_ms; + res_msg->io_ms = (uint8_t)g_w2_hypervisor_ema_io_ms; + res_msg->sercomm_ms = (uint8_t)g_w2_hypervisor_ema_sercomm_ms; + res_msg->mode_ms = (uint8_t)g_w2_hypervisor_ema_mode_ms; + res_msg->uptime_s = w2_bin_hton32((uint32_t)(g_w2_hypervisor_uptime_ms / 1e3)); + res_msg->mode = g_w2_mode_history[g_w2_mode_history_index]; + + w2_sercomm_append_msg(res_bin); + free(res_bin); } void w2_cmd_disp_rx(w2_s_bin *data) { return; } @@ -192,7 +172,8 @@ void w2_cmd_play_rx(w2_s_bin *data) { return; } void w2_cmd_cled_rx(w2_s_bin *data) { return; } -void w2_cmd_ping_tx(w2_s_bin *data) {} +#pragma GCC diagnostic pop + 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) {} diff --git a/robot/setup.c b/robot/setup.c index 95b201e..4706d64 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -19,6 +19,9 @@ void w2_setup_main() { // clear lcd clear(); + // modes array + w2_modes_init(); + // start serial i/o w2_cmd_setup_handlers(); serial_set_baud_rate(W2_SERIAL_BAUD); diff --git a/robot/sim.c b/robot/sim.c index 6283694..9cce12f 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -4,15 +4,15 @@ #include <stdint.h> #include <unistd.h> #include <termios.h> +#include <fcntl.h> #include "sim.h" #include "../shared/consts.h" #include "../shared/protocol.h" #include "sercomm.h" -#include "errcatch.h" +#include "../shared/errcatch.h" struct timespec reference_time; // NOLINT -bool g_w2_sim_headless = false; static const char* const W2_CMD_NAMES[] = { "PING", @@ -74,14 +74,13 @@ void serial_set_baud_rate(unsigned int rate) { simprintfunc("serial_set_baud_rate", "%u", 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; - } +void serial_send_blocking(char* message, unsigned int length) { + for (unsigned int byte = 0; byte < length; byte++) + putc(message[byte] & 0xff, stdout); + fflush(stdout); + return; - if (DBG_ENABLE_PRINTFUNC) simprintfunc("serial_send", "<%u byte%s>", length, length == 1 ? "" : "s"); + simprintfunc("serial_send", "0x%02x", (uint8_t) message[0]); } void serial_receive_ring(char* buffer, unsigned char size) { @@ -93,16 +92,14 @@ unsigned char serial_get_received_bytes() { return g_w2_serial_buffer_head; } -void w2_sim_setup(int argc, char **argv) { - if (argc > 1 && strcmp(argv[1], "headless") == 0) - g_w2_sim_headless = true; - +void w2_sim_setup() { // disable echo and enable raw mode + fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK); struct termios term; tcgetattr(STDIN_FILENO, &term); term.c_lflag &= ~(ECHO | ICANON); term.c_cc[VTIME] = 0; - term.c_cc[VMIN] = 0; + term.c_cc[VMIN] = 1; tcsetattr(STDIN_FILENO, 0, &term); // debug error @@ -110,17 +107,18 @@ void w2_sim_setup(int argc, char **argv) { } void w2_sim_cycle_begin() { + fflush(stdout); + // 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; } void w2_sim_print_serial(w2_s_bin *data) { - if (g_w2_sim_headless) return; simprintf(COL_GRN "[%s_%s]" COL_RST, W2_CMD_NAMES[data->data[0] >> 1], W2_CMD_DIRECTIONS[data->data[0] & W2_CMD_DIRECTION_MASK]); for (int i = 0; i < data->bytes; i++) - printf(" %02x", data->data[i]); - printf("\n"); + fprintf(stderr, " %02x", data->data[i]); + fprintf(stderr, "\n"); } void set_motors(int left, int right) { diff --git a/robot/sim.h b/robot/sim.h index 249414d..7d7c091 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -9,21 +9,21 @@ #include "../shared/bin.h" #include "../shared/protocol.h" -extern bool g_w2_sim_headless; - // debug fine-tuning -#define DBG_ENABLE_PRINTFUNC (1) +#define DBG_ENABLE_PRINTFUNC (0) #define DBG_ENABLE_SIMWARN (1) #define DBG_ENABLE_SIMINFO (1) #define DBG_ENABLE_CYCLEINFO (0) -#define DBG_ENABLE_SERIAL (1) +#define DBG_ENABLE_SERIAL (0) -#define DBG_MAX_CYCLES (10) +#define DBG_CYCLE_DELAY (10e3) +#define DBG_MAX_CYCLES (-1) // debug print options #define DBG_BYTES_PER_LINE 16 // debug colors +#ifdef DBG_ENABLE_COLOR #define COL_BLK "\e[0;30m" #define COL_RED "\e[0;31m" #define COL_GRN "\e[0;32m" @@ -33,9 +33,20 @@ extern bool g_w2_sim_headless; #define COL_CYN "\e[0;36m" #define COL_WHT "\e[0;37m" #define COL_RST "\e[0m" +#else +#define COL_BLK "" +#define COL_RED "" +#define COL_GRN "" +#define COL_YEL "" +#define COL_BLU "" +#define COL_MAG "" +#define COL_CYN "" +#define COL_WHT "" +#define COL_RST "" +#endif // debug stdout print macros -#define simprintf(message, ...) if (!g_w2_sim_headless) printf(COL_RED "[SIM] " COL_RST message, ##__VA_ARGS__) +#define simprintf(message, ...) fprintf(stderr, COL_RED "[SIM] " COL_RST message, ##__VA_ARGS__) #define simprint(message) simprintf(message "\n") #define simprintfunc(name, fmt, ...) if (DBG_ENABLE_PRINTFUNC) { simprintf(COL_BLU "[FUNC] " \ COL_CYN name COL_RST "(" COL_YEL fmt COL_RST ")\n", ##__VA_ARGS__); } @@ -60,7 +71,7 @@ void green_led(unsigned char on); // NOLINT void clear(); // NOLINT void play(const char *melody); // NOLINT void serial_set_baud_rate(unsigned int rate); // NOLINT -void serial_send(char *message, unsigned int length); // NOLINT +void serial_send_blocking(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 @@ -69,7 +80,7 @@ void qtr_read(unsigned int* sensor_values, unsigned char read_mode); // NOLINT unsigned int analog_read(unsigned char channel); // NOLINT void print(const char* str); // NOLINT -void w2_sim_setup(int argc, char **argv); +void w2_sim_setup(); void w2_sim_cycle_begin(); void w2_sim_print_serial(w2_s_bin *data); |