dirc working

7 files changed, 13 insertions, 12 deletions

diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index 4f994a8..478d3a5 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -27,7 +27,7 @@ void w2_hypervisor_main() {
 	unsigned long sercomm_time = get_ms();
 	w2_errcatch_main();
 	unsigned long errcatch_time = get_ms() - sercomm_time;
-	w2_io_main();
+	// w2_io_main();
 	unsigned long io_time = get_ms() - errcatch_time;
 	w2_modes_main();
 	unsigned long mode_time = get_ms() - io_time;
diff --git a/robot/main.c b/robot/main.c
index b0302bd..2f15e97 100644
--- a/robot/main.c
+++ b/robot/main.c
@@ -5,7 +5,6 @@
 #include "sim.h"
 #endif
 
-#include <unistd.h>
 int main() {
 #ifdef W2_SIM
 	w2_sim_setup();
diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c
index 9ed9fef..5988816 100644
--- a/robot/mode_dirc.c
+++ b/robot/mode_dirc.c
@@ -1,10 +1,13 @@
 #include "mode_dirc.h"
 #include "io.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);
 	g_w2_io.motor_left.speed  = g_w2_mode_dirc_motor_l;
 	g_w2_io.motor_right.speed = g_w2_mode_dirc_motor_r;
 }
diff --git a/robot/modes.c b/robot/modes.c
index f73a809..7decf47 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -46,13 +46,11 @@ void w2_modes_switch(w2_e_mode new_mode, bool replace) {
 	}
 
 	// 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_CREATE_MSG_BIN(w2_s_cmd_mode_tx, msg, msg_bin);
+	msg->opcode = W2_CMD_MODE | W2_CMDDIR_TX;
+	msg->mode	= new_mode;
+
 	w2_sercomm_append_msg(msg_bin);
-	free(msg);
 	free(msg_bin);
 }
 
diff --git a/robot/sercomm.c b/robot/sercomm.c
index deef39f..519568d 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -37,10 +37,11 @@ void w2_sercomm_main() {
 #ifdef W2_SIM
 		w2_sim_print_serial(data);
 #endif
-		serial_send("\xff", 1);
+		serial_send_blocking("\xff", 1);
 		for (uint8_t i = 0; i < data->bytes; i++) {
 			uint8_t byte = data->data[i];
-			byte == 0xff ? serial_send("\xff\xff", 2) : serial_send((char *)&byte, 1);
+			byte == 0xff ? serial_send_blocking("\xff\xff", 2)
+						 : serial_send_blocking((char *)&byte, 1);
 		}
 		g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE;
 	}
diff --git a/robot/sim.c b/robot/sim.c
index 5956fbb..9cce12f 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -74,7 +74,7 @@ void serial_set_baud_rate(unsigned int rate) {
 	simprintfunc("serial_set_baud_rate", "%u", rate);
 }
 
-void serial_send(char* message, unsigned int length) {
+void serial_send_blocking(char* message, unsigned int length) {
 	for (unsigned int byte = 0; byte < length; byte++)
 		putc(message[byte] & 0xff, stdout);
 	fflush(stdout);
diff --git a/robot/sim.h b/robot/sim.h
index 9e250da..9d73585 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -71,7 +71,7 @@ void green_led(unsigned char on); // NOLINT
 void clear(); // 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_blocking(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