implemented obstacle detection

4 files changed, 17 insertions, 88 deletions

diff --git a/shared/consts.h b/shared/consts.h
index 11e6d14..9c2358f 100644
--- a/shared/consts.h
+++ b/shared/consts.h
@@ -4,7 +4,7 @@
 
 #ifndef W2_BUILD_STR
 // is defined by CFLAGS += -DW2_BUILD_STR in makefile
-#define W2_BUILD_STR ("????????")
+#define W2_BUILD_STR "????????"
 #endif
 
 #if !defined W2_HOST_WIN32 && !defined W2_HOST_LINUX
@@ -13,32 +13,39 @@
 #endif
 
 /** serial baud rate (bit/s) */
-#define W2_SERIAL_BAUD (9600)
+#define W2_SERIAL_BAUD 9600
 /** size of input (receive) buffer (in bytes) */
-#define W2_SERIAL_READ_BUFFER_SIZE (255)
+#define W2_SERIAL_READ_BUFFER_SIZE 255
 
 /** size of the error handling buffer (in errors, not bytes) */
-#define W2_ERROR_BUFFER_SIZE (16)
+#define W2_ERROR_BUFFER_SIZE 16
 /** size of the serial communication buffer (in messages, not bytes) */
-#define W2_SERCOMM_BUFFER_SIZE (16)
+#define W2_SERCOMM_BUFFER_SIZE 16
 /** size of mode history buffer */
-#define W2_MODE_HISTORY_BUFFER_SIZE (8)
+#define W2_MODE_HISTORY_BUFFER_SIZE 8
 
 /** max logic module execution time in milliseconds */
-#define W2_MAX_MODULE_CYCLE_MS (20)
+#define W2_MAX_MODULE_CYCLE_MS 20
 
 /** exponential moving average new measurement weight (double 0-1) */
-#define W2_EMA_WEIGHT (0.10)
+#define W2_EMA_WEIGHT 0.10
 
 /** minimal time between pings */
-#define W2_PING_FREQUENCY (1e3)
+#define W2_PING_FREQUENCY 1e3
 /** max time between ping and answer */
-#define W2_PING_TIMEOUT (5e3)
+#define W2_PING_TIMEOUT 5e3
 
 /** default map width/height */
 #define W2_MAP_DEFAULT_HEIGHT 5
 #define W2_MAP_DEFAULT_WIDTH 5
 
+/** distance too close schmitt trigger low threshold */
+#define W2_IO_DISTANCE_CLOSE_THRESHOLD 400
+/** distance too close schmitt trigger high threshold */
+#define W2_IO_DISTANCE_FAR_THRESHOLD 100
+/** go into emergency mode if object still present after n ms */
+#define W2_IO_DISTANCE_TOO_CLOSE_TIMEOUT 5e3
+
 /** front-facing distance sensor pinout */
 #define W2_FRONT_SENSOR_PIN 5
 /** battery voltage sensor pinout */
diff --git a/shared/io.h b/shared/io.h
deleted file mode 100644
index 584ad1e..0000000
--- a/shared/io.h
+++ /dev/null
@@ -1,59 +0,0 @@
-#pragma once
-
-#include <stdio.h>
-
-#include "bool.h"
-
-#pragma pack(push, 1)
-
-/** momentary button input struct */
-typedef struct {
-	bool pressed;
-} w2_s_i_push;
-
-/** qtr contrast sensor input struct */
-typedef struct {
-	uint16_t range;
-} w2_s_i_contrast;
-
-/** distance sensor input struct */
-typedef struct {
-	uint16_t detection;
-} w2_s_i_distance;
-
-/** battery input struct */
-typedef struct {
-	uint16_t charge_level;
-} w2_s_i_battery;
-
-/** motor output struct */
-typedef struct {
-	int16_t speed;
-} w2_s_o_motor;
-
-/** underside led output struct */
-typedef struct {
-	bool on;
-} w2_s_o_led;
-
-/** lcd output struct */
-typedef struct {
-	char text[16];
-} w2_s_o_display;
-
-/** struct containing all i/o */
-typedef struct {
-	w2_s_i_push button[5];
-	w2_s_i_contrast qtr[5];
-	w2_s_i_distance front_distance;
-	w2_s_i_distance side_distance;
-	w2_s_i_battery battery;
-
-	w2_s_o_motor motor_left;
-	w2_s_o_motor motor_right;
-	w2_s_o_led led_red;
-	w2_s_o_led led_green;
-	w2_s_o_display lcd;
-} w2_s_io_all;
-
-#pragma pack(pop)
diff --git a/shared/protocol.c b/shared/protocol.c
index 9be1d31..02d746a 100644
--- a/shared/protocol.c
+++ b/shared/protocol.c
@@ -18,8 +18,6 @@ void w2_cmd_setup_handlers() {
 	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;
@@ -46,9 +44,6 @@ size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_leng
 
 	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);
 
diff --git a/shared/protocol.h b/shared/protocol.h
index 02d5526..5e1a12b 100644
--- a/shared/protocol.h
+++ b/shared/protocol.h
@@ -8,7 +8,6 @@
 #include "bin.h"
 #include "bool.h"
 #include "consts.h"
-#include "io.h"
 
 #define W2_SERIAL_START_BYTE 0xff
 
@@ -136,15 +135,6 @@ typedef struct {
 
 typedef struct {
 	uint8_t opcode;
-} w2_s_cmd_sens_rx;
-
-typedef struct {
-	uint8_t opcode;
-	w2_s_io_all io;
-} w2_s_cmd_sens_tx;
-
-typedef struct {
-	uint8_t opcode;
 } w2_s_cmd_info_rx;
 
 typedef struct {
@@ -205,10 +195,6 @@ void w2_cmd_bomd_tx(w2_s_bin *data);
 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) */