aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--.clang-tidy11
-rw-r--r--client/makefile7
-rw-r--r--protocol.md34
-rw-r--r--robot/calibration.c1
-rw-r--r--robot/consts.h36
-rw-r--r--robot/errcatch.c49
-rw-r--r--robot/errcatch.h23
-rw-r--r--robot/grid.c1
-rw-r--r--robot/halt.c5
-rw-r--r--robot/hypervisor.c36
-rw-r--r--robot/hypervisor.h10
-rw-r--r--robot/main.c9
-rw-r--r--robot/makefile13
-rw-r--r--robot/maze.c1
-rw-r--r--robot/mode_chrg.c3
-rw-r--r--robot/mode_chrg.h9
-rw-r--r--robot/mode_dirc.c7
-rw-r--r--robot/mode_dirc.h13
-rw-r--r--robot/mode_grid.c3
-rw-r--r--robot/mode_grid.h (renamed from robot/grid.h)0
-rw-r--r--robot/mode_halt.c3
-rw-r--r--robot/mode_halt.h (renamed from robot/halt.h)0
-rw-r--r--robot/mode_lcal.c1
-rw-r--r--robot/mode_lcal.h (renamed from robot/calibration.h)2
-rw-r--r--robot/mode_maze.c3
-rw-r--r--robot/mode_maze.h (renamed from robot/maze.h)0
-rw-r--r--robot/mode_scal.c3
-rw-r--r--robot/mode_scal.h8
-rw-r--r--robot/mode_spin.c3
-rw-r--r--robot/mode_spin.h8
-rw-r--r--robot/modes.c44
-rw-r--r--robot/modes.h35
-rw-r--r--robot/orangutan_shim.h3
-rw-r--r--robot/readme.md13
-rw-r--r--robot/sercomm.c132
-rw-r--r--robot/sercomm.h21
-rw-r--r--robot/setup.c24
-rw-r--r--robot/sim.c100
-rw-r--r--robot/sim.h36
-rw-r--r--robot/tests/dirc.binbin0 -> 6 bytes
-rw-r--r--robot/tests/info.bin1
-rw-r--r--robot/tests/mcfg.binbin0 -> 11 bytes
-rw-r--r--robot/tests/mode.bin1
-rw-r--r--robot/tests/padded_info.bin1
-rw-r--r--robot/tests/ping.binbin0 -> 3 bytes
-rw-r--r--robot/tests/readme.md11
-rw-r--r--robot/tests/undefined.bin1
-rw-r--r--shared/bin.c80
-rw-r--r--shared/bin.h34
-rw-r--r--shared/consts.h21
-rw-r--r--shared/errors.h64
-rw-r--r--shared/makefile5
-rw-r--r--shared/protocol.c97
-rw-r--r--shared/protocol.h219
-rw-r--r--shared/readme.md8
-rw-r--r--shared/semver.c9
-rw-r--r--shared/semver.h11
-rw-r--r--shared/serial_parse.c44
-rw-r--r--shared/serial_parse.h8
-rw-r--r--shared/util.c6
-rw-r--r--shared/util.h7
61 files changed, 1192 insertions, 146 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..8bc8ac7 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/protocol.md b/protocol.md
index b5ddeac..c6cb3fb 100644
--- a/protocol.md
+++ b/protocol.md
@@ -27,22 +27,22 @@ 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 (`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 sercomm.h for code
+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
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
new file mode 100644
index 0000000..1aea35c
--- /dev/null
+++ b/robot/tests/dirc.bin
Binary files differ
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
new file mode 100644
index 0000000..a43ecf8
--- /dev/null
+++ b/robot/tests/mcfg.bin
Binary files differ
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
new file mode 100644
index 0000000..456804e
--- /dev/null
+++ b/robot/tests/ping.bin
Binary files differ
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(&current_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);