diff options
63 files changed, 1304 insertions, 184 deletions
diff --git a/.clang-tidy b/.clang-tidy index 054a943..4dd70ad 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,16 +1,19 @@ Checks: '-*,readability-identifier-naming' CheckOptions: + - { key: readability-identifier-naming.EnumCase, value: lower_case } + - { key: readability-identifier-naming.EnumPrefix, value: w2_e_ } - { key: readability-identifier-naming.FunctionCase, value: lower_case } - { key: readability-identifier-naming.FunctionPrefix, value: w2_ } - - { key: readability-identifier-naming.GlobalVariableCase, value: lower_case } - - { key: readability-identifier-naming.GlobalVariablePrefix, value: g_w2_ } - { key: readability-identifier-naming.GlobalConstantCase, value: UPPER_CASE } + - { key: readability-identifier-naming.GlobalConstantIgnoredRegexp, value: _.* } - { key: readability-identifier-naming.GlobalConstantPrefix, value: W2_ } + - { key: readability-identifier-naming.GlobalVariableCase, value: lower_case } + - { key: readability-identifier-naming.GlobalVariableIgnoredRegexp, value: _.* } + - { key: readability-identifier-naming.GlobalVariablePrefix, value: g_w2_ } - { key: readability-identifier-naming.MacroDefinitionCase, value: UPPER_CASE } + - { key: readability-identifier-naming.MacroDefinitionIgnoredRegexp, value: _.* } - { key: readability-identifier-naming.MacroDefinitionPrefix, value: W2_ } - { key: readability-identifier-naming.StructCase, value: lower_case } - { key: readability-identifier-naming.StructPrefix, value: w2_s_ } - - { key: readability-identifier-naming.EnumCase, value: lower_case } - - { key: readability-identifier-naming.EnumPrefix, value: w2_e_ } # vim: ft=yaml diff --git a/client/makefile b/client/makefile index 23d5ce6..4cca15a 100644 --- a/client/makefile +++ b/client/makefile @@ -4,19 +4,20 @@ RM = rm -f CFLAGS = EXECNAME = main +all: $(EXECNAME) + SOURCES := $(wildcard *.c) HEADERS := $(wildcard *.h) +# include ../shared/makefile OBJECTS := $(patsubst %.c,%.o, $(SOURCES)) -all: main - .o: $(CC) -c $(CFLAGS) $< $(EXECNAME): $(OBJECTS) $(CC) $(OBJECTS) -o $(EXECNAME) -clean: +clean:: $(RM) $(EXECNAME) *.o format: diff --git a/client/readme.md b/client/readme.md new file mode 100644 index 0000000..990504d --- /dev/null +++ b/client/readme.md @@ -0,0 +1,82 @@ +# client code + +this is the subdirectory for all client code (runs on pc/laptop) + +this page is WIP + +## features + +|feature|due|status|author|description| +|-|-|-|-|-| +|view warnings / errors| +|direct control| +|enable/disable emergency mode| + + + + + +## interface + +the client is a user-facing application that allows control and monitoring of +the robot in various ways. it primarily works in a command-line way, with the +user typing commands that get translated to protocol messages, and sent to the +robot. to start the interface, the user should run `./main <com-port> [command] +[args]`. by not specifying a command, the interactive shell is launched which +looks like this (and it's supposed to be in dutch): + +``` +verbonden, 2ms ping (0.0.2-11-g92c394b) batterij 100% +[huidige logica-modus] 0 waarschuwingen, 0 foutmeldingen +-------------------------------------------------------------------------------- + + + + + + + + + + + + + + +welkom in de wall-e2 console applicatie! deze client is versie +0.0.2-11-g92c394b + +typ 'help' om alle commando's te zien in een lijst, of 'doei' om deze +applicatie weer te sluiten. + +w2> +``` + +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. + +going from top-left in reading order the status bar contains: connection +status, ping time, robot version number, robot battery info, current logic +mode, warnings and critical exceptions. the version number is aligned to the +terminal center, and the battery and warning counters are aligned right. when a +connection hasn't been established between the robot and client, the fields +containing robot info should dissapear. + +## code structure + +because multithreading is hard, another cyclic system is used. the following +modules are executed after each other: + +- serial read +- stdin read (user input) +- optional control mode handling + +## notes on ascii escape codes + +- color codes +- terminal echo codes +- how to read terminal (re)size +- cursor movement + diff --git a/protocol.md b/protocol.md index f5bb0cc..c6cb3fb 100644 --- a/protocol.md +++ b/protocol.md @@ -25,24 +25,24 @@ is converted to a single `0xff` on the receiving end, so these duplicated bytes and the starting byte don't count towards message length. opcodes are picked sequentially, but the direction bit (LSB) is reserved to -indicate a transfer from robot to client. this means that the opcode for a -sensor data request would be `0x12`, but the response opcode would be `0x13`. -these opcodes are stored as enum constants inside consts.h for code +indicate a transfer from robot to client (`tx`). this means that the opcode for +a sensor data request would be `0x12`, but the response opcode would be `0x13`. +these opcodes are stored as enum constants inside shared/protocol.h for code readability. |code|name|implemented|directions|full name| |--:|---|:-:|:-:|---| -|`0x00`|[PING](#ping)|no|`r <=> c`|<u>ping</u> -|`0x02`|[EXPT](#expt)|no|`r --> c`|<u>ex</u>ce<u>pt</u>ion -|`0x04`|[MODE](#mode)|no|`r <=> c`|<u>mode</u> +|`0x00`|[PING](#ping)|yes|`r <=> c`|<u>ping</u> +|`0x02`|[EXPT](#expt)|yes|`r --> c`|<u>ex</u>ce<u>pt</u>ion +|`0x04`|[MODE](#mode)|yes|`r <=> c`|<u>mode</u> |`0x06`|[SPED](#sped)|no|`r <-- c`|<u>spe</u>e<u>d</u> -|`0x08`|[DIRC](#dirc)|no|`r <-- c`|<u>dir</u>ect <u>c</u>ontrol +|`0x08`|[DIRC](#dirc)|yes|`r <-- c`|<u>dir</u>ect <u>c</u>ontrol |`0x0a`|[CORD](#cord)|no|`r <=> c`|<u>co</u>o<u>rd</u>inate |`0x0c`|[BOMD](#bomd)|no|`r <=> c`|<u>b</u>ack<u>o</u>rder <u>m</u>o<u>d</u>ify |`0x0e`|[SRES](#sres)|no|`r <-- c`|<u>s</u>oft <u>res</u>et |`0x10`|[MCFG](#mcfg)|no|`r <-- c`|<u>m</u>ap <u>c</u>on<u>f</u>i<u>g</u> |`0x12`|[SENS](#sens)|no|`r <-> c`|<u>sens</u>or data -|`0x14`|[INFO](#info)|no|`r <-> c`|<u>info</u> +|`0x14`|[INFO](#info)|yes|`r <-> c`|<u>info</u> |`0x16`|[DISP](#disp)|no|`r <-- c`|<u>disp</u>lay control |`0x18`|[PLAY](#play)|no|`r <-- c`|<u>play</u> midi |`0x1a`|[CLED](#cled)|no|`r <-- c`|<u>c</u>ontrol <u>led</u>s @@ -54,6 +54,9 @@ a double stroke arrow means that the command can be initiated from either the robot or the client, while a single arrow indicates a request-response structure. +in *both* the robot and client code `r <-- c` is referred to as `rx` and `r +--> c` as `tx` (from the *robots* view). + ### PING #### ping (`r <=> c`) (2 bytes) @@ -93,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) @@ -291,10 +303,10 @@ requests robot info |-:|-| |`uint8_t`|opcode (`0x14 + 1`)| |`uint8_t[32]`|build string| -|`uint8_t`|errcatch module cycle time (ms)| -|`uint8_t`|io module cycle time (ms)| -|`uint8_t`|sercomm module cycle time (ms)| -|`uint8_t`|modes module cycle time (ms)| +|`uint8_t`|exponential moving average errcatch module cycle time (ms)| +|`uint8_t`|exponential moving average io module cycle time (ms)| +|`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)| robot info response @@ -14,14 +14,6 @@ gebruikte externe libraries: |-|-| |[yan9a/serial](https://github.com/yan9a/serial)|cross-compatibiliteit voor seriële poorten lezen/schrijven voor windows en linux| -## git test :tada: - -- [x] abdullaahi -- [x] fiona -- [x] jorn -- [x] loek - - ## samenvatting werking hoop onder constructie @@ -50,14 +42,15 @@ De lijnen van het doolhof hoeven niet te voldoen aan de tegelgrootte, maar de in- en uitgangen moeten wel een rechte overloop hebben op het warenhuis-gedeelte. -## te bespreken +## de kaart + +voor de kaart worden de volgende afmetingen aangehouden: -- Welke software wordt er gebruikt om de kaart te maken? - (moet met exacte afstanden kunnen werken, lieft in centimeters; als het kan - ook fijn om in samen te werken) - - Figma? - - Adobe Illustrator? - - MS Visio? +- 3/4" (~19mm) breedte +- gebogen lijnen niet scherper dan een radius van 3" (~750mm) +- het oplaadstation is een zwarte stip met een diameter van 3" (~750mm) +- paginamarge en minimale ruimte tussen lijnen van 3" (~750mm) +- (binnen grid) tegelgrootte van 8" (~20cm) ## noob hoek @@ -156,7 +149,3 @@ let wel op dat je **MSYS2 MinGW x64** moet gebruiken als terminal wanneer je `make` wil gebruiken. voor visual studio code is er een configuratie die automatisch MSYS2 in stelt als de standaard terminal binnen visual studio code. als je een losse terminal wil gebruiken moet je hier dus wel op letten - -test - - 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/consts.h b/robot/consts.h deleted file mode 100644 index a81908d..0000000 --- a/robot/consts.h +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#ifndef W2_BUILD_STR -// is defined by CFLAGS += -DW2_BUILD_STR in makefile -#define W2_BUILD_STR ("????????") -#endif - -#define W2_MAX_MODULE_CYCLE_MS (20) -#define W2_SERIAL_BAUD (9600) -#define W2_ERROR_BUFFER_SIZE (16) - -#define W2_ERR_TYPE_CRIT (0b00 << 6) -#define W2_ERR_TYPE_WARN (0b01 << 6) -#define W2_ERR_TYPE_INFO (0b10 << 6) -#define W2_ERR_TYPE_VERB (0b11 << 6) -#define W2_ERR_TYPE_MASK (0b11 << 6) - -/** - * enum storing all error codes - * - * error codes are between 0-63 because the two most significant bits are - * reserved for error type checking - */ -enum w2_e_errorcodes { - // critical error codes - W2_ERR_CONN_LOST = 0x00 | W2_ERR_TYPE_CRIT, - W2_ERR_COM_UNAVAILABLE = 0x01 | W2_ERR_TYPE_CRIT, // client-only - W2_ERR_LINE_LOST = 0x02 | W2_ERR_TYPE_CRIT, - W2_ERR_OBSTACLE_STUCK = 0x03 | W2_ERR_TYPE_CRIT, - - // warnings - W2_ERR_BATTERY_LOW = 0x00 | W2_ERR_TYPE_WARN, - W2_ERR_OBSTACLE_DETECTED = 0x01 | W2_ERR_TYPE_WARN, - W2_ERR_CYCLE_EXPIRED = 0x02 | W2_ERR_TYPE_WARN, - W2_ERR_UNCAUGHT_ERROR = 0x03 | W2_ERR_TYPE_WARN, -}; diff --git a/robot/errcatch.c b/robot/errcatch.c index d5ad320..8df90b8 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -1,61 +1,82 @@ -#include <stdio.h> #include <stdlib.h> #include <string.h> -#include "consts.h" #include "errcatch.h" -#include "halt.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; +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_index != g_w2_error_offset) { - w2_s_error *error = &g_w2_error_buffer[g_w2_error_offset]; + 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); - free(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(enum w2_e_errorcodes code, uint16_t length, const char *message) { - w2_s_error *error = calloc(sizeof(w2_s_error) + length, 1); +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(enum w2_e_errorcodes code) { w2_errcatch_throw_msg(code, 0, ""); } -void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const char *message) { - w2_s_error error = *w2_alloc_error(code, length, message); +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; - g_w2_error_index = (g_w2_error_index + 1) % W2_ERROR_BUFFER_SIZE; + 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_ERR_TYPE_MASK; + uint8_t severity = error->code & W2_E_TYPE_MASK; // trigger emergency mode for critical errors - if ((severity ^ W2_ERR_TYPE_CRIT) == 0) g_w2_current_mode = &w2_mode_halt; + if ((severity ^ W2_E_TYPE_CRIT) == 0) w2_modes_call(W2_M_HALT); // TODO: handle more error types switch (error->code) { - case W2_ERR_UNCAUGHT_ERROR: { + case W2_E_WARN_UNCAUGHT_ERROR: { break; } default: { - w2_errcatch_throw(W2_ERR_UNCAUGHT_ERROR); + g_w2_error_uncaught = 1; #ifdef W2_SIM - simwarn("Uncaught/unhandled error found with code 0x%02x", error->code); + simwarn("Uncaught/unhandled error found with code 0x%02x\n", error->code); #endif } } - // TODO: forward error to sercomm + // 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; + 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 index 2297886..836da1b 100644 --- a/robot/errcatch.h +++ b/robot/errcatch.h @@ -2,22 +2,11 @@ #include <stdint.h> -#include "consts.h" - -/** - * error struct - * - * holds an error with type `code`, and an optional `message` with length - * `message_length` - */ -typedef struct { - enum w2_e_errorcodes code; - uint8_t message_length; - char message[]; -} w2_s_error; +#include "../shared/consts.h" +#include "../shared/errors.h" /** error ring buffer */ -extern w2_s_error g_w2_error_buffer[W2_ERROR_BUFFER_SIZE]; +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 */ @@ -30,14 +19,14 @@ void w2_errcatch_main(); void w2_errcatch_handle_error(w2_s_error *error); /** append error to error buffer */ -void w2_errcatch_throw(enum w2_e_errorcodes code); +void w2_errcatch_throw(w2_e_errorcode code); /** append error to error buffer (with debug message) */ -void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const char *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 calloc + * TODO: doesn't handle null pointers from malloc */ -w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const char *message); +w2_s_error *w2_alloc_error(w2_e_errorcode code, uint16_t length, const char *message); 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/halt.c b/robot/halt.c deleted file mode 100644 index 2f159f0..0000000 --- a/robot/halt.c +++ /dev/null @@ -1,5 +0,0 @@ -#include <stdbool.h> - -#include "halt.h" - -void w2_mode_halt() { return; } diff --git a/robot/hypervisor.c b/robot/hypervisor.c index 6b32776..0baa406 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -1,12 +1,25 @@ #include "hypervisor.h" -#include "consts.h" +#include "../shared/util.h" #include "errcatch.h" #include "io.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" +uint64_t g_w2_hypervisor_cycles = 0; +uint64_t g_w2_hypervisor_uptime_ms = 0; +unsigned long g_w2_hypervisor_ema_sercomm_ms = 0; +unsigned long g_w2_hypervisor_ema_errcatch_ms = 0; +unsigned long g_w2_hypervisor_ema_io_ms = 0; +unsigned long g_w2_hypervisor_ema_mode_ms = 0; + void w2_hypervisor_main() { +#ifdef W2_SIM + w2_sim_cycle_begin(); + if (DBG_ENABLE_CYCLEINFO) siminfo("cycle start\n"); +#endif + + g_w2_hypervisor_uptime_ms += get_ms(); time_reset(); w2_sercomm_main(); @@ -18,14 +31,23 @@ void w2_hypervisor_main() { w2_modes_main(); unsigned long mode_time = get_ms() - io_time; + // calculate exponential moving averages + g_w2_hypervisor_ema_sercomm_ms = + w2_util_exp_mov_avg(g_w2_hypervisor_ema_sercomm_ms, sercomm_time); + g_w2_hypervisor_ema_errcatch_ms = + w2_util_exp_mov_avg(g_w2_hypervisor_ema_errcatch_ms, errcatch_time); + g_w2_hypervisor_ema_io_ms = w2_util_exp_mov_avg(g_w2_hypervisor_ema_io_ms, io_time); + g_w2_hypervisor_ema_mode_ms = w2_util_exp_mov_avg(g_w2_hypervisor_ema_mode_ms, mode_time); + + if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_E_WARN_CYCLE_EXPIRED); + #ifdef W2_SIM - siminfo("sercomm: %lums\n", sercomm_time); - siminfo("errcatch: %lums\n", errcatch_time); - siminfo("io: %lums\n", io_time); - siminfo("mode: %lums\n", mode_time); + if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n"); + if (!g_w2_sim_headless) usleep(100e3); - usleep(100e3); + if (g_w2_sim_headless && DBG_MAX_CYCLES > -1 && g_w2_hypervisor_cycles > DBG_MAX_CYCLES) + exit(0); #endif - if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_ERR_CYCLE_EXPIRED); + g_w2_hypervisor_cycles++; } diff --git a/robot/hypervisor.h b/robot/hypervisor.h index 0e73259..5008c8f 100644 --- a/robot/hypervisor.h +++ b/robot/hypervisor.h @@ -1,5 +1,15 @@ #pragma once +#include <stdint.h> + +extern uint64_t g_w2_hypervisor_cycles; +extern uint64_t g_w2_hypervisor_uptime_ms; + +extern unsigned long g_w2_hypervisor_ema_sercomm_ms; +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; + /** * backbone of all other modules * diff --git a/robot/main.c b/robot/main.c index fbfd38b..d76dbaf 100644 --- a/robot/main.c +++ b/robot/main.c @@ -1,8 +1,15 @@ #include "main.h" #include "hypervisor.h" #include "setup.h" +#ifdef W2_SIM +#include "sim.h" +#endif + +int main(int argc, char **argv) { +#ifdef W2_SIM + w2_sim_setup(argc, argv); +#endif -int main() { w2_setup_main(); for (;;) w2_hypervisor_main(); diff --git a/robot/makefile b/robot/makefile index 6f50519..4039ae3 100644 --- a/robot/makefile +++ b/robot/makefile @@ -10,8 +10,11 @@ PORT ?= /dev/ttyACM0 CFLAGS=-g -Wall $(DEVICE_SPECIFIC_CFLAGS) -Os LDFLAGS=-Wl,-gc-sections -Wl,-relax +all: $(if $(SIM), a.out, out.hex) + SOURCES := $(filter-out sim.c, $(wildcard *.c)) HEADERS := $(filter-out sim.h, $(wildcard *.h)) +include ../shared/makefile # simulation # SIM = true @@ -29,12 +32,10 @@ OBJ2HEX=$(PREFIX)objcopy # debug build info string BUILD_STR=$(shell git update-index -q --refresh; git describe --tags --dirty='*' --broken='x' | cut -c1-20) -CFLAGS += -DW2_BUILD_STR="$(BUILD_STR)" - -all: $(if $(SIM), a.out, out.hex) +CFLAGS += -DW2_BUILD_STR=\"$(BUILD_STR)\" -clean: - rm -f *.o out.hex a.out compile_commands.json +clean:: + rm -f *.o out.hex a.out a.out: $(OBJECTS) $(CC) $(OBJECTS) $(CFLAGS) $(LDFLAGS) 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/mode_chrg.c b/robot/mode_chrg.c new file mode 100644 index 0000000..81808d5 --- /dev/null +++ b/robot/mode_chrg.c @@ -0,0 +1,3 @@ +#include "mode_chrg.h" + +void w2_mode_chrg() {} diff --git a/robot/mode_chrg.h b/robot/mode_chrg.h new file mode 100644 index 0000000..d9b5cc0 --- /dev/null +++ b/robot/mode_chrg.h @@ -0,0 +1,9 @@ +#pragma once + +/** + * charge station mode + * + * go to the charging station transition in the grid, and continue until a + * black circle is found + */ +void w2_mode_chrg(); diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c new file mode 100644 index 0000000..0bbf3cb --- /dev/null +++ b/robot/mode_dirc.c @@ -0,0 +1,7 @@ +#include "mode_dirc.h" +#include "orangutan_shim.h" + +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 new file mode 100644 index 0000000..5b9bbf4 --- /dev/null +++ b/robot/mode_dirc.h @@ -0,0 +1,13 @@ +#pragma once + +#include <stdint.h> + +extern int16_t g_w2_mode_dirc_motor_l; +extern int16_t g_w2_mode_dirc_motor_r; + +/** + * direct control mode + * + * respond to DIRC commands + */ +void w2_mode_dirc(); 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/grid.h b/robot/mode_grid.h index fcf9100..fcf9100 100644 --- a/robot/grid.h +++ b/robot/mode_grid.h 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/halt.h b/robot/mode_halt.h index d92905e..d92905e 100644 --- a/robot/halt.h +++ b/robot/mode_halt.h 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/calibration.h b/robot/mode_lcal.h index 5c1af38..dd373f0 100644 --- a/robot/calibration.h +++ b/robot/mode_lcal.h @@ -6,4 +6,4 @@ * 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(); +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/maze.h b/robot/mode_maze.h index 9fbeb8c..9fbeb8c 100644 --- a/robot/maze.h +++ b/robot/mode_maze.h diff --git a/robot/mode_scal.c b/robot/mode_scal.c new file mode 100644 index 0000000..e3a9c97 --- /dev/null +++ b/robot/mode_scal.c @@ -0,0 +1,3 @@ +#include "mode_scal.h" + +void w2_mode_scal() {} diff --git a/robot/mode_scal.h b/robot/mode_scal.h new file mode 100644 index 0000000..4f1f04d --- /dev/null +++ b/robot/mode_scal.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * sensor calibration mode + * + * calibrate underside uv sensors + */ +void w2_mode_scal(); diff --git a/robot/mode_spin.c b/robot/mode_spin.c new file mode 100644 index 0000000..9145eb3 --- /dev/null +++ b/robot/mode_spin.c @@ -0,0 +1,3 @@ +#include "mode_spin.h" + +void w2_mode_spin() {} diff --git a/robot/mode_spin.h b/robot/mode_spin.h new file mode 100644 index 0000000..926137e --- /dev/null +++ b/robot/mode_spin.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * wet floor simulation + * + * spin uncontrollably (simulating wet floor??) + */ +void w2_mode_spin(); diff --git a/robot/modes.c b/robot/modes.c index c22875c..8d3a099 100644 --- a/robot/modes.c +++ b/robot/modes.c @@ -1,6 +1,44 @@ +#include <stdbool.h> + +#include "../shared/util.h" +#include "errcatch.h" #include "modes.h" -#include "halt.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; + +void w2_modes_main() { (*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 = + g_w2_mode_history_index + (new_mode == W2_M_PREV ? -1 : 1) * (replace - 1); + if (next_history_index == -1 || next_history_index == W2_MODE_HISTORY_BUFFER_SIZE - 1) { + next_history_index = W2_RANGE(0, next_history_index, W2_MODE_HISTORY_BUFFER_SIZE); + w2_errcatch_throw(W2_E_WARN_MODE_HISTORY_BUFFER_IOB); + } + + if (new_mode == W2_M_PREV) { + 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]; + } + + // 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); +} -void (*g_w2_current_mode)() = &w2_mode_halt; +void w2_modes_call(w2_e_mode mode) { w2_modes_switch(mode, false); } -void w2_modes_main() { (*g_w2_current_mode)(); } +void w2_modes_swap(w2_e_mode mode) { w2_modes_switch(mode, true); } diff --git a/robot/modes.h b/robot/modes.h index dd34690..3423a0f 100644 --- a/robot/modes.h +++ b/robot/modes.h @@ -1,6 +1,15 @@ #pragma once -extern void (*g_w2_current_mode)(); +#include "../shared/consts.h" + +#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" /** * mode logic @@ -8,3 +17,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, + &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) */ +void w2_modes_swap(w2_e_mode mode); diff --git a/robot/orangutan_shim.h b/robot/orangutan_shim.h index de57c98..cd624d0 100644 --- a/robot/orangutan_shim.h +++ b/robot/orangutan_shim.h @@ -3,5 +3,8 @@ #ifdef W2_SIM #include "sim.h" #else +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" #include <pololu/orangutan.h> +#pragma GCC diagnostic pop #endif diff --git a/robot/readme.md b/robot/readme.md index 4a7aca3..eb1dd37 100644 --- a/robot/readme.md +++ b/robot/readme.md @@ -52,17 +52,21 @@ handling module, which then both handles the error and forwards it to pc communication for logging purposes). here's a quick run-down of all modules and what they're supposed to do: -|module |internal name|author|purpose| -|----------------|-------------|-|-| -|hypervisor |`hypervisor `|N/a| backbone of all other modules; stores global variables; controls when other modules run| -|pc communication|`sercomm `|Fiona| reads and parses incoming serial data; sends all data in the message buffer| -|error handling |`errcatch `|Loek| receives error codes; controls how errors are handled| -|i/o read & write|`io `|Jorn & Abdullaahi| reads all inputs to global state; writes all outputs| -|mode logic |`modes `|N/a| executes the appropriate module for current mode| -|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| +|module |internal name|due|author|purpose| +|------------------|-------------|-|-|-| +|hypervisor |`hypervisor `|done|N/a| backbone of all other modules; stores global variables; controls when other modules run| +|pc communication |`sercomm `|may 27|Loek| reads and parses incoming serial data; sends all data in the message buffer| +|error handling |`errcatch `|done|Loek| receives error codes; controls how errors are handled| +|i/o read & write |`io `|may 27|Jorn & Abdullaahi| reads all inputs to global state; writes all outputs| +|mode logic |`modes `|done|N/a| executes the appropriate module for current mode| +|maze |`mode_maze `|may 31|Jorn & Abdullaahi| controls robot during maze portion of map; hands off control to warehouse module| +|warehouse |`mode_grid `|may 31|Loek| controls robot during warehouse portion of map; hands off control to maze module| +|emergency stop |`mode_halt `|may 31|Fiona| stops all execution until emergency mode is reset by software or user| +|line finding |`mode_lcal `|may 31|Fiona| find line by turning on own axis if lost| +|charge station |`mode_chrg `|may 31|Fiona| go to the charging station transition in the grid, and continue until a black circle is found| +|direct control |`mode_dirc `|may 31|Loek| respond to [DIRC](../protocol.md#DIRC) commands| +|wet floor |`mode_spin `|may 31|Fiona| spin uncontrollably (simulating wet floor??)| +|sensor calibration|`mode_scal `|may 31|Jorn & Abdullaahi| calibrate underside uv sensors| ## some standards @@ -100,16 +104,18 @@ this list will probably get updated from time to time: global todo: -- [ ] add test/simulation mode for wet floor (spinning) -- [ ] add a manual control mode - [ ] start robot in calibration mode - [ ] assume robot starts in maze -- [ ] maze-grid transition detection in seperate file (used by grid and maze +- [ ] 'crosswalk' transition detection in seperate file (used by grid and maze mode) -- [ ] clear global timer at start of cycle instead of just for mode selection +- [ ] client software architecture +- [x] mode 'return' buffer +- [x] clear global timer at start of cycle instead of just for mode selection 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 a0eed3a..2786b85 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -1,3 +1,133 @@ +#include <stdlib.h> +#include <string.h> + +#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" -void w2_sercomm_main() {} +w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_BUFFER_SIZE] = {0}; +uint8_t g_w2_sercomm_index = 0; +uint8_t g_w2_sercomm_offset = 0; +uint8_t g_w2_sercomm_buffer_full = 0; + +char g_w2_serial_buffer[W2_SERIAL_READ_BUFFER_SIZE] = {0}; +uint8_t g_w2_serial_buffer_index = 0; +uint8_t g_w2_serial_buffer_head = 0; + +void w2_sercomm_main() { +#ifdef W2_SIM + simprintfunc("w2_sercomm_main", ""); +#endif + // read and parse data + while (serial_get_received_bytes() != g_w2_serial_buffer_index) { + 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; + } + + // send data + while (g_w2_sercomm_offset != g_w2_sercomm_index) { + w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_offset]; + char *data_cast = malloc(data->bytes); + memcpy(data_cast, data->data, data->bytes); + serial_send(data_cast, data->bytes); + g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE; + } +} + +void w2_sercomm_append_msg(w2_s_bin *data) { +#ifdef W2_SIM + simprintfunc("w2_sercomm_append_msg", ""); +#endif + 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]); + 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; +} + +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_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); +} + +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); + + 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; } + +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) { + 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); +} + +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; } + +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/sercomm.h b/robot/sercomm.h index 58c79b9..b1f69c7 100644 --- a/robot/sercomm.h +++ b/robot/sercomm.h @@ -1,5 +1,23 @@ #pragma once +#include "../shared/bin.h" +#include "../shared/consts.h" +#include "../shared/protocol.h" + +/** sercomm ring buffer */ +extern w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_BUFFER_SIZE]; +/** stores head of ring buffer */ +extern uint8_t g_w2_sercomm_index; +/** stores start of ring buffer */ +extern uint8_t g_w2_sercomm_offset; + +/** serial input (receive) buffer */ +extern char g_w2_serial_buffer[W2_SERIAL_READ_BUFFER_SIZE]; +/** serial input (receive) buffer current position */ +extern uint8_t g_w2_serial_buffer_index; +/** serial input (receive) buffer head (sim only) */ +extern uint8_t g_w2_serial_buffer_head; + /** * serial pc-robot communication module * @@ -7,3 +25,6 @@ * - sends all data in the message buffer */ void w2_sercomm_main(); + +/** append binary message to send buffer */ +void w2_sercomm_append_msg(w2_s_bin *data); diff --git a/robot/setup.c b/robot/setup.c index c74cca9..06c8fa4 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -1,13 +1,20 @@ #include <stdlib.h> -#include "consts.h" -#include "halt.h" +#include "../shared/bin.h" +#include "../shared/consts.h" #include "modes.h" #include "orangutan_shim.h" +#include "sercomm.h" #include "setup.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_setup_main() { - serial_set_baud_rate(W2_SERIAL_BAUD); + // check endianness + g_w2_endianness = *_ptest; // reset underside leds red_led(0); @@ -16,6 +23,17 @@ void w2_setup_main() { // clear lcd 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); + + // reset timer + time_reset(); + + // set default mode + w2_modes_swap(W2_M_HALT); + // indicate startup done play("L50 c>c"); } diff --git a/robot/sim.c b/robot/sim.c index 6bd5838..ddc208a 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -1,15 +1,44 @@ #include <stdio.h> #include <time.h> #include <string.h> +#include <stdint.h> +#include <termios.h> +#include <unistd.h> #include "sim.h" +#include "../shared/consts.h" +#include "../shared/protocol.h" +#include "sercomm.h" +#include "errcatch.h" struct timespec reference_time; // NOLINT +bool g_w2_sim_headless = false; + +static const char* const W2_CMD_NAMES[] = { + "PING", + "EXPT", + "MODE", + "SPED", + "DIRC", + "CORD", + "BOMD", + "SRES", + "MCFG", + "SENS", + "INFO", + "DISP", + "PLAY", + "CLED", +}; + +static const char* const W2_CMD_DIRECTIONS[] = { + "RX", + "TX", +}; void time_reset() { simprintfunc("time_reset", ""); clock_gettime(CLOCK_MONOTONIC, &reference_time); - return; } unsigned long get_ms() { @@ -22,43 +51,78 @@ 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) { - simprintfunc("serial_send", "<see below>, %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]); - bytes++; + if (g_w2_sim_headless) { + for (unsigned int byte = 0; byte < length; byte++) + putc(message[byte] & 0xff, stdout); + return; } + + if (DBG_ENABLE_PRINTFUNC) simprintfunc("serial_send", "<see below>, %u", length); + + if (!DBG_ENABLE_SERIAL) 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); +} + +unsigned char serial_get_received_bytes() { + simprintfunc("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; + + // disable echo and enable raw mode + struct termios term; + tcgetattr(STDIN_FILENO, &term); + term.c_lflag &= ~(ECHO | ICANON); + term.c_cc[VTIME] = 0; + term.c_cc[VMIN] = 0; + tcsetattr(STDIN_FILENO, 0, &term); + + // debug error + // w2_errcatch_throw(W2_E_WARN_BATTERY_LOW); +} + +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; +} + +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"); - return; } +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 15a1b4b..a595042 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -3,6 +3,20 @@ #include <stdio.h> #include <stdlib.h> #include <unistd.h> +#include <stdbool.h> + +#include "../shared/bin.h" + +extern bool g_w2_sim_headless; + +// debug fine-tuning +#define DBG_ENABLE_PRINTFUNC (1) +#define DBG_ENABLE_SIMWARN (1) +#define DBG_ENABLE_SIMINFO (1) +#define DBG_ENABLE_CYCLEINFO (0) +#define DBG_ENABLE_SERIAL (1) + +#define DBG_MAX_CYCLES (10) // debug print options #define DBG_BYTES_PER_LINE 16 @@ -19,12 +33,12 @@ #define COL_RST "\e[0m" // debug stdout print macros -#define simprintf(message, ...) printf(COL_RED "[SIM] " COL_RST message, ##__VA_ARGS__) +#define simprintf(message, ...) if (!g_w2_sim_headless) printf(COL_RED "[SIM] " COL_RST message, ##__VA_ARGS__) #define simprint(message) simprintf(message "\n") -#define simprintfunc(name, fmt, ...) simprintf(COL_BLU "[FUNC] " \ - COL_CYN name COL_RST "(" COL_YEL fmt COL_RST ")\n", ##__VA_ARGS__) -#define simwarn(message, ...) simprintf(COL_YEL "[WARN] " COL_RST message, ##__VA_ARGS__) -#define siminfo(message, ...) simprintf(COL_MAG "[INFO] " COL_RST message, ##__VA_ARGS__) +#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__); } +#define simwarn(message, ...) if (DBG_ENABLE_SIMWARN) { simprintf(COL_YEL "[WARN] " COL_RST message, ##__VA_ARGS__); } +#define siminfo(message, ...) if (DBG_ENABLE_SIMINFO) { simprintf(COL_MAG "[INFO] " COL_RST message, ##__VA_ARGS__); } /** * simulates pololu library functions for local testing @@ -35,6 +49,14 @@ unsigned long get_ms(); // NOLINT void red_led(unsigned char on); // NOLINT void green_led(unsigned char on); // NOLINT void clear(); // NOLINT -void play(const char* melody); // 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(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 Binary files differnew file mode 100644 index 0000000..1aea35c --- /dev/null +++ b/robot/tests/dirc.bin 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/mcfg.bin b/robot/tests/mcfg.bin Binary files differnew file mode 100644 index 0000000..a43ecf8 --- /dev/null +++ b/robot/tests/mcfg.bin diff --git a/robot/tests/mode.bin b/robot/tests/mode.bin new file mode 100644 index 0000000..9cd9175 --- /dev/null +++ b/robot/tests/mode.bin @@ -0,0 +1 @@ +ÿ
\ No newline at end of file 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/robot/tests/ping.bin b/robot/tests/ping.bin Binary files differnew file mode 100644 index 0000000..456804e --- /dev/null +++ b/robot/tests/ping.bin 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 + 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/bin.c b/shared/bin.c new file mode 100644 index 0000000..4b3dcc6 --- /dev/null +++ b/shared/bin.c @@ -0,0 +1,80 @@ +#include <stdlib.h> +#include <string.h> + +#include "bin.h" + +#define W2_ENDIAN_LITTLE (1) +#define W2_ENDIAN_BIG (0) + +#define _SHIFT_0B (8 * 0) +#define _SHIFT_1B (8 * 1) +#define _SHIFT_2B (8 * 2) +#define _SHIFT_3B (8 * 3) +#define _BYTE_0 ((uint32_t)(0xff << (_SHIFT_0B))) +#define _BYTE_1 ((uint32_t)(0xff << (_SHIFT_1B))) +#define _BYTE_2 ((uint32_t)(0xff << (_SHIFT_2B))) +#define _BYTE_3 ((uint32_t)(0xff << (_SHIFT_3B))) + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wshift-count-overflow" +w2_s_bin *w2_bin_from_uint8_t(uint8_t data) { + size_t size = 1; + w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size); + ret->bytes = size; + ret->data[0] = data; + return ret; +} + +w2_s_bin *w2_bin_from_uint16_t(uint16_t data) { + size_t size = 2; + w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size); + data = w2_bin_hton16(data); + ret->bytes = size; + ret->data[0] = (data & _BYTE_1) >> _SHIFT_1B; + ret->data[1] = (data & _BYTE_0) >> _SHIFT_0B; + return ret; +} + +w2_s_bin *w2_bin_from_uint32_t(uint32_t data) { + size_t size = 4; + w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size); + data = w2_bin_hton32(data); + ret->bytes = size; + ret->data[0] = (data & _BYTE_3) >> _SHIFT_3B; + ret->data[1] = (data & _BYTE_2) >> _SHIFT_2B; + ret->data[2] = (data & _BYTE_1) >> _SHIFT_1B; + ret->data[3] = (data & _BYTE_0) >> _SHIFT_0B; + return ret; +} + +uint32_t w2_bin_hton32(uint32_t h32) { + if (g_w2_endianness == W2_ENDIAN_BIG) return h32; + return ((h32 & _BYTE_0) << _SHIFT_3B) | ((h32 & _BYTE_1) << _SHIFT_1B) | + ((h32 & _BYTE_2) >> _SHIFT_1B) | ((h32 & _BYTE_3) >> _SHIFT_3B); +} +#pragma GCC diagnostic pop + +uint16_t w2_bin_hton16(uint16_t h16) { + if (g_w2_endianness == W2_ENDIAN_BIG) return h16; + return ((h16 & _BYTE_0) << _SHIFT_1B) | ((h16 & _BYTE_1) >> _SHIFT_1B); +} + +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; +} + +w2_s_bin *w2_bin_s_cat(w2_s_bin *a, w2_s_bin *b) { + uint8_t data[a->bytes + b->bytes]; + memcpy(data, a->data, a->bytes); + memcpy(data + a->bytes, b->data, b->bytes); + w2_s_bin *c = w2_bin_s_alloc(a->bytes + b->bytes, data); + free(a); + free(b); + return c; +} diff --git a/shared/bin.h b/shared/bin.h new file mode 100644 index 0000000..2b65c95 --- /dev/null +++ b/shared/bin.h @@ -0,0 +1,34 @@ +#pragma once +/** + * helper file for binary data + * + * - fix endianness with functions inspired by UNIX arpa/inet.h + * - convert uint16_t and uint32_t to w2_s_bin + */ + +#include <stdint.h> + +extern uint8_t g_w2_endianness; + +typedef struct { + uint16_t bytes; + 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); +/** concatenate 2 w2_s_bin structs, deallocates `a` and `b` */ +w2_s_bin *w2_bin_s_cat(w2_s_bin *a, w2_s_bin *b); + +w2_s_bin *w2_bin_from_uint8_t(uint8_t data); +w2_s_bin *w2_bin_from_uint16_t(uint16_t data); +w2_s_bin *w2_bin_from_uint32_t(uint32_t data); + +/** convert 32-bit value from host endian to network (big-endian) */ +uint32_t w2_bin_hton32(uint32_t h32); +/** convert 16-bit value from host endian to network (big-endian) */ +uint16_t w2_bin_hton16(uint16_t h16); +/** convert 32-bit value from network (big-endian) to host endian */ +uint32_t w2_bin_ntoh32(uint32_t n32); +/** convert 16-bit value from network (big-endian) to host endian */ +uint16_t w2_bin_ntoh16(uint16_t n16); diff --git a/shared/consts.h b/shared/consts.h new file mode 100644 index 0000000..25ca94f --- /dev/null +++ b/shared/consts.h @@ -0,0 +1,21 @@ +#pragma once + +#ifndef W2_BUILD_STR +// is defined by CFLAGS += -DW2_BUILD_STR in makefile +#define W2_BUILD_STR ("????????") +#endif + +/** max logic module execution time in milliseconds */ +#define W2_MAX_MODULE_CYCLE_MS (20) +/** serial baud rate (bit/s) */ +#define W2_SERIAL_BAUD (9600) +/** size of the error handling buffer (in errors, not bytes) */ +#define W2_ERROR_BUFFER_SIZE (16) +/** size of the serial communication buffer (in messages, not bytes) */ +#define W2_SERCOMM_BUFFER_SIZE (16) +/** size of input (receive) buffer (in bytes) */ +#define W2_SERIAL_READ_BUFFER_SIZE (255) +/** exponential moving average new measurement weight (double 0-1) */ +#define W2_EMA_WEIGHT (0.10) +/** size of mode history buffer */ +#define W2_MODE_HISTORY_BUFFER_SIZE (4) diff --git a/shared/errors.h b/shared/errors.h new file mode 100644 index 0000000..ac8e95f --- /dev/null +++ b/shared/errors.h @@ -0,0 +1,64 @@ +#pragma once + +#include <stdint.h> + +#define W2_E_TYPE_MASK (0b11 << 6) + +#define W2_E_TYPE_CRIT (0b00 << 6) +#define W2_E_TYPE_WARN (0b01 << 6) +#define W2_E_TYPE_INFO (0b10 << 6) +#define W2_E_TYPE_VERB (0b11 << 6) + +/** + * enum storing all error codes + * + * error codes are between 0-63 because the two most significant bits are + * reserved for error type checking + */ +typedef enum { + /** wireless connection lost from either robot or client-side */ + W2_E_CRIT_CONN_LOST = 0x00 | W2_E_TYPE_CRIT, + /** serial COM-port unavalable. client-side only */ + W2_E_CRIT_COM_UNAVAILABLE = 0x01 | W2_E_TYPE_CRIT, + /** line unable to be found automatically */ + W2_E_CRIT_LINE_LOST = 0x02 | W2_E_TYPE_CRIT, + /** obstacle unavoidable, robot stuck */ + W2_E_CRIT_OBSTACLE_STUCK = 0x03 | W2_E_TYPE_CRIT, + /** semver major version doesn't match */ + W2_E_CRIT_VERSION_INCOMPATIBLE = 0x04 | W2_E_TYPE_CRIT, + + /** battery low, returning to charging station */ + W2_E_WARN_BATTERY_LOW = 0x00 | W2_E_TYPE_WARN, + /** obstacle detected, waiting then trying other route */ + W2_E_WARN_OBSTACLE_DETECTED = 0x01 | W2_E_TYPE_WARN, + /** logic cycle took longer than `W2_MAX_MODULE_CYCLE_MS` */ + W2_E_WARN_CYCLE_EXPIRED = 0x02 | W2_E_TYPE_WARN, + /** error thrown without handler, gets thrown on next cycle */ + W2_E_WARN_UNCAUGHT_ERROR = 0x03 | W2_E_TYPE_WARN, + /** error buffer full, gets thrown on next cycle */ + W2_E_WARN_ERR_BUFFER_FULL = 0x04 | W2_E_TYPE_WARN, + /** line lost, trying to calibrate */ + W2_E_WARN_LINE_LOST = 0x05 | W2_E_TYPE_WARN, + /** serial buffer full, gets thrown on next cycle */ + W2_E_WARN_SERCOMM_BUFFER_FULL = 0x06 | W2_E_TYPE_WARN, + /** semver minor version doesn't match */ + W2_E_WARN_VERSION_INCOMPATIBLE = 0x07 | W2_E_TYPE_WARN, + /** serial byte took to long to receive */ + W2_E_WARN_SERIAL_TIMEOUT = 0x08 | W2_E_TYPE_WARN, + /** unknown message encountered (noisy channel?) */ + 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, +} w2_e_errorcode; + +/** + * error struct + * + * holds an error with type `code`, and an optional `message` with length + * `message_length` + */ +typedef struct { + w2_e_errorcode code; + uint8_t message_length; + char message[]; +} w2_s_error; diff --git a/shared/makefile b/shared/makefile new file mode 100644 index 0000000..cfdf8ac --- /dev/null +++ b/shared/makefile @@ -0,0 +1,5 @@ +SOURCES += $(wildcard ../shared/*.c) +HEADERS += $(wildcard ../shared/*.h) + +clean:: + rm -f ../shared/*.o diff --git a/shared/protocol.c b/shared/protocol.c new file mode 100644 index 0000000..77ec466 --- /dev/null +++ b/shared/protocol.c @@ -0,0 +1,97 @@ +#include <stdbool.h> + +#include "protocol.h" +#ifdef W2_SIM +#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); + + 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 sizeof(w2_s_cmd_sped_rx); + + 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 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 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 sizeof(w2_s_cmd_sres_rx); + + 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 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_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; +} + +#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); +} + +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_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) { +#ifdef W2_SIM + // TODO throw warning + simwarn("unknown serial message with code 0x%02x\n", data[0]); +#endif + } else { +#ifdef W2_SIM + w2_sim_print_serial(copy); +#endif + handler(copy); + } + + free(copy); +} diff --git a/shared/protocol.h b/shared/protocol.h new file mode 100644 index 0000000..057f67d --- /dev/null +++ b/shared/protocol.h @@ -0,0 +1,219 @@ +#pragma once + +#include <stdint.h> +#include <stdlib.h> + +#include "bin.h" +#include "consts.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) + +#define W2_CMD_COUNT 28 +typedef enum { + /** ping command */ + W2_CMD_PING = 0x00, + /** exception command */ + W2_CMD_EXPT = 0x02, + /** mode command */ + W2_CMD_MODE = 0x04, + /** speed command */ + W2_CMD_SPED = 0x06, + /** direct control command */ + W2_CMD_DIRC = 0x08, + /** coordinate command */ + W2_CMD_CORD = 0x0a, + /** backorder modify command */ + W2_CMD_BOMD = 0x0c, + /** soft reset command */ + W2_CMD_SRES = 0x0e, + /** map config command */ + W2_CMD_MCFG = 0x10, + /** sensor data command */ + W2_CMD_SENS = 0x12, + /** info command */ + W2_CMD_INFO = 0x14, + /** display control command */ + W2_CMD_DISP = 0x16, + /** play midi command */ + W2_CMD_PLAY = 0x18, + /** control leds command */ + W2_CMD_CLED = 0x1a, +} w2_e_scmds; + +#pragma pack(push, 1) + +typedef struct { + uint8_t opcode; + uint8_t id; +} w2_s_cmd_ping_rx; + +typedef struct { + uint8_t opcode; + uint8_t id; +} w2_s_cmd_ping_tx; + +typedef struct { + uint8_t opcode; + uint8_t error; + uint8_t length; + uint8_t message[]; +} w2_s_cmd_expt_tx; + +typedef struct { + uint8_t opcode; + uint8_t mode; +} w2_s_cmd_mode_rx; + +typedef struct { + uint8_t opcode; + uint8_t mode; +} w2_s_cmd_mode_tx; + +typedef struct { + uint8_t opcode; + uint8_t speed; +} w2_s_cmd_sped_rx; + +typedef struct { + uint8_t opcode; + uint16_t left; + uint16_t right; +} w2_s_cmd_dirc_rx; + +typedef struct { + uint8_t opcode; + uint32_t position; + uint8_t orientation; +} w2_s_cmd_cord_rx; + +typedef struct { + uint8_t opcode; + uint32_t position; + uint8_t orientation; +} w2_s_cmd_cord_tx; + +typedef struct { + uint8_t opcode; + uint32_t id; + uint32_t position; +} w2_s_cmd_bomd_rx; + +typedef struct { + uint8_t opcode; + uint32_t id; + uint8_t status; +} w2_s_cmd_bomd_tx; + +typedef struct { + uint8_t opcode; + uint8_t type; +} w2_s_cmd_sres_rx; + +typedef struct { + uint16_t position; + uint8_t kind; +} w2_s_cmd_mcfg_feature; + +typedef struct { + uint8_t opcode; + uint8_t width; + uint8_t height; + uint8_t length; + w2_s_cmd_mcfg_feature features[]; +} w2_s_cmd_mcfg_rx; + +typedef struct { + uint8_t opcode; +} w2_s_cmd_sens_rx; + +typedef struct { + uint8_t opcode; + // TODO: sensor data +} w2_s_cmd_sens_tx; + +typedef struct { + uint8_t opcode; +} w2_s_cmd_info_rx; + +typedef struct { + uint8_t opcode; + uint8_t build_str[32]; + uint8_t errcatch_ms; + uint8_t io_ms; + uint8_t sercomm_ms; + uint8_t mode_ms; + uint32_t uptime_s; +} w2_s_cmd_info_tx; + +typedef struct { +} w2_s_cmd_disp_rx; + +typedef struct { +} w2_s_cmd_play_rx; + +typedef struct { +} w2_s_cmd_cled_rx; + +#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 */ +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_cmd_ping_rx(w2_s_bin *data); +/** handler for ping_tx (on complete message) */ +void w2_cmd_ping_tx(w2_s_bin *data); +/** handler for expt_tx (on complete message) */ +void w2_cmd_expt_tx(w2_s_bin *data); +/** handler for mode_rx (on complete message) */ +void w2_cmd_mode_rx(w2_s_bin *data); +/** handler for mode_tx (on complete message) */ +void w2_cmd_mode_tx(w2_s_bin *data); +/** handler for sped_rx (on complete message) */ +void w2_cmd_sped_rx(w2_s_bin *data); +/** handler for dirc_rx (on complete message) */ +void w2_cmd_dirc_rx(w2_s_bin *data); +/** handler for cord_rx (on complete message) */ +void w2_cmd_cord_rx(w2_s_bin *data); +/** handler for cord_tx (on complete message) */ +void w2_cmd_cord_tx(w2_s_bin *data); +/** handler for bomd_rx (on complete message) */ +void w2_cmd_bomd_rx(w2_s_bin *data); +/** handler for bomd_tx (on complete message) */ +void w2_cmd_bomd_tx(w2_s_bin *data); +/** handler for sres_rx (on complete message) */ +void w2_cmd_sres_rx(w2_s_bin *data); +/** handler for mcfg_rx (on complete message) */ +void w2_cmd_mcfg_rx(w2_s_bin *data); +/** handler for sens_rx (on complete message) */ +void w2_cmd_sens_rx(w2_s_bin *data); +/** handler for sens_tx (on complete message) */ +void w2_cmd_sens_tx(w2_s_bin *data); +/** handler for info_rx (on complete message) */ +void w2_cmd_info_rx(w2_s_bin *data); +/** handler for info_tx (on complete message) */ +void w2_cmd_info_tx(w2_s_bin *data); +/** handler for disp_rx (on complete message) */ +void w2_cmd_disp_rx(w2_s_bin *data); +/** handler for play_rx (on complete message) */ +void w2_cmd_play_rx(w2_s_bin *data); +/** handler for cled_rx (on complete message) */ +void w2_cmd_cled_rx(w2_s_bin *data); + +/** calculate message length for expt_tx (incomplete message) */ +size_t w2_cmd_expt_tx_sizeof(w2_s_bin *data); +/** calculate message length for mcfg_rx (incomplete message) */ +size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data); diff --git a/shared/readme.md b/shared/readme.md new file mode 100644 index 0000000..870f015 --- /dev/null +++ b/shared/readme.md @@ -0,0 +1,8 @@ +# shared code + +this is the subdirectory for all code that is shared between the robot code and +the client code. to use these, include the .h files with a relative path (e.g. +`#include "../shared/consts.h"`). makefiles should add `include +../shared/makefile` to add the .c and .h files to `$SOURCES` and `$HEADERS` in +the makefile targets (this is already done for the robot and client +subdirectories). diff --git a/shared/semver.c b/shared/semver.c new file mode 100644 index 0000000..07a7057 --- /dev/null +++ b/shared/semver.c @@ -0,0 +1,9 @@ +#include "semver.h" + +#include <stdio.h> + +w2_semver_t w2_semver_parse(const char *str, uint8_t length) { + w2_semver_t version; + sscanf(str, "%d.%d.%d", (int *)&version.major, (int *)&version.minor, (int *)&version.patch); + return version; +} diff --git a/shared/semver.h b/shared/semver.h new file mode 100644 index 0000000..a99ae61 --- /dev/null +++ b/shared/semver.h @@ -0,0 +1,11 @@ +#pragma once + +#include <stdint.h> + +typedef struct { + uint8_t major; + uint8_t minor; + uint8_t patch; +} w2_semver_t; + +w2_semver_t w2_semver_parse(const char *str, uint8_t length); diff --git a/shared/serial_parse.c b/shared/serial_parse.c new file mode 100644 index 0000000..89c5809 --- /dev/null +++ b/shared/serial_parse.c @@ -0,0 +1,44 @@ +#include <stdbool.h> +#include <string.h> + +#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; + 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; + + complete_message_length = w2_cmd_sizeof(current_message, current_message_index); + + if (current_message_index == complete_message_length) { + w2_cmd_handler(current_message, current_message_index); + + 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..a1c8fb9 --- /dev/null +++ b/shared/serial_parse.h @@ -0,0 +1,8 @@ +#pragma once + +#include <stdint.h> + +#include "protocol.h" + +/** parse serial data byte by byte */ +void w2_serial_parse(uint8_t byte); diff --git a/shared/util.c b/shared/util.c new file mode 100644 index 0000000..55f3491 --- /dev/null +++ b/shared/util.c @@ -0,0 +1,6 @@ +#include "consts.h" + +unsigned long w2_util_exp_mov_avg(unsigned long current_avg, unsigned long new_meas) { + return (unsigned long)((((double)(current_avg)) * ((double)(1.f - W2_EMA_WEIGHT))) + + (((double)(new_meas)) * ((double)(W2_EMA_WEIGHT)))); +} diff --git a/shared/util.h b/shared/util.h new file mode 100644 index 0000000..9e4d8ac --- /dev/null +++ b/shared/util.h @@ -0,0 +1,7 @@ +#pragma once + +#define W2_MIN(a, b) (((a) < (b)) ? (a) : (b)) +#define W2_MAX(a, b) (((a) > (b)) ? (a) : (b)) +#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); |