diff options
Diffstat (limited to 'robot')
44 files changed, 545 insertions, 128 deletions
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 0dc60e9..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; +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) { + 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(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) { +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\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 fa7a15b..836da1b 100644 --- a/robot/errcatch.h +++ b/robot/errcatch.h @@ -2,19 +2,8 @@ #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]; @@ -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..a6e96ee 100644 --- a/robot/makefile +++ b/robot/makefile @@ -10,11 +10,14 @@ 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 +SIM = true CFLAGS += $(if $(SIM), -DW2_SIM, -mcall-prologues -mmcu=$(MCU)) LDFLAGS += $(if $(SIM), , -lpololu_$(DEVICE)) PREFIX := $(if $(SIM), , avr-) @@ -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 5daa826..eb1dd37 100644 --- a/robot/readme.md +++ b/robot/readme.md @@ -62,13 +62,12 @@ what they're supposed to do: |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_calb `|may 31|Fiona| find line by turning on own axis if lost| +|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 this list will probably get updated from time to time: @@ -109,10 +108,14 @@ global todo: - [ ] assume robot starts in maze - [ ] 'crosswalk' transition detection in seperate file (used by grid and maze mode) -- [ ] calibrate (line-detecting) light sensors manually by placing the robot - and pressing a button - [ ] client software architecture -- [ ] mode 'return' buffer +- [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 |