diff options
55 files changed, 1085 insertions, 290 deletions
@@ -9,3 +9,4 @@ client/main *.log scripts/InstallationLog.txt docs/ +tty diff --git a/client/commands.c b/client/commands.c new file mode 100644 index 0000000..778e9c1 --- /dev/null +++ b/client/commands.c @@ -0,0 +1,55 @@ +#include <stdlib.h> + +#include "../shared/bin.h" +#include "../shared/modes.h" +#include "../shared/protocol.h" +#include "commands.h" +#include "main.h" +#include "time.h" + +void w2_send_bin(w2_s_bin *data) { + w2_serial_write("\xff", 1); + for (uint8_t i = 0; i < data->bytes; i++) { + uint8_t byte = data->data[i]; + byte == 0xff ? w2_serial_write("\xff\xff", 2) : w2_serial_write((char *)&byte, 1); + } +} + +void w2_send_info() { + W2_CREATE_MSG_BIN(w2_s_cmd_info_rx, msg, msg_bin); + msg->opcode = W2_CMD_INFO | W2_CMDDIR_RX; + + w2_send_bin(msg_bin); + free(msg_bin); +} + +void w2_send_dirc(uint16_t left, uint16_t right) { + W2_CREATE_MSG_BIN(w2_s_cmd_dirc_rx, msg, msg_bin); + msg->opcode = W2_CMD_DIRC | W2_CMDDIR_RX; + msg->left = w2_bin_hton16(left); + msg->right = w2_bin_hton16(right); + + w2_send_bin(msg_bin); + free(msg_bin); +} + +void w2_send_ping() { + g_w2_state.ping_id = (uint8_t)rand(); + W2_CREATE_MSG_BIN(w2_s_cmd_ping_rx, msg, msg_bin); + msg->opcode = W2_CMD_PING | W2_CMDDIR_RX; + msg->id = g_w2_state.ping_id; + + w2_send_bin(msg_bin); + free(msg_bin); + + w2_timer_start(W2_TIMER_PING); +} + +void w2_send_mode(w2_e_mode mode) { + W2_CREATE_MSG_BIN(w2_s_cmd_mode_rx, msg, msg_bin); + msg->opcode = W2_CMD_MODE | W2_CMDDIR_RX; + msg->mode = mode; + + w2_send_bin(msg_bin); + free(msg_bin); +} diff --git a/client/commands.h b/client/commands.h new file mode 100644 index 0000000..02ae313 --- /dev/null +++ b/client/commands.h @@ -0,0 +1,12 @@ +#pragma once + +#include "../shared/bin.h" +#include "../shared/modes.h" +#include "serial.h" + +void w2_send_bin(w2_s_bin *data); + +void w2_send_info(); +void w2_send_ping(); +void w2_send_mode(w2_e_mode mode); +void w2_send_dirc(uint16_t left, uint16_t right); diff --git a/client/errcatch.c b/client/errcatch.c new file mode 100644 index 0000000..5e39b00 --- /dev/null +++ b/client/errcatch.c @@ -0,0 +1,18 @@ +#include "../shared/errcatch.h" + +void w2_errcatch_handle_error(w2_s_error *error) { + // TODO: handle more error types + switch (error->code) { + case W2_E_WARN_UNCAUGHT_ERROR: { + break; + } + default: { + g_w2_error_uncaught = true; +#ifdef W2_SIM + simwarn("Uncaught/unhandled error found with code 0x%02x\n", error->code); +#endif + } + } + + return; +} diff --git a/client/i18n.h b/client/i18n.h new file mode 100644 index 0000000..06febee --- /dev/null +++ b/client/i18n.h @@ -0,0 +1,6 @@ +#pragma once + +#ifndef W2_LANG_DEFAULT +#define W2_LANG_DEFAULT +#include "i18n/en_us.h" +#endif diff --git a/client/i18n/en_us.h b/client/i18n/en_us.h new file mode 100644 index 0000000..5547db5 --- /dev/null +++ b/client/i18n/en_us.h @@ -0,0 +1,19 @@ +#pragma once + +#define W2_UI_CLI_USAGE "usage: %s <serial port>\n" +#define W2_UI_CLI_SERPORT_ERROR "serial port open fout\n" +#define W2_UI_CLI_INITSCR_FAIL "ncurses initscr() failed\n" +#define W2_UI_CONN_STAT_CONNECTED "connected" +#define W2_UI_CONN_STAT_DISCONNECTED "disconnected" +#define W2_UI_CONN_STAT_PING "ping" +#define W2_UI_BATT_STAT_BATTERY "battery" +#define W2_UI_EXPT_STAT_WARNINGS "warning(s)" +#define W2_UI_EXPT_STAT_ERRORS "error(s)" +#define W2_UI_MODE_CHRG "charging station" +#define W2_UI_MODE_DIRC "direct control" +#define W2_UI_MODE_GRID "grid" +#define W2_UI_MODE_HALT "emergency stop" +#define W2_UI_MODE_LCAL "line calibration" +#define W2_UI_MODE_MAZE "maze" +#define W2_UI_MODE_SCAL "sensor calibration" +#define W2_UI_MODE_SPIN "wet floor simulation" diff --git a/client/main.c b/client/main.c index 76e8197..b5af0e8 100644 --- a/client/main.c +++ b/client/main.c @@ -1 +1,33 @@ -int main() { return 0; } +#include "main.h" +#include "../shared/consts.h" +#include "../shared/errcatch.h" +#include "commands.h" +#include "serial.h" +#include "setup.h" +#include "time.h" +#include "ui.h" + +w2_s_client_state g_w2_state = {.ping_received = true}; + +int main(int argc, char **argv) { + w2_client_setup(argc, argv); + + while (1) { + w2_serial_main(); + w2_errcatch_main(); + w2_ui_main(); + + if (!g_w2_state.ping_received && w2_timer_end(W2_TIMER_PING) > W2_PING_TIMEOUT) { + g_w2_state.ping_timeout = true; + g_w2_state.connected = false; + w2_errcatch_throw(W2_E_WARN_PING_TIMEOUT); + } + + if ((g_w2_state.ping_received && w2_timer_end(W2_TIMER_PING) > W2_PING_FREQUENCY) || + g_w2_state.ping_timeout) { + g_w2_state.ping_timeout = false; + g_w2_state.ping_received = false; + w2_send_ping(); + } + } +} diff --git a/client/main.h b/client/main.h new file mode 100644 index 0000000..cc4c728 --- /dev/null +++ b/client/main.h @@ -0,0 +1,20 @@ +#pragma once + +#include "../shared/protocol.h" + +typedef struct { + unsigned int ping; + uint8_t ping_id; + bool ping_received; + bool ping_timeout; + + bool connected; + uint8_t battery_level; + + uint8_t mode; + + w2_s_cmd_info_tx info; + w2_s_cmd_sens_tx io; +} w2_s_client_state; + +extern w2_s_client_state g_w2_state; diff --git a/client/makefile b/client/makefile index f79dd11..a23ce4f 100644 --- a/client/makefile +++ b/client/makefile @@ -2,20 +2,26 @@ CC = gcc LD = gcc RM = rm -f CFLAGS = +LDFLAGS = -lncursesw EXECNAME = main +include ../shared/os.mk + all: $(EXECNAME) SOURCES := $(wildcard *.c) HEADERS := $(wildcard *.h) -# include ../shared/makefile +include ../shared/makefile + +CFLAGS += $(if $(WIN32),-I/mingw64/include/ncursesw,) + OBJECTS := $(patsubst %.c,%.o, $(SOURCES)) .o: $(CC) -c $(CFLAGS) $< $(EXECNAME): $(OBJECTS) - $(CC) $(OBJECTS) -o $(EXECNAME) + $(CC) $(OBJECTS) -o $(EXECNAME) $(LDFLAGS) clean:: $(RM) $(EXECNAME) *.o diff --git a/client/readme.md b/client/readme.md index 8b4baee..63cd086 100644 --- a/client/readme.md +++ b/client/readme.md @@ -6,19 +6,19 @@ this page is WIP ## features -|feature|due|status|author|description| -|-|-|-|-|-| -|view warnings / errors| -|direct control| -|configure map| -|input orders| -|enable/disable emergency mode| -|enable/disable sensor calibration mode| -|enable/disable wet floor mode| -|read sensor values| -|set display contents|optional -|play music|optional -|control leds|optional +|feature|status|author|description| +|-|-|-|-| +|view warnings / errors|unimplemented||see a log of parsed warnings/errors +|direct control|done|Loek|directly control the robot with tank-style controls +|configure map|unimplemented||set map width/height and define entry/exitpoints +|input orders|unimplemented||type orders with lists of coordinates to visit +|enable/disable emergency mode|unimplemented||self-explanatory +|enable/disable sensor calibration mode|unimplemented||self-explanatory +|enable/disable wet floor mode|unimplemented||self-explanatory +|read sensor values|unimplemented||dashboard that displays all i/o as bar graphs +|set display contents|unimplemented||send text to display on lcd +|play music|unimplemented||play tunes +|control leds|unimplemented||turn on/off underside leds ## interface @@ -27,7 +27,8 @@ the robot in various ways. it primarily works in a bios-like way, with the user having access to multiple tabs containing options or custom interface elements. to start the interface, the user should run `./main <com-port>`. the interface -could look something like this (with colored text for element seperation): +could look something like this (with colored text for element seperation, [see +on figma](https://www.figma.com/file/vZ6rQp2G1HBAmbdrkxIZJ3/terminal-app)): ``` verbonden, 2ms ping (0.0.2-11-g92c394b) batterij 100% @@ -57,9 +58,9 @@ sneltoetsen: ``` the top status bar is always supposed to be visible, and is sort of inspired by -[ncmpcpp](https://github.com/ncmpcpp/ncmpcpp). because the client software -should use no libraries if possible, a custom renderer needs to be implemented, -though it doesn't matter if it's very primitive. +[ncmpcpp](https://github.com/ncmpcpp/ncmpcpp). we're using +[ncurses](https://invisible-mirror.net/ncurses/ncurses.html) to assist in +creating a cross-platform terminal ui. going from top-left in reading order the status bar contains: connection status, ping time, robot version number, robot battery info, current logic @@ -81,10 +82,3 @@ modules are executed after each other: - status bar paint - current tab paint -## notes on ascii escape codes - -- color codes -- terminal echo codes -- how to read terminal (re)size -- cursor movement(?) - diff --git a/client/serial.c b/client/serial.c new file mode 100644 index 0000000..112fd81 --- /dev/null +++ b/client/serial.c @@ -0,0 +1,53 @@ +#include <memory.h> + +#include "../shared/protocol.h" +#include "../shared/serial_parse.h" +#include "commands.h" +#include "main.h" +#include "serial.h" +#include "time.h" + +void w2_serial_main() { + int temp; + while ((temp = w2_serial_read()) != -1) w2_serial_parse(temp); +} + +void w2_cmd_ping_rx(w2_s_bin *data) { + W2_CAST_BIN(w2_s_cmd_ping_tx, data, cast); + + if (g_w2_state.ping_received) return; + if (g_w2_state.ping_id != cast->id) return; + + g_w2_state.ping = w2_timer_end(W2_TIMER_PING); + g_w2_state.ping_received = true; + g_w2_state.ping_timeout = false; + g_w2_state.connected = true; +} + +void w2_cmd_ping_tx(w2_s_bin *data) { w2_send_bin(data); } + +void w2_cmd_expt_tx(w2_s_bin *data) {} +void w2_cmd_mode_tx(w2_s_bin *data) { + W2_CAST_BIN(w2_s_cmd_mode_tx, data, cast); + g_w2_state.mode = cast->mode; +} +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) { + memcpy(&g_w2_state.info, data->data, sizeof(w2_s_cmd_info_tx)); +} + +void w2_cmd_mode_rx(w2_s_bin *data) { return; } +void w2_cmd_sped_rx(w2_s_bin *data) { return; } +void w2_cmd_dirc_rx(w2_s_bin *data) { return; } +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) { return; } +void w2_cmd_mcfg_rx(w2_s_bin *data) { return; } +void w2_cmd_sens_rx(w2_s_bin *data) { return; } +void w2_cmd_info_rx(w2_s_bin *data) { return; } +void w2_cmd_disp_rx(w2_s_bin *data) { return; } +void w2_cmd_play_rx(w2_s_bin *data) { return; } +void w2_cmd_cled_rx(w2_s_bin *data) { return; } diff --git a/client/serial.h b/client/serial.h new file mode 100644 index 0000000..b29dfb5 --- /dev/null +++ b/client/serial.h @@ -0,0 +1,20 @@ +#pragma once + +#include "../shared/bool.h" + +/** @file serial.h */ + +/** + * read byte from serial port + * + * @return -1 if read fails, else char read + */ +int w2_serial_read(); +/** write `data` with length `length` to serial port */ +bool w2_serial_write(char *data, uint8_t length); +/** open serial port */ +bool w2_serial_open(const char *port_name); +/** close serial port */ +void w2_serial_close(); + +void w2_serial_main(); diff --git a/client/serial_linux.c b/client/serial_linux.c new file mode 100644 index 0000000..2497fac --- /dev/null +++ b/client/serial_linux.c @@ -0,0 +1,97 @@ +#ifdef W2_HOST_LINUX + +#include <fcntl.h> +#include <stdio.h> +#include <stdlib.h> +#include <termios.h> +#include <unistd.h> + +#include "../shared/consts.h" +#include "serial.h" + +struct termios g_w2_tty; +struct termios g_w2_tty_old; +int g_w2_serial_handle; + +char g_w2_serial_buffer[W2_SERIAL_READ_BUFFER_SIZE]; +uint8_t g_w2_serial_buffer_index; +uint8_t g_w2_serial_buffer_head; + +speed_t w2_baud_map(int baud) { + switch (baud) { + case 9600: + return B9600; + case 19200: + return B19200; + case 38400: + return B38400; + case 57600: + return B57600; + case 115200: + return B115200; + case 230400: + return B230400; + case 460800: + return B460800; + case 500000: + return B500000; + case 576000: + return B576000; + case 921600: + return B921600; + case 1000000: + return B1000000; + case 1152000: + return B1152000; + case 1500000: + return B1500000; + case 2000000: + return B2000000; + case 2500000: + return B2500000; + case 3000000: + return B3000000; + case 3500000: + return B3500000; + case 4000000: + return B4000000; + default: + return B0; + } +} + +int w2_serial_read() { + int return_val; + int bytes = read(g_w2_serial_handle, &return_val, 1); + return bytes != 1 ? -1 : (uint8_t)return_val; +} + +bool w2_serial_write(char *data, uint8_t length) { + return write(g_w2_serial_handle, data, length) == length; +} + +bool w2_serial_open(const char *port_name) { + g_w2_serial_handle = open(port_name, O_RDWR | O_NONBLOCK); + if (g_w2_serial_handle < 0 || tcgetattr(g_w2_serial_handle, &g_w2_tty) != 0) return false; + + g_w2_tty_old = g_w2_tty; + + speed_t baud = w2_baud_map(W2_SERIAL_BAUD); + cfsetospeed(&g_w2_tty, baud); + cfsetispeed(&g_w2_tty, baud); + + g_w2_tty.c_cc[VMIN] = 0; + g_w2_tty.c_cc[VTIME] = 0; + + cfmakeraw(&g_w2_tty); + + tcflush(g_w2_serial_handle, TCIFLUSH); + + if (tcsetattr(g_w2_serial_handle, TCSANOW, &g_w2_tty) != 0) return false; + + return true; +} + +void w2_serial_close() { close(g_w2_serial_handle); } + +#endif diff --git a/client/serial_win32.c b/client/serial_win32.c new file mode 100644 index 0000000..edc9db1 --- /dev/null +++ b/client/serial_win32.c @@ -0,0 +1,21 @@ +#ifdef W2_HOST_WIN32 + +#include "serial.h" + +int w2_serial_read() { + return 0x00; +} + +bool w2_serial_write(char *data, uint8_t length) { + return true; +} + +bool w2_serial_open(const char *port_name) { + return true; +} + +void w2_serial_close() { + return; +} + +#endif diff --git a/client/setup.c b/client/setup.c new file mode 100644 index 0000000..fd37c13 --- /dev/null +++ b/client/setup.c @@ -0,0 +1,44 @@ +#include <ncurses.h> +#include <stdio.h> +#include <stdlib.h> + +#include "../shared/bin.h" +#include "../shared/protocol.h" +#include "commands.h" +#include "serial.h" +#include "setup.h" +#include "strings.h" +#include "ui.h" + +// pointers for endianness check +static const uint16_t _test = 1; +static const uint8_t *_ptest = (uint8_t *)&_test; +uint8_t g_w2_endianness; + +void w2_client_setup(int argc, char **argv) { + if (argc < 2) { + printf(W2_UI_CLI_USAGE, argv[0]); + exit(1); + } + + if (w2_serial_open(argv[1]) == 0) { + printf(W2_UI_CLI_SERPORT_ERROR); + exit(1); + } + + if ((g_w2_ui_win = initscr()) == NULL) { + printf(W2_UI_CLI_INITSCR_FAIL); + exit(1); + } + noecho(); + curs_set(false); + nodelay(g_w2_ui_win, true); + + w2_strings_init(); + w2_cmd_setup_handlers(); + + w2_send_info(); + + // check endianness + g_w2_endianness = *_ptest; +} diff --git a/client/setup.h b/client/setup.h new file mode 100644 index 0000000..bcad11e --- /dev/null +++ b/client/setup.h @@ -0,0 +1,3 @@ +#pragma once + +void w2_client_setup(int argc, char **argv); diff --git a/client/strings.c b/client/strings.c new file mode 100644 index 0000000..b97d4b2 --- /dev/null +++ b/client/strings.c @@ -0,0 +1,16 @@ +#include "strings.h" + +char *g_w2_mode_strings[W2_MODE_COUNT]; + +void w2_strings_modes_init() { + g_w2_mode_strings[W2_M_CHRG] = W2_UI_MODE_CHRG; + g_w2_mode_strings[W2_M_DIRC] = W2_UI_MODE_DIRC; + g_w2_mode_strings[W2_M_GRID] = W2_UI_MODE_GRID; + g_w2_mode_strings[W2_M_HALT] = W2_UI_MODE_HALT; + g_w2_mode_strings[W2_M_LCAL] = W2_UI_MODE_LCAL; + g_w2_mode_strings[W2_M_MAZE] = W2_UI_MODE_MAZE; + g_w2_mode_strings[W2_M_SCAL] = W2_UI_MODE_SCAL; + g_w2_mode_strings[W2_M_SPIN] = W2_UI_MODE_SPIN; +} + +void w2_strings_init() { w2_strings_modes_init(); } diff --git a/client/strings.h b/client/strings.h new file mode 100644 index 0000000..0085228 --- /dev/null +++ b/client/strings.h @@ -0,0 +1,10 @@ +#pragma once + +#include "../shared/modes.h" +#include "i18n.h" + +#define W2_STRINGS_MODE_MAP_BUFFER_SIZE 32 + +extern char *g_w2_mode_strings[W2_MODE_COUNT]; + +void w2_strings_init(); diff --git a/client/time.c b/client/time.c new file mode 100644 index 0000000..d48be64 --- /dev/null +++ b/client/time.c @@ -0,0 +1,3 @@ +#include "time.h" + +unsigned long g_w2_client_timers[W2_CLIENT_TIMER_COUNT] = {0}; diff --git a/client/time.h b/client/time.h new file mode 100644 index 0000000..a989c5c --- /dev/null +++ b/client/time.h @@ -0,0 +1,12 @@ +#pragma once + +/** amount of parallel timers */ +#define W2_CLIENT_TIMER_COUNT (4) +extern unsigned long g_w2_client_timers[W2_CLIENT_TIMER_COUNT]; +typedef enum { + W2_TIMER_PING = 0, + W2_TIMER_UPDATE = 1, +} w2_e_client_timers; + +void w2_timer_start(w2_e_client_timers label); +unsigned long w2_timer_end(w2_e_client_timers label); diff --git a/client/time_linux.c b/client/time_linux.c new file mode 100644 index 0000000..f6d50d8 --- /dev/null +++ b/client/time_linux.c @@ -0,0 +1,19 @@ +#ifdef W2_HOST_LINUX + +#include <time.h> + +#include "time.h" + +unsigned long w2_get_time() { + struct timespec now; + clock_gettime(CLOCK_MONOTONIC, &now); + return ((now.tv_sec * 1000) + (now.tv_nsec / 1000000)); +} + +void w2_timer_start(w2_e_client_timers label) { g_w2_client_timers[label] = w2_get_time(); } + +unsigned long w2_timer_end(w2_e_client_timers label) { + return w2_get_time() - g_w2_client_timers[label]; +} + +#endif diff --git a/client/time_windows.c b/client/time_windows.c new file mode 100644 index 0000000..f9082d6 --- /dev/null +++ b/client/time_windows.c @@ -0,0 +1,15 @@ +#ifdef W2_HOST_WIN32 + +#include "time.h" + +unsigned long w2_get_time() { + return 0; +} + +void w2_timer_start(w2_e_client_timers label) { g_w2_client_timers[label] = w2_get_time(); } + +unsigned long w2_timer_end(w2_e_client_timers label) { + return w2_get_time() - g_w2_client_timers[label]; +} + +#endif diff --git a/client/ui.c b/client/ui.c new file mode 100644 index 0000000..1cde52f --- /dev/null +++ b/client/ui.c @@ -0,0 +1,71 @@ +#include <ncurses.h> +#include <string.h> + +#include "../shared/bin.h" +#include "../shared/util.h" +#include "i18n.h" +#include "main.h" +#include "strings.h" +#include "term.h" +#include "time.h" +#include "ui.h" + +WINDOW *g_w2_ui_win; +unsigned int g_w2_ui_width = 0; +unsigned int g_w2_ui_height = 0; +void (*g_w2_ui_current_tab)(bool first) = &w2_ui_dirc; +void (*g_w2_ui_last_tab)(bool first) = NULL; + +void w2_ui_main() { + g_w2_ui_width = getmaxx(g_w2_ui_win); + g_w2_ui_height = getmaxy(g_w2_ui_win); + + w2_ui_paint(); +} + +void w2_ui_paint() { + w2_ui_paint_statusbar(); + if (w2_timer_end(W2_TIMER_UPDATE) >= (1000 / W2_UI_UPDATE_FPS)) { + (*g_w2_ui_current_tab)(g_w2_ui_last_tab != g_w2_ui_current_tab); + g_w2_ui_last_tab = g_w2_ui_current_tab; + w2_timer_start(W2_TIMER_UPDATE); + } + refresh(); +} + +void w2_ui_paint_statusbar() { + char temp[g_w2_ui_width]; + + for (unsigned int i = 0; i < g_w2_ui_width; i++) temp[i] = ' '; + mvaddnstr(0, 0, temp, g_w2_ui_width); + mvaddnstr(1, 0, temp, g_w2_ui_width); + mvaddnstr(2, 0, temp, g_w2_ui_width); + + sprintf(temp, "%s, %ims %s", + g_w2_state.connected ? W2_UI_CONN_STAT_CONNECTED : W2_UI_CONN_STAT_DISCONNECTED, + g_w2_state.ping, W2_UI_CONN_STAT_PING); + mvaddstr(0, 0, temp); + + sprintf(temp, "(%s)", g_w2_state.info.build_str); + mvaddstr(0, g_w2_ui_width / 2 - strlen(temp) / 2, temp); + + sprintf(temp, "%s %i%%", W2_UI_BATT_STAT_BATTERY, g_w2_state.battery_level); + mvaddstr(0, g_w2_ui_width - strlen(temp), temp); + + sprintf(temp, "[%s]", g_w2_mode_strings[g_w2_state.mode]); + mvaddstr(1, 0, temp); + + sprintf(temp, "%i %s, %i %s", 0, W2_UI_EXPT_STAT_WARNINGS, 0, W2_UI_EXPT_STAT_ERRORS); + mvaddstr(1, g_w2_ui_width - strlen(temp), temp); + + w2_ui_paint_tabbar(); + + for (unsigned int i = 0; i < g_w2_ui_width; i++) temp[i] = '-'; + mvaddnstr(3, 0, temp, g_w2_ui_width); +} + +void w2_ui_paint_tabbar() { + char temp[g_w2_ui_width]; + sprintf(temp, "-- tab bar here --"); + mvaddstr(2, g_w2_ui_width / 2 - strlen(temp) / 2, temp); +} diff --git a/client/ui.h b/client/ui.h new file mode 100644 index 0000000..0d375de --- /dev/null +++ b/client/ui.h @@ -0,0 +1,27 @@ +#pragma once + +#include <ncurses.h> +#include <stdint.h> + +#define W2_UI_UPDATE_FPS (60) + +extern WINDOW *g_w2_ui_win; +extern unsigned int g_w2_ui_width; +extern unsigned int g_w2_ui_height; +extern void (*g_w2_ui_current_tab)(bool first); + +/** update terminal props */ +void w2_ui_update(); +/** clear screen */ +void w2_ui_clear(); +/** draw complete ui */ +void w2_ui_paint(); +/** update and paint */ +void w2_ui_main(); + +/** draw status bar */ +void w2_ui_paint_statusbar(); +/** draw tab bar */ +void w2_ui_paint_tabbar(); + +void w2_ui_dirc(bool first); diff --git a/client/ui_dirc.c b/client/ui_dirc.c new file mode 100644 index 0000000..ed69cd2 --- /dev/null +++ b/client/ui_dirc.c @@ -0,0 +1,112 @@ +#include "../shared/protocol.h" +#include "../shared/util.h" +#include "commands.h" +#include "ui.h" + +/** decay modifier */ +#define W2_DIRC_MOD ((double)0.95) +/** add value per key press */ +#define W2_DIRC_ADD ((double)17.0) +/** padding */ +#define W2_DIRC_PAD ((double)3.00) +/** average samples */ +#define W2_DIRC_SPL ((unsigned int)14) +/** steering padding */ +#define W2_DIRC_STP ((double)0.2) + +int w2_avg(int *samples, unsigned int sample_count) { + double total = 0; + for (int i = 0; i < sample_count; i++) { + total += (double)samples[i] / (double)sample_count; + } + return (int)total; +} + +#define W2_DIRC_MOTOR_DRIVER(name) \ + int w2_dirc_motor_##name(unsigned int forward, unsigned int backward) { \ + static unsigned int idx = 0; \ + \ + static double drive = 0.f; \ + static int drive_avg[W2_DIRC_SPL] = {0}; \ + \ + drive *= W2_DIRC_MOD; \ + drive += W2_DIRC_ADD * forward + -W2_DIRC_ADD * backward; \ + drive = W2_RANGE(-254, drive, 255); \ + \ + idx = (idx + 1) % W2_DIRC_SPL; \ + drive_avg[idx] = (int)W2_RANGE(-254, drive * W2_DIRC_PAD, 255); \ + \ + return w2_avg(drive_avg, W2_DIRC_SPL); \ + } + +W2_DIRC_MOTOR_DRIVER(l); +W2_DIRC_MOTOR_DRIVER(r); + +void w2_ui_dirc_init() { w2_send_mode(W2_M_DIRC); } + +void w2_ui_bar_graph(unsigned int y, unsigned int x, unsigned int width, double value) { + char temp[width]; + temp[0] = '|'; + temp[width - 1] = '|'; + for (unsigned int i = 0; i < width - 2; i++) temp[i + 1] = i < width * value ? '*' : ' '; + + mvaddnstr(y, x, temp, width); +} + +void w2_ui_bar_graph_pm(unsigned int y, unsigned int x, unsigned int width, double value) { + char temp[width]; + temp[0] = '|'; + temp[width - 1] = '|'; + width -= 2; + unsigned int hw = width / 2; + if (value >= 0) { + for (unsigned int i = 0; i < width; i++) + temp[i + 1] = i < hw ? ' ' : (i - hw) < (hw * value) ? '*' : ' '; + } else { + for (unsigned int i = 0; i < width; i++) + temp[i + 1] = i > hw ? ' ' : (hw - i) < -(hw * value) ? '*' : ' '; + } + + mvaddnstr(y, x, temp, width + 2); +} + +void w2_ui_dirc_paint(int left, int right) { + mvaddstr(4, 0, "left drive: "); + w2_ui_bar_graph_pm(4, 13, g_w2_ui_width - 13, (double)left / 255); + mvaddstr(5, 0, "right drive: "); + w2_ui_bar_graph_pm(5, 13, g_w2_ui_width - 13, (double)right / 255); + + mvaddstr(7, 0, + " controls:\n" + "\n" + " <q> <w> <e> forward\n" + " <a> <s> <d> backward\n" + "left both right\n" + "\n" + "<space> send dirc mode command"); +} + +void w2_ui_dirc(bool first) { + if (first) w2_ui_dirc_init(); + int ch = 0; + unsigned int lb = 0; + unsigned int lf = 0; + unsigned int rb = 0; + unsigned int rf = 0; + while ((ch = getch()) != -1) { + if (ch == 'e' || ch == 'w') lf++; + if (ch == 'd' || ch == 's') lb++; + if (ch == 'q' || ch == 'w') rf++; + if (ch == 'a' || ch == 's') rb++; + if (ch == ' ') w2_send_mode(W2_M_DIRC); + } + + int drive_l = w2_dirc_motor_l(lf, lb); + int drive_r = w2_dirc_motor_r(rf, rb); + + drive_l += drive_r * W2_DIRC_STP; + drive_r += drive_l * W2_DIRC_STP; + + w2_send_dirc(drive_l, drive_r); + w2_ui_dirc_paint(drive_l, drive_r); +} diff --git a/protocol.md b/protocol.md index bc6a8c0..9c45f56 100644 --- a/protocol.md +++ b/protocol.md @@ -66,9 +66,11 @@ in *both* the robot and client code `r <-- c` is referred to as `rx` and `r |`uint8_t`|opcode (`0x00 + 0` or `0x00 + 1`)| |`uint8_t`|ping id| -**ping** sends back an identical message either way with the direction bit -toggled. _ping id_ is a random 8-bit value that makes sure the same ping -doesn't keep bouncing back and forth indefinitely. +**ping** sends back an identical message either way with the **same** direction +bit. _ping id_ is a random 8-bit value that identifies the ping message. this +is the only command that makes either the robot or client send a message with +an opcode not matching the respective sender. the direction bit indicates which +device initiated the ping message. ### EXPT @@ -297,7 +299,7 @@ packet. requests robot info -#### robot info response (`r --> c`) (41 bytes) +#### robot info response (`r --> c`) (42 bytes) |type|description| |-:|-| @@ -308,6 +310,7 @@ requests robot info |`uint8_t`|exponential moving average sercomm module cycle time (ms)| |`uint8_t`|exponential moving average modes module cycle time (ms)| |`uint32_t`|total robot uptime (s)| +|`uint8_t`|current mode code| robot info response @@ -6,6 +6,19 @@ this project is divided in two subfolders, one for robot code, and one for client code that runs on your PC and is able to control the robot remotely. +## supported features + +the build toolchain is compatible with windows and linux, though some features +are only supported on linux: + +|feature|windows|linux| +|-|:-:|:-:| +|robot code compilation (avr)|yes|yes| +|robot exec upload|yes|yes| +|client code compilation|yes|yes| +|robot code simulation (x86)| |yes| +|use client with robot sim| |yes| + ## toolchain installation on windows > look in the scripts/ subdirectory if you're concerned about what these 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); diff --git a/scripts/install-mingw-packages.sh b/scripts/install-mingw-packages.sh index fd82909..e1c000c 100644 --- a/scripts/install-mingw-packages.sh +++ b/scripts/install-mingw-packages.sh @@ -1,2 +1,2 @@ #!/bin/sh -pacman --noconfirm -Sy make git unzip mingw-w64-x86_64-avr-toolchain mingw-w64-x86_64-toolchain mingw-w64-clang-x86_64-clang mingw-w64-x86_64-avrdude python3 python3-pip +pacman --noconfirm -Sy make git unzip mingw-w64-x86_64-avr-toolchain mingw-w64-x86_64-toolchain mingw-w64-clang-x86_64-clang mingw-w64-x86_64-avrdude python3 python3-pip ncurses mingw-w64-x86_64-ncurses diff --git a/scripts/patch-june-5.sh b/scripts/patch-june-5.sh new file mode 100644 index 0000000..cad705a --- /dev/null +++ b/scripts/patch-june-5.sh @@ -0,0 +1,3 @@ +#!/bin/sh + +pacman --noconfirm -Sy ncurses mingw-w64-x86_64-ncurses diff --git a/shared/bin.h b/shared/bin.h index 48485c8..4f7db81 100644 --- a/shared/bin.h +++ b/shared/bin.h @@ -13,6 +13,13 @@ extern uint8_t g_w2_endianness; +#define W2_CAST_BIN(type, in, out) type *out = (type *)&in->data; +#define W2_CREATE_MSG_BIN(type, normal, bin) W2_CREATE_MSG_SIZE_BIN(type, sizeof(type), normal, bin) +#define W2_CREATE_MSG_SIZE_BIN(type, size, normal, bin) \ + w2_s_bin *bin = malloc(sizeof(w2_s_bin) + size); \ + bin->bytes = size; \ + type *normal = (type *)&bin->data; + typedef struct { uint16_t bytes; uint8_t data[]; diff --git a/shared/bool.h b/shared/bool.h index 9ea97f7..4ffb6b4 100644 --- a/shared/bool.h +++ b/shared/bool.h @@ -4,6 +4,8 @@ /** @file bool.h */ +#ifndef bool typedef uint8_t bool; #define false 0 /* NOLINT */ #define true 1 /* NOLINT */ +#endif diff --git a/shared/consts.h b/shared/consts.h index cdd96b3..cd6dff1 100644 --- a/shared/consts.h +++ b/shared/consts.h @@ -22,7 +22,7 @@ /** size of the serial communication buffer (in messages, not bytes) */ #define W2_SERCOMM_BUFFER_SIZE (16) /** size of mode history buffer */ -#define W2_MODE_HISTORY_BUFFER_SIZE (4) +#define W2_MODE_HISTORY_BUFFER_SIZE (8) /** max logic module execution time in milliseconds */ #define W2_MAX_MODULE_CYCLE_MS (20) @@ -30,6 +30,11 @@ /** exponential moving average new measurement weight (double 0-1) */ #define W2_EMA_WEIGHT (0.10) +/** minimal time between pings */ +#define W2_PING_FREQUENCY (1e3) +/** max time between ping and answer */ +#define W2_PING_TIMEOUT (5e3) + /** front-facing distance sensor pinout */ #define W2_FRONT_SENSOR_PIN 5 /** battery voltage sensor pinout */ diff --git a/shared/errcatch.c b/shared/errcatch.c new file mode 100644 index 0000000..76af10a --- /dev/null +++ b/shared/errcatch.c @@ -0,0 +1,45 @@ +#include <stdlib.h> +#include <string.h> + +#include "errcatch.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; +bool g_w2_error_buffer_full = 0; +bool 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; +} diff --git a/shared/errors.h b/shared/errcatch.h index 344a506..a56bc00 100644 --- a/shared/errors.h +++ b/shared/errcatch.h @@ -1,9 +1,12 @@ #pragma once -/** @file errors.h */ +/** @file errcatch.h */ #include <stdint.h> +#include "bool.h" +#include "consts.h" + #define W2_E_TYPE_MASK (0b11 << 6) #define W2_E_TYPE_CRIT (0b00 << 6) @@ -49,6 +52,8 @@ typedef enum { W2_E_WARN_SERIAL_NOISY = 0x09 | W2_E_TYPE_WARN, /** mode history index out of bounds */ W2_E_WARN_MODE_HISTORY_BUFFER_IOB = 0x0a | W2_E_TYPE_WARN, + /** ping timeout reached */ + W2_E_WARN_PING_TIMEOUT = 0x0b | W2_E_TYPE_WARN, } w2_e_errorcode; /** @@ -62,3 +67,33 @@ typedef struct { uint8_t message_length; char message[]; } w2_s_error; + +/** 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 buffer full flag */ +extern bool g_w2_error_buffer_full; +/** uncaught error flag */ +extern bool g_w2_error_uncaught; + +/** 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/shared/modes.h b/shared/modes.h new file mode 100644 index 0000000..ff939ea --- /dev/null +++ b/shared/modes.h @@ -0,0 +1,16 @@ +#pragma once + +#define W2_MODE_COUNT 8 + +/** 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; diff --git a/shared/protocol.h b/shared/protocol.h index 93e53f4..02d5526 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -155,6 +155,7 @@ typedef struct { uint8_t sercomm_ms; uint8_t mode_ms; uint32_t uptime_s; + uint8_t mode; } w2_s_cmd_info_tx; typedef struct { diff --git a/shared/serial_parse.c b/shared/serial_parse.c index b1b4f50..d07c67f 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -1,7 +1,11 @@ #include <string.h> #include "consts.h" +#include "errcatch.h" #include "serial_parse.h" +#ifdef W2_SIM +#include "../robot/orangutan_shim.h" +#endif bool w2_serial_parse(uint8_t byte) { static uint8_t current_message[W2_SERIAL_READ_BUFFER_SIZE] = {0}; @@ -38,3 +42,19 @@ bool w2_serial_parse(uint8_t byte) { return W2_SERIAL_READ_SUCCESS; } + +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]]; + + if (handler == NULL) { + w2_errcatch_throw(W2_E_WARN_SERIAL_NOISY); + } else { +#ifdef W2_SIM + if (DBG_ENABLE_SERIAL) w2_sim_print_serial(copy); +#endif + handler(copy); + } + + free(copy); +} diff --git a/shared/util.c b/shared/util.c index 55f3491..68503e8 100644 --- a/shared/util.c +++ b/shared/util.c @@ -4,3 +4,5 @@ unsigned long w2_util_exp_mov_avg(unsigned long current_avg, unsigned long new_m return (unsigned long)((((double)(current_avg)) * ((double)(1.f - W2_EMA_WEIGHT))) + (((double)(new_meas)) * ((double)(W2_EMA_WEIGHT)))); } + +int w2_sign(int n) { return (n > 0) - (n < 0); } diff --git a/shared/util.h b/shared/util.h index 465e043..230c3e4 100644 --- a/shared/util.h +++ b/shared/util.h @@ -7,3 +7,4 @@ #define W2_RANGE(min, val, max) W2_MIN(max, W2_MAX(val, min)) unsigned long w2_util_exp_mov_avg(unsigned long current_avg, unsigned long new_meas); +int w2_sign(int n); |