3 files changed, 6 insertions, 15 deletions

diff --git a/robot/errcatch.c b/robot/errcatch.c
index 82da820..2a59d3d 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -14,7 +14,7 @@ uint8_t g_w2_error_buffer_full						= 0;
 uint8_t g_w2_error_uncaught							= 0;
 
 void w2_errcatch_main() {
-	while (g_w2_error_index != g_w2_error_offset) {
+	while (g_w2_error_offset != g_w2_error_index) {
 		w2_s_error *error = g_w2_error_buffer[g_w2_error_offset];
 		w2_errcatch_handle_error(error);
 		g_w2_error_offset = (g_w2_error_offset + 1) % W2_ERROR_BUFFER_SIZE;
diff --git a/robot/sercomm.c b/robot/sercomm.c
index 9c67aab..59b0f5a 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -15,22 +15,12 @@ void w2_sercomm_main() {
 	simprintfunc("w2_sercomm_main", "");
 #endif
 	// send data
-	while (g_w2_sercomm_index != g_w2_sercomm_offset) {
-#ifdef W2_SIM
-		simprint("line 0");
-		w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_index];
-		simprint("line 1");
-
-		simprintf("bytes: %i\n", data->bytes);
+	while (g_w2_sercomm_offset != g_w2_sercomm_index) {
+		w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_offset];
 		char *data_cast = malloc(data->bytes);
-		simprint("line 2");
 		memcpy(data_cast, data->data, data->bytes);
-		simprint("line 3");
 		serial_send(data_cast, data->bytes);
-		simprint("line 4");
 		g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE;
-		simprint("line 5");
-#endif
 	}
 }
 
@@ -42,7 +32,8 @@ void w2_sercomm_append_msg(w2_s_bin *data) {
 	g_w2_sercomm_buffer_full = next_index == g_w2_sercomm_offset;
 	free(g_w2_sercomm_buffer[g_w2_sercomm_index]);
 	w2_s_bin *data_copy = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * data->bytes);
-	memcpy(&data_copy->bytes, data->data, data->bytes);
+	memcpy(&data_copy->data, data->data, data->bytes);
+	data_copy->bytes = data->bytes;
 	g_w2_sercomm_buffer[g_w2_sercomm_index] = data_copy;
 	if (g_w2_sercomm_buffer_full) return;
 	g_w2_sercomm_index = next_index;
diff --git a/robot/sim.c b/robot/sim.c
index 6bd5838..1e88feb 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -55,7 +55,7 @@ void serial_send(char* message, unsigned int length) {
 			printf("\n");
 			simprintf("");
 		}
-		printf("%02x ", message[byte]);
+		printf("%02x ", message[byte] & 0xff);
 		bytes++;
 	}
 	printf("\n");