implement SENS command0.2.0

5 files changed, 32 insertions, 5 deletions

diff --git a/robot/modes.c b/robot/modes.c
index 8d3a099..9877f6e 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -1,8 +1,7 @@
-#include <stdbool.h>
-
+#include "modes.h"
+#include "../shared/protocol.h"
 #include "../shared/util.h"
 #include "errcatch.h"
-#include "modes.h"
 #include "sercomm.h"
 
 /** function pointer to current mode */
diff --git a/robot/readme.md b/robot/readme.md
index c1cae72..b27c06f 100644
--- a/robot/readme.md
+++ b/robot/readme.md
@@ -99,6 +99,9 @@ this list will probably get updated from time to time:
 - general constants should be placed in `consts.h`
 - don't import `<pololu/orangutan.h>` directly, instead use
   `"orangutan_shim.h"` to keep code compatible with the simulator
+- don't use `<stdbool.h>`, instead use `"../shared/protocol.h"`. this makes
+  sure that `bool` values are equal to `uint8_t`. they're functionally
+  identical.
 
 ## todo
 
diff --git a/robot/sercomm.c b/robot/sercomm.c
index 2786b85..2736030 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -4,6 +4,7 @@
 #include "../shared/bin.h"
 #include "../shared/serial_parse.h"
 #include "hypervisor.h"
+#include "io.h"
 #include "mode_dirc.h"
 #include "modes.h"
 #include "orangutan_shim.h"
@@ -93,7 +94,30 @@ 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_sens_rx(w2_s_bin *data) {
+	w2_s_cmd_sens_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes));
+	memcpy(message, data->data, data->bytes);
+
+	size_t return_size				 = sizeof(w2_s_cmd_sens_tx);
+	w2_s_cmd_sens_tx *return_message = malloc(return_size);
+	return_message->opcode			 = W2_CMD_SENS | W2_CMDDIR_TX;
+	memcpy((uint8_t *)&return_message->io, (uint8_t *)&g_w2_io, sizeof(w2_s_io_all));
+
+	for (int i = 0; i < 5; i++) w2_bin_repl_hton16(&return_message->io.qtr[i].range);
+	w2_bin_repl_hton16(&return_message->io.front_distance.detection);
+	w2_bin_repl_hton16(&return_message->io.side_distance.detection);
+	w2_bin_repl_hton16(&return_message->io.battery.charge_level);
+	w2_bin_repl_hton16((uint16_t *)&return_message->io.motor_left.speed);
+	w2_bin_repl_hton16((uint16_t *)&return_message->io.motor_right.speed);
+
+	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_info_rx(w2_s_bin *data) {
 	w2_s_cmd_info_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes));
diff --git a/robot/sim.h b/robot/sim.h
index f087517..ab1cd77 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -3,9 +3,9 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
-#include <stdbool.h>
 
 #include "../shared/bin.h"
+#include "../shared/protocol.h"
 
 extern bool g_w2_sim_headless;
 
diff --git a/robot/tests/sens.bin b/robot/tests/sens.bin
new file mode 100644
index 0000000..0f21683
--- /dev/null
+++ b/robot/tests/sens.bin
@@ -0,0 +1 @@
+ÿ
\ No newline at end of file