implement direct control

8 files changed, 26 insertions, 5 deletions

diff --git a/protocol.md b/protocol.md
index 345fe50..c6cb3fb 100644
--- a/protocol.md
+++ b/protocol.md
@@ -36,7 +36,7 @@ readability.
 |`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
diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c
index 6c5d2bb..0bbf3cb 100644
--- a/robot/mode_dirc.c
+++ b/robot/mode_dirc.c
@@ -1,3 +1,7 @@
 #include "mode_dirc.h"
+#include "orangutan_shim.h"
 
-void w2_mode_dirc() {}
+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
index 25a664a..5b9bbf4 100644
--- a/robot/mode_dirc.h
+++ b/robot/mode_dirc.h
@@ -1,5 +1,10 @@
 #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
  *
diff --git a/robot/sercomm.c b/robot/sercomm.c
index c9c6194..2786b85 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 "mode_dirc.h"
 #include "modes.h"
 #include "orangutan_shim.h"
 #include "sercomm.h"
@@ -76,7 +77,13 @@ void w2_cmd_mode_rx(w2_s_bin *data) {
 
 void w2_cmd_sped_rx(w2_s_bin *data) { return; }
 
-void w2_cmd_dirc_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; }
 
diff --git a/robot/sim.c b/robot/sim.c
index baf8a8a..ddc208a 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -123,3 +123,6 @@ void w2_sim_print_serial(w2_s_bin *data) {
 	printf("\n");
 }
 
+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 25cc713..a595042 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -10,7 +10,7 @@
 extern bool g_w2_sim_headless;
 
 // debug fine-tuning
-#define DBG_ENABLE_PRINTFUNC (0)
+#define DBG_ENABLE_PRINTFUNC (1)
 #define DBG_ENABLE_SIMWARN (1)
 #define DBG_ENABLE_SIMINFO (1)
 #define DBG_ENABLE_CYCLEINFO (0)
@@ -54,6 +54,8 @@ void serial_set_baud_rate(unsigned int rate); // 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
diff --git a/robot/tests/mode.bin b/robot/tests/mode.bin
index 35ebd35..9cd9175 100644
--- a/robot/tests/mode.bin
+++ b/robot/tests/mode.bin
@@ -1 +1 @@
-ÿ
\ No newline at end of file
+ÿ
\ No newline at end of file