diff options
| -rw-r--r-- | robot/consts.h | 5 | ||||
| -rw-r--r-- | robot/errcatch.c | 58 | ||||
| -rw-r--r-- | robot/errcatch.h | 29 | ||||
| -rw-r--r-- | robot/hypervisor.c | 26 | ||||
| -rw-r--r-- | robot/makefile | 28 | ||||
| -rw-r--r-- | robot/orangutan_shim.h | 7 | ||||
| -rw-r--r-- | robot/readme.md | 37 | ||||
| -rw-r--r-- | robot/setup.c | 2 | ||||
| -rw-r--r-- | robot/sim.c | 64 | ||||
| -rw-r--r-- | robot/sim.h | 40 |
10 files changed, 266 insertions, 30 deletions
diff --git a/robot/consts.h b/robot/consts.h index 1195422..a81908d 100644 --- a/robot/consts.h +++ b/robot/consts.h @@ -1,17 +1,19 @@ #pragma once #ifndef W2_BUILD_STR -// should be defined with -DBUILD_STR in makefile +// 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 @@ -30,4 +32,5 @@ enum w2_e_errorcodes { 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 5b42bb4..d5ad320 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -1,7 +1,61 @@ +#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" + +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; -void w2_errcatch_main() {} +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]; + w2_errcatch_handle_error(error); + free(error); + g_w2_error_offset = (g_w2_error_offset + 1) % W2_ERROR_BUFFER_SIZE; + } +} -void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const char *message) {} +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); + 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); + g_w2_error_buffer[g_w2_error_index] = error; + g_w2_error_index = (g_w2_error_index + 1) % W2_ERROR_BUFFER_SIZE; +} + +void w2_errcatch_handle_error(w2_s_error *error) { + uint8_t severity = error->code & W2_ERR_TYPE_MASK; + + // trigger emergency mode for critical errors + if ((severity ^ W2_ERR_TYPE_CRIT) == 0) g_w2_current_mode = &w2_mode_halt; + + // TODO: handle more error types + switch (error->code) { + case W2_ERR_UNCAUGHT_ERROR: { + break; + } + default: { + w2_errcatch_throw(W2_ERR_UNCAUGHT_ERROR); +#ifdef W2_SIM + simwarn("Uncaught/unhandled error found with code 0x%02x", error->code); +#endif + } + } + + // TODO: forward error to sercomm + + return; +} diff --git a/robot/errcatch.h b/robot/errcatch.h index 48e2a75..2297886 100644 --- a/robot/errcatch.h +++ b/robot/errcatch.h @@ -4,11 +4,40 @@ #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; + +/** 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(enum w2_e_errorcodes 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); + +/** + * allocate and initialize error struct + * + * TODO: doesn't handle null pointers from calloc + */ +w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const char *message); diff --git a/robot/hypervisor.c b/robot/hypervisor.c index 381d9af..6b32776 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -1,19 +1,31 @@ -#include <pololu/orangutan.h> - +#include "hypervisor.h" #include "consts.h" #include "errcatch.h" -#include "hypervisor.h" #include "io.h" #include "modes.h" +#include "orangutan_shim.h" #include "sercomm.h" void w2_hypervisor_main() { + time_reset(); + w2_sercomm_main(); + unsigned long sercomm_time = get_ms(); w2_errcatch_main(); + unsigned long errcatch_time = get_ms() - sercomm_time; w2_io_main(); - - time_reset(); + unsigned long io_time = get_ms() - errcatch_time; w2_modes_main(); - unsigned long elapsed_ms = get_ms(); - if (elapsed_ms > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_ERR_CYCLE_EXPIRED); + unsigned long mode_time = get_ms() - io_time; + +#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); + + usleep(100e3); +#endif + + if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_ERR_CYCLE_EXPIRED); } diff --git a/robot/makefile b/robot/makefile index 598e0fa..6f50519 100644 --- a/robot/makefile +++ b/robot/makefile @@ -5,23 +5,33 @@ DEVICE ?= atmega168 MCU ?= atmega168 AVRDUDE_DEVICE ?= m168 -CFLAGS=-g -Wall -mcall-prologues -mmcu=$(MCU) $(DEVICE_SPECIFIC_CFLAGS) -Os -LDFLAGS=-Wl,-gc-sections -lpololu_$(DEVICE) -Wl,-relax - PORT ?= /dev/ttyACM0 -SOURCES := $(wildcard *.c) -HEADERS := $(wildcard *.h) +CFLAGS=-g -Wall $(DEVICE_SPECIFIC_CFLAGS) -Os +LDFLAGS=-Wl,-gc-sections -Wl,-relax + +SOURCES := $(filter-out sim.c, $(wildcard *.c)) +HEADERS := $(filter-out sim.h, $(wildcard *.h)) + +# simulation +# SIM = true +CFLAGS += $(if $(SIM), -DW2_SIM, -mcall-prologues -mmcu=$(MCU)) +LDFLAGS += $(if $(SIM), , -lpololu_$(DEVICE)) +PREFIX := $(if $(SIM), , avr-) +SOURCES += $(if $(SIM), sim.c, ) +HEADERS += $(if $(SIM), sim.h, ) + OBJECTS := $(patsubst %.c,%.o, $(SOURCES)) AVRDUDE=avrdude -CC=avr-gcc -OBJ2HEX=avr-objcopy +CC=$(PREFIX)gcc +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: out.hex +all: $(if $(SIM), a.out, out.hex) clean: rm -f *.o out.hex a.out compile_commands.json @@ -40,6 +50,8 @@ flash: out.hex $(AVRDUDE) -p $(AVRDUDE_DEVICE) -c avrisp2 -P $(PORT) -U flash:w:out.hex format: + $(eval SOURCES := $(filter-out sim.c, $(SOURCES))) + $(eval HEADERS := $(filter-out sim.h, $(HEADERS))) clang-format -i $(SOURCES) $(HEADERS) clang-tidy --fix-errors $(SOURCES) $(HEADERS) diff --git a/robot/orangutan_shim.h b/robot/orangutan_shim.h new file mode 100644 index 0000000..de57c98 --- /dev/null +++ b/robot/orangutan_shim.h @@ -0,0 +1,7 @@ +#pragma once + +#ifdef W2_SIM +#include "sim.h" +#else +#include <pololu/orangutan.h> +#endif diff --git a/robot/readme.md b/robot/readme.md index bb88e80..4a7aca3 100644 --- a/robot/readme.md +++ b/robot/readme.md @@ -17,6 +17,13 @@ device manager on windows, or by running `ls /dev/ttyACM*` on linux. once the com port is configured, run `make flash` to upload the executable and automatically reboot the robot. +another fun option in the makefile is the SIM mode. by uncommenting the line `# +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!* + ## module hierarchy the software is divided into seperate 'modules' for organizational, @@ -48,12 +55,12 @@ 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 `|Jorn & Abdullaahi| reads and parses incoming serial data; sends all data in the message buffer| +|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 `|Fiona| reads all inputs to global state; writes all outputs| +|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 `|TBD| controls robot during maze portion of map; hands off control to warehouse module| -|warehouse |`mode_grid `|TDB| controls robot during warehouse portion of map; hands off control to maze module| +|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| @@ -86,6 +93,8 @@ this list will probably get updated from time to time: - arbitrary numbers should be aliased to `#define` statements or `enum`s if part of a series. - general constants should be placed in `consts.h` +- don't import `<pololu/orangutan.h>` directly, instead use + `"orangutan_shim.h"` to keep code compatible with the simulator ## todo @@ -120,19 +129,25 @@ to act on accordingly. the error handling module (a) provides functions for other modules to report errors, and (b) handles errors accordingly. -- [ ] create an error `struct` that holds: - - [ ] error code - - [ ] message length - - [ ] message contents -- [ ] create a global error ring buffer with an appropriate size that holds +- [x] create an error `struct` that holds: + - [x] error code + - [x] message length + - [x] message contents +- [x] create a global error ring buffer with an appropriate size that holds error messages - [ ] handle errors in the error buffer, referencing the functional specification for details on what the robot should do to resolve each kind of error - [ ] forward error codes to the pc-communication module -empty function declarations are in place for providing other modules an error -reporting function. +~empty function declarations are in place for providing other modules an error +reporting function.~ + +this module is as good as finished but full functionality is currently +dependent on: + +- [ ] pc communication +- [ ] other mode implementations ### i/o read & write diff --git a/robot/setup.c b/robot/setup.c index 10001c7..c74cca9 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -1,9 +1,9 @@ -#include <pololu/orangutan.h> #include <stdlib.h> #include "consts.h" #include "halt.h" #include "modes.h" +#include "orangutan_shim.h" #include "setup.h" void w2_setup_main() { diff --git a/robot/sim.c b/robot/sim.c new file mode 100644 index 0000000..6bd5838 --- /dev/null +++ b/robot/sim.c @@ -0,0 +1,64 @@ +#include <stdio.h> +#include <time.h> +#include <string.h> + +#include "sim.h" + +struct timespec reference_time; // NOLINT + +void time_reset() { + simprintfunc("time_reset", ""); + clock_gettime(CLOCK_MONOTONIC, &reference_time); + return; +} + +unsigned long get_ms() { + simprintfunc("get_ms", ""); + struct timespec elapsed; + clock_gettime(CLOCK_MONOTONIC, &elapsed); + return ((elapsed.tv_sec * 1000) + (elapsed.tv_nsec / 1000000)) - + ((reference_time.tv_sec * 1000) + (reference_time.tv_nsec / 1000000)); +} + +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++; + } + printf("\n"); + return; +} + diff --git a/robot/sim.h b/robot/sim.h new file mode 100644 index 0000000..15a1b4b --- /dev/null +++ b/robot/sim.h @@ -0,0 +1,40 @@ +#pragma once + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> + +// debug print options +#define DBG_BYTES_PER_LINE 16 + +// debug colors +#define COL_BLK "\e[0;30m" +#define COL_RED "\e[0;31m" +#define COL_GRN "\e[0;32m" +#define COL_YEL "\e[0;33m" +#define COL_BLU "\e[0;34m" +#define COL_MAG "\e[0;35m" +#define COL_CYN "\e[0;36m" +#define COL_WHT "\e[0;37m" +#define COL_RST "\e[0m" + +// debug stdout print macros +#define simprintf(message, ...) 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__) + +/** + * simulates pololu library functions for local testing + * NOLINT is so clang-tidy doesn't correct function names + */ +void time_reset(); // NOLINT +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 serial_set_baud_rate(unsigned int rate); // NOLINT +void serial_send(char* message, unsigned int length); // NOLINT |