25 files changed, 421 insertions, 35 deletions
diff --git a/robot/calibration.c b/robot/calibration.c
new file mode 100644
index 0000000..7788532
--- /dev/null
+++ b/robot/calibration.c
@@ -0,0 +1 @@
+#include "calibration.h"
diff --git a/robot/calibration.h b/robot/calibration.h
new file mode 100644
index 0000000..5c1af38
--- /dev/null
+++ b/robot/calibration.h
@@ -0,0 +1,9 @@
+#pragma once
+
+/**
+ * calibration mode
+ *
+ * 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();
diff --git a/robot/consts.h b/robot/consts.h
new file mode 100644
index 0000000..6530528
--- /dev/null
+++ b/robot/consts.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#ifndef BUILD_STR
+// should be defined with -DBUILD_STR in makefile
+#define BUILD_STR ("????????")
+#endif
+
+#define W2_MAX_MODULE_CYCLE_MS (20)
+#define W2_SERIAL_BAUD (9600)
+
+#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)
+
+/**
+ * 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,
+};
diff --git a/robot/errcatch.c b/robot/errcatch.c
new file mode 100644
index 0000000..5b42bb4
--- /dev/null
+++ b/robot/errcatch.c
@@ -0,0 +1,7 @@
+#include "errcatch.h"
+
+void w2_errcatch_main() {}
+
+void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const char *message) {}
+
+void w2_errcatch_throw(enum w2_e_errorcodes code) { w2_errcatch_throw_msg(code, 0, ""); }
diff --git a/robot/errcatch.h b/robot/errcatch.h
new file mode 100644
index 0000000..48e2a75
--- /dev/null
+++ b/robot/errcatch.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <stdint.h>
+
+#include "consts.h"
+
+/** error-handler module main */
+void w2_errcatch_main();
+
+/** append error to error buffer */
+void w2_errcatch_throw(enum w2_e_errorcodes code);
+
+/** append error to error buffer (with debug message) */
+void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const char *message);
diff --git a/robot/grid.c b/robot/grid.c
new file mode 100644
index 0000000..0c83272
--- /dev/null
+++ b/robot/grid.c
@@ -0,0 +1 @@
+#include "grid.h"
diff --git a/robot/grid.h b/robot/grid.h
new file mode 100644
index 0000000..fcf9100
--- /dev/null
+++ b/robot/grid.h
@@ -0,0 +1,8 @@
+#pragma once
+
+/**
+ * warehouse mode
+ *
+ * processes orders from the order buffer
+ */
+void w2_mode_grid();
diff --git a/robot/halt.c b/robot/halt.c
new file mode 100644
index 0000000..2f159f0
--- /dev/null
+++ b/robot/halt.c
@@ -0,0 +1,5 @@
+#include <stdbool.h>
+
+#include "halt.h"
+
+void w2_mode_halt() { return; }
diff --git a/robot/halt.h b/robot/halt.h
new file mode 100644
index 0000000..d92905e
--- /dev/null
+++ b/robot/halt.h
@@ -0,0 +1,8 @@
+#pragma once
+
+/**
+ * halt (emergency) mode
+ *
+ * stops all execution until emergency status is manually cleared by the user
+ */
+void w2_mode_halt();
diff --git a/robot/hypervisor.c b/robot/hypervisor.c
new file mode 100644
index 0000000..381d9af
--- /dev/null
+++ b/robot/hypervisor.c
@@ -0,0 +1,19 @@
+#include <pololu/orangutan.h>
+
+#include "consts.h"
+#include "errcatch.h"
+#include "hypervisor.h"
+#include "io.h"
+#include "modes.h"
+#include "sercomm.h"
+
+void w2_hypervisor_main() {
+	w2_sercomm_main();
+	w2_errcatch_main();
+	w2_io_main();
+
+	time_reset();
+	w2_modes_main();
+	unsigned long elapsed_ms = get_ms();
+	if (elapsed_ms > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_ERR_CYCLE_EXPIRED);
+}
diff --git a/robot/hypervisor.h b/robot/hypervisor.h
new file mode 100644
index 0000000..0e73259
--- /dev/null
+++ b/robot/hypervisor.h
@@ -0,0 +1,8 @@
+#pragma once
+
+/**
+ * backbone of all other modules
+ *
+ * stores global variables and controls when other modules run
+ */
+void w2_hypervisor_main();
diff --git a/robot/io.c b/robot/io.c
new file mode 100644
index 0000000..4a85458
--- /dev/null
+++ b/robot/io.c
@@ -0,0 +1,3 @@
+#include "io.h"
+
+void w2_io_main() {}
diff --git a/robot/io.h b/robot/io.h
new file mode 100644
index 0000000..14fe0af
--- /dev/null
+++ b/robot/io.h
@@ -0,0 +1,4 @@
+#pragma once
+
+/** i/o module main */
+void w2_io_main();
diff --git a/robot/main.c b/robot/main.c
index 21d0e5c..fbfd38b 100644
--- a/robot/main.c
+++ b/robot/main.c
@@ -1,34 +1,12 @@
-#include <pololu/orangutan.h>
-#include <stdlib.h>
+#include "main.h"
+#include "hypervisor.h"
+#include "setup.h"
 
 int main() {
-	play("L50 c>c");
-	serial_set_baud_rate(9600);
+	w2_setup_main();
 
-	char *buf = malloc(20);
-	unsigned int counter = 0;
-
-	while (1) {
-		serial_receive_blocking(buf, 1, 65e3);
-
-		switch (buf[0]) {
-			case 0x7f: {
-				counter--;
-				lcd_goto_xy(counter, 0);
-				print(" ");
-				lcd_goto_xy(counter, 0);
-				break;
-			}
-			default: {
-				print(&buf[0]);
-				counter++;
-				if (counter > 20) {
-					counter = 0;
-					lcd_goto_xy(0, 0);
-				}
-			}
-		}
-	}
+	for (;;) w2_hypervisor_main();
 
+	// satisfy compiler
 	return 0;
 }
diff --git a/robot/main.h b/robot/main.h
new file mode 100644
index 0000000..5b0a1b2
--- /dev/null
+++ b/robot/main.h
@@ -0,0 +1,4 @@
+#pragma once
+
+/** program entrypoint */
+int main();
diff --git a/robot/makefile b/robot/makefile
index d01ad30..8ddcb28 100644
--- a/robot/makefile
+++ b/robot/makefile
@@ -8,7 +8,7 @@ AVRDUDE_DEVICE ?= m168
 CFLAGS=-g -Wall -mcall-prologues -mmcu=$(MCU) $(DEVICE_SPECIFIC_CFLAGS) -Os
 LDFLAGS=-Wl,-gc-sections -lpololu_$(DEVICE) -Wl,-relax
 
-PORT ?= /dev/ttyACM1
+PORT ?= /dev/ttyACM0
 
 SOURCES := $(wildcard *.c)
 HEADERS := $(wildcard *.h)
@@ -18,6 +18,9 @@ AVRDUDE=avrdude
 CC=avr-gcc
 OBJ2HEX=avr-objcopy
 
+BUILD_STR=$(shell git update-index -q --refresh; git describe --tags --dirty='*' --broken='x' | cut -c1-20)
+CFLAGS += -DBUILD_STR="$(BUILD_STR)"
+
 all: out.hex
 
 clean:
@@ -31,6 +34,7 @@ a.out: $(OBJECTS)
 
 out.hex: a.out
 	$(OBJ2HEX) -R .eeprom -O ihex $< $@
+	$(info build $(BUILD_STR) complete)
 
 flash: out.hex
 	$(AVRDUDE) -p $(AVRDUDE_DEVICE) -c avrisp2 -P $(PORT) -U flash:w:out.hex
diff --git a/robot/maze.c b/robot/maze.c
new file mode 100644
index 0000000..a27414f
--- /dev/null
+++ b/robot/maze.c
@@ -0,0 +1 @@
+#include "maze.h"
diff --git a/robot/maze.h b/robot/maze.h
new file mode 100644
index 0000000..9fbeb8c
--- /dev/null
+++ b/robot/maze.h
@@ -0,0 +1,8 @@
+#pragma once
+
+/**
+ * maze mode
+ *
+ * finds route out of maze
+ */
+void w2_mode_maze();
diff --git a/robot/modes.c b/robot/modes.c
new file mode 100644
index 0000000..c22875c
--- /dev/null
+++ b/robot/modes.c
@@ -0,0 +1,6 @@
+#include "modes.h"
+#include "halt.h"
+
+void (*g_w2_current_mode)() = &w2_mode_halt;
+
+void w2_modes_main() { (*g_w2_current_mode)(); }
diff --git a/robot/modes.h b/robot/modes.h
new file mode 100644
index 0000000..dd34690
--- /dev/null
+++ b/robot/modes.h
@@ -0,0 +1,10 @@
+#pragma once
+
+extern void (*g_w2_current_mode)();
+
+/**
+ * mode logic
+ *
+ * executes mode in g_w2_current_mode
+ */
+void w2_modes_main();
diff --git a/robot/readme.md b/robot/readme.md
index 5e7efb6..fa5774c 100644
--- a/robot/readme.md
+++ b/robot/readme.md
@@ -1,10 +1,221 @@
 # robot code
 
-dit is de submap voor alle code die op de robot zelf draait
+this is the subdirectory for all code that runs on the 3pi robot
 
-## make gedoe
+## make
+
+the makefile in this directory works the same as a regular makefile, with the
+exception of the `make flash` command.
+
+to upload the compiled robot executable, you need to change the com port
+specified in the makefile in this directory (line that says `PORT ?=
+/dev/ttyACMx`). for windows this should be changed to `PORT ?= COMx`, where x
+is the number of the 'programming' com port on the programmer. this will
+probably change every time you re-plug the programmer, so you should try every
+com port until it uploads successfully. you can find available com ports in
+device manager on windows, or by running `ls /dev/ttyACM*` on linux. once the
+com port is configured, run `make flash` to upload the executable and
+automatically reboot the robot.
+
+## module hierarchy
+
+the software is divided into seperate 'modules' for organizational,
+maintenance, testing and debugging purposes. the sizes of the blocks in the
+following diagram are a bit misleading, as some of these blocks are mostly
+organizational and form more of a software 'skeleton', while the 'maze' and
+'warehouse' modules provide the majority of the actual control logic.
+
+```
+┌──────────────────────────────────────────────────────────────────────┐
+│                              "Hypervisor"                            │
+└────────┬──────────────────┬──────────────────┬────────────────┬──────┘
+┌────────┴───────┐┌─────────┴────────┐┌────────┴─────────┐┌─────┴──────┐
+│ Error handling ││ I/O Read & Write ││ PC communication ││ Mode logic │
+└────────────────┘└──────────────────┘└──────────────────┘└─────┬──────┘
+                      ┌──────────┬──────────────┬───────────────┤
+                  ┌───┴──┐┌──────┴────┐┌────────┴───────┐┌──────┴──────┐
+      *modes* ->  │ Maze ││ Warehouse ││ Emergency stop ││ Calibration │
+                  └──────┘└───────────┘└────────────────┘└─────────────┘
+```
+
+this diagram roughly describes how different parts of the robot software are
+called. most of these modules can talk to each other using functions exposed by
+the modules themselves (e.g. the maze module sending an error code to the error
+handling module, which then both handles the error and forwards it to pc
+communication for logging purposes). here's a quick run-down of all modules and
+what they're supposed to do:
+
+|module          |internal name|purpose|
+|----------------|-------------|-|
+|hypervisor      |`hypervisor `|backbone of all other modules; stores global variables; controls when other modules run|
+|pc communication|`sercomm    `|reads and parses incoming serial data; sends all data in the message buffer|
+|error handling  |`errcatch   `|receives error codes; controls how errors are handled|
+|i/o read & write|`io         `|reads all inputs to global state; writes all outputs|
+|mode logic      |`modes      `|executes the appropriate module for current mode|
+|maze            |`mode_maze  `|controls robot during maze portion of map; hands off control to warehouse module|
+|warehouse       |`mode_grid  `|controls robot during warehouse portion of map; hands off control to maze module|
+|emergency stop  |`mode_halt  `|stops all execution until emergency mode is reset by software or user|
+|calibration     |`mode_calb  `|find line by turning on own axis if lost|
+
+## some standards
+
+this list will probably get updated from time to time:
+
+- modules shouldn't create any global state variables, they should use `static`
+  variables instead.
+- modules are run cyclically, so they shouldn't take more than
+  `W2_MAX_MODULE_CYCLE_MS` to execute (this is an arbitrary number, and may be
+  changed).
+- documentation comments should follow the [javadoc-style doxygen
+  format](https://wiki.scilab.org/Doxygen%20documentation%20Examples) and be
+  placed in header (.h) files if possible. this only applies to public members
+  (e.g. no local variables or module-internal code).
+- code style is mostly handled by `clang-format` and `clang-tidy`, but you
+  should still follow these naming conventions (`<angle brackets>` indicate
+  placeholders):
+  |symbol type|name|example|
+  |-|-|-|
+  |function|`w2_<module>_<name>`|`w2_errcatch_pushcode`; `w2_sercomm_sendraw`|
+  |variable|`g_w2_<name>`|`g_w2_inputs`; `g_w2_current_mode`|
+  |constant|`W2_<NAME>`|`W2_MAX_MODULE_CYCLE_MS`; `W2_SERIAL_BAUD`|
+  |struct|`w2_s_<name>`|`w2_s_input_data`; `w2_s_output_data`|
+  |enum|`w2_e_<name>`|`w2_e_errorcodes`; `w2_e_serial_commands`|
+  
+  this again only applies to public members. local variables should still have
+  short descriptive names, but shouldn't be prefixed with `w2_*`.
+- arbitrary numbers should be aliased to `#define` statements or `enum`s if
+  part of a series.
+- general constants should be placed in `consts.h`
+
+## todo
+
+global todo:
+
+- [ ] add test/simulation mode for wet floor (spinning)
+- [ ] add a manual control mode
+- [ ] start robot in calibration mode
+- [ ] assume robot starts in maze
+- [ ] maze-grid transition detection in seperate file (used by grid and maze
+  mode)
+- [ ] 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
+
+### hypervisor
+
+the hypervisor executes all other modules, and measures execution time. it also
+provides all other modules with a central place for defining global variables.
+
+### pc communication
+
+> this mode can't be implemented until the pc-communication protocol spec is
+> finished
+
+the pc communication module sends messages in a binary format over the serial
+connection provided by the wixel modules. this module should also send a 'ping'
+command each cycle to check if the connection is still intact. the pc will also
+periodically send ping, and various other commands which this module will have
+to act on accordingly.
+
+### error handling
+
+the error handling module (a) provides functions for other modules to report
+errors, and (b) handles errors accordingly.
+
+- [ ] create an error `struct` that holds:
+  - [ ] error code
+  - [ ] message length
+  - [ ] message contents
+- [ ] create a global error ring buffer with an appropriate size that holds
+  error messages
+- [ ] handle errors in the error buffer, referencing the functional
+  specification for details on what the robot should do to resolve each kind of
+  error
+- [ ] forward error codes to the pc-communication module
+
+empty function declarations are in place for providing other modules an error
+reporting function.
+
+### i/o read & write
+
+the i/o module reads all inputs once and writes all outputs once. this keeps
+cycle time constant, and makes sure no time is wasted re-reading inputs, or
+writing outputs more than once.
+
+- [ ] create `struct`s for each type of input:  
+  - [ ] button
+  - [ ] infrared light sensor
+  - [ ] time-of-flight distance sensor
+- [ ] create a single `struct` that holds all input data
+- [ ] create a single `struct` that holds output data values for:
+  - [ ] left motor speed
+  - [ ] right motor speed
+  - [ ] red led
+  - [ ] green led
+
+extra (requires external interrupt setup):
+- [ ] add a `pressed` property to the button struct that turns on if the button
+  was pressed outside the i/o module execution span
+- [ ] add a `press_duration` property to the button struct that measures button
+  press duration, and that works when the button is pressed outside the i/o
+  module execution span
+
+technically the wixel serial channel, programmer debug serial channel, lcd
+contents and speaker tones are also considered outputs, but these all take
+significant time or memory to update, so these will not be updated using the
+cyclic i/o module.
+
+### modes
+
+modes is a shim module that forwards execution to the currently selected mode.
+the global variable `g_w2_current_mode` holds a pointer to the current mode.
+this makes sure only a single mode handler gets ran on every execution cycle.
+
+### maze
+
+the maze mode controls the robot when it's in the maze, and sets execution to
+grid mode when it detects the maze-grid transition. the solving algorithm will
+constantly keep either left or right until (a) the maze-grid transition is
+detected, (b) the charging pad is detected, or (c) the starting point is
+detected. depending on which location is desired, the robot may continue to
+venture through the maze when it finds any of these. exact implementation
+details for this mode are yet to be determined.
+
+### warehouse
+
+the warehouse mode controls the robot when it's in the warehouse, and sets
+execution to maze mode when it detects the maze-grid transition. exact
+implementation details for this mode are yet to be determined.
+
+### emergency stop
+
+> this mode can't be implemented until the pc-communication protocol spec is
+> finished
+
+the emergency stop mode stops the robot from doing anything until the user
+determines it is safe to resume execution.
+
+- [ ] create a global variable that holds the previous mode
+- [ ] create a global variable that holds a 'safe' state (startup/default
+  value = false)
+- [ ] add a condition in the supervisor that switches to the emergency mode if
+  the 'safe' state variable is false
+- [ ] add a condition in the emergency mode handler that switches to the
+  previous mode if the 'safe' state returns to false
+
+### calibration
+
+the calibration sequence is used during the maze mode for re-finding the line
+when the robot gets lost. the robot will first try to find the line by itself
+when it gets lost. when it does this it will send a warning to the error
+buffer. in case it can't find the line anymore, it will go into emergency mode
+and send a critical warning.
+
+- [ ] implement line-finding sequence  
+  - turn 360 degrees (about robot's own axis)
+  - if a line is found at any point during this rotation, stop turning
+  - if a full rotation is completed without a found line, enter emergency
+    mode
+- [ ] add a warning for line lost
 
-om de code te uploaden naar de robot moet je de juiste com-poort instellen in
-de makefile in deze map (regel waar `PORT ?= /dev/ttyACM0` staat). deze kun je
-waarschijnlijk vinden in apparaatbeheer op windows (bijv. `PORT ?= COM4`).
-daarna kun je `make flash` uitvoeren om de code te uploaden
diff --git a/robot/sercomm.c b/robot/sercomm.c
new file mode 100644
index 0000000..a0eed3a
--- /dev/null
+++ b/robot/sercomm.c
@@ -0,0 +1,3 @@
+#include "sercomm.h"
+
+void w2_sercomm_main() {}
diff --git a/robot/sercomm.h b/robot/sercomm.h
new file mode 100644
index 0000000..58c79b9
--- /dev/null
+++ b/robot/sercomm.h
@@ -0,0 +1,9 @@
+#pragma once
+
+/**
+ * serial pc-robot communication module
+ *
+ * - reads and parses incoming serial data
+ * - sends all data in the message buffer
+ */
+void w2_sercomm_main();
diff --git a/robot/setup.c b/robot/setup.c
new file mode 100644
index 0000000..10001c7
--- /dev/null
+++ b/robot/setup.c
@@ -0,0 +1,21 @@
+#include <pololu/orangutan.h>
+#include <stdlib.h>
+
+#include "consts.h"
+#include "halt.h"
+#include "modes.h"
+#include "setup.h"
+
+void w2_setup_main() {
+	serial_set_baud_rate(W2_SERIAL_BAUD);
+
+	// reset underside leds
+	red_led(0);
+	green_led(0);
+
+	// clear lcd
+	clear();
+
+	// indicate startup done
+	play("L50 c>c");
+}
diff --git a/robot/setup.h b/robot/setup.h
new file mode 100644
index 0000000..17ac78d
--- /dev/null
+++ b/robot/setup.h
@@ -0,0 +1,11 @@
+#pragma once
+
+/**
+ * runs once at startup, plays beep when setup finishes
+ *
+ * configures:
+ * - serial connection (wixel)
+ * - lcd display
+ * - underside leds
+ */
+void w2_setup_main();