From 054cf1c2a433c87b7caee503c28db2b7005d1060 Mon Sep 17 00:00:00 2001
From: lonkaars <loek@pipeframe.xyz>
Date: Wed, 29 Jun 2022 10:35:58 +0200
Subject: fix order receiving

---
 robot/errcatch.c  | 3 +++
 robot/mode_grid.c | 9 +++++++++
 robot/sercomm.c   | 6 ------
 shared/bin.c      | 8 ++++----
 shared/errcatch.h | 3 +++
 5 files changed, 19 insertions(+), 10 deletions(-)

diff --git a/robot/errcatch.c b/robot/errcatch.c
index 27026b5..3e62270 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -20,6 +20,7 @@ void w2_errcatch_display_error(uint8_t code) {
 
 void w2_errcatch_error_beep(uint8_t code) {
 	if (code == W2_E_WARN_UNCAUGHT_ERROR) return;
+	if (code == W2_E_WARN_CYCLE_EXPIRED) return;
 	uint8_t severity = code & W2_E_TYPE_MASK;
 	if ((severity ^ W2_E_TYPE_CRIT) == 0) {
 		play("L70 O6 fRfRfRfRf");
@@ -48,6 +49,8 @@ void w2_errcatch_handle_error(w2_s_error *error) {
 			break;
 		case W2_E_WARN_BATTERY_LOW:
 			break;
+		case W2_E_WARN_CYCLE_EXPIRED:
+			break;
 		default: {
 			g_w2_error_uncaught = true;
 #ifdef W2_SIM
diff --git a/robot/mode_grid.c b/robot/mode_grid.c
index ff80dd2..b6ed572 100644
--- a/robot/mode_grid.c
+++ b/robot/mode_grid.c
@@ -1,9 +1,13 @@
+#include <stdio.h>
+#include <string.h>
+
 #include "mode_grid.h"
 #include "hypervisor.h"
 #include "io.h"
 #include "modes.h"
 #include "movement.h"
 #include "orangutan_shim.h"
+#include "../shared/errcatch.h"
 
 /**
  * TODO: mode_grid g_w2_target_area laten volgen
@@ -204,6 +208,11 @@ void w2_turn_east() {
 // signals when the product is picked
 void w2_arrived_message() {
 	if (g_w2_location.x == g_w2_destination.x && g_w2_location.y == g_w2_destination.y) {
+		char msg[64];
+		sprintf(msg, "reached %i, %i", g_w2_destination.x, g_w2_destination.y);
+		size_t len = strlen(msg);
+		w2_errcatch_throw_msg(W2_E_INFO_ORDER_ARRIVED, len, msg);
+
 		play_frequency(400, 500, 7);
 		delay(500);
 	}
diff --git a/robot/sercomm.c b/robot/sercomm.c
index 16134d1..8df6157 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -118,17 +118,11 @@ void w2_cmd_dirc_rx(w2_s_bin *data) {
 
 void w2_cmd_cord_rx(w2_s_bin *data) { return; }
 
-#include <stdio.h>
 void w2_cmd_bomd_rx(w2_s_bin *data) {
 	W2_CAST_BIN(w2_s_cmd_bomd_rx, data, req);
 
 	req->position = w2_bin_hton32(req->position);
 
-	char buf[32];
-	clear();
-	// sprintf(buf, "%lu, %lu", req->position % W2_MAP_DEFAULT_WIDTH, req->position / W2_MAP_DEFAULT_WIDTH);
-	sprintf(buf, "%lu", req->position);
-	print(buf);
 	g_w2_order[g_w2_order_index].x = req->position % W2_MAP_DEFAULT_WIDTH;
 	g_w2_order[g_w2_order_index].y = req->position / W2_MAP_DEFAULT_WIDTH;
 	g_w2_order_index++;
diff --git a/shared/bin.c b/shared/bin.c
index a0937e8..9d67bec 100644
--- a/shared/bin.c
+++ b/shared/bin.c
@@ -10,10 +10,10 @@
 #define _SHIFT_1B (8 * 1)
 #define _SHIFT_2B (8 * 2)
 #define _SHIFT_3B (8 * 3)
-#define _BYTE_0 ((uint32_t)(0xff << (_SHIFT_0B)))
-#define _BYTE_1 ((uint32_t)(0xff << (_SHIFT_1B)))
-#define _BYTE_2 ((uint32_t)(0xff << (_SHIFT_2B)))
-#define _BYTE_3 ((uint32_t)(0xff << (_SHIFT_3B)))
+#define _BYTE_0 ((uint32_t) 0xff << (_SHIFT_0B))
+#define _BYTE_1 ((uint32_t) 0xff << (_SHIFT_1B))
+#define _BYTE_2 ((uint32_t) 0xff << (_SHIFT_2B))
+#define _BYTE_3 ((uint32_t) 0xff << (_SHIFT_3B))
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wshift-count-overflow"
diff --git a/shared/errcatch.h b/shared/errcatch.h
index a56bc00..b14379d 100644
--- a/shared/errcatch.h
+++ b/shared/errcatch.h
@@ -54,6 +54,9 @@ typedef enum {
 	W2_E_WARN_MODE_HISTORY_BUFFER_IOB = 0x0a | W2_E_TYPE_WARN,
 	/** ping timeout reached */
 	W2_E_WARN_PING_TIMEOUT = 0x0b | W2_E_TYPE_WARN,
+
+	/** order reached */
+	W2_E_INFO_ORDER_ARRIVED = 0x00 | W2_E_TYPE_INFO,
 } w2_e_errorcode;
 
 /**
-- 
cgit v1.2.3