implement mode switching tab

6 files changed, 47 insertions, 8 deletions

diff --git a/client/commands.c b/client/commands.c
index 778e9c1..05b92d9 100644
--- a/client/commands.c
+++ b/client/commands.c
@@ -46,10 +46,21 @@ void w2_send_ping() {
 }
 
 void w2_send_mode(w2_e_mode mode) {
-	W2_CREATE_MSG_BIN(w2_s_cmd_mode_rx, msg, msg_bin);
-	msg->opcode = W2_CMD_MODE | W2_CMDDIR_RX;
-	msg->mode	= mode;
+	if (mode == W2_M_PREV) {
+		W2_CREATE_MSG_BIN(w2_s_cmd_sres_rx, msg, msg_bin);
 
-	w2_send_bin(msg_bin);
-	free(msg_bin);
+		msg->opcode = W2_CMD_SRES | W2_CMDDIR_RX;
+		msg->type = W2_CMD_SRES_RX_TYPE_PREVMODE;
+
+		w2_send_bin(msg_bin);
+		free(msg_bin);
+	} else {
+		W2_CREATE_MSG_BIN(w2_s_cmd_mode_rx, msg, msg_bin);
+
+		msg->opcode = W2_CMD_MODE | W2_CMDDIR_RX;
+		msg->mode	= mode;
+
+		w2_send_bin(msg_bin);
+		free(msg_bin);
+	}
 }
diff --git a/client/i18n/en_us.h b/client/i18n/en_us.h
index ceca215..d198296 100644
--- a/client/i18n/en_us.h
+++ b/client/i18n/en_us.h
@@ -101,3 +101,15 @@
 #define W2_UI_ORDER_MSG_ORDER_DONE_ERR "no order active\n\n"
 #define W2_UI_ORDER_MSG_ORDER_SENT "order sent to robot\n\n"
 
+#define W2_UI_MODES_INFO \
+	"press keys (0-9) for:\n" \
+	"\n" \
+	"1 - set to maze mode\n" \
+	"2 - set to grid mode\n" \
+	"3 - halt (emergency stop)\n" \
+	"4 - set to charging station mode\n" \
+	"5 - set to spinning mode (wet floor simulation)\n" \
+	"6 - calibrate sensors\n" \
+	"\n" \
+	"0 - previous\n" \
+
diff --git a/client/ui.c b/client/ui.c
index 13bcfb2..cdf2f3b 100644
--- a/client/ui.c
+++ b/client/ui.c
@@ -75,6 +75,10 @@ void w2_ui_paint() {
 void w2_ui_paint_statusbar() {
 	char temp[g_w2_ui_width];
 
+	for (int i = 0; i < g_w2_ui_width; i++) temp[i] = ' ';
+	w2_wmvaddnstr(g_w2_ui_pad_statusbar, 0, 0, temp, g_w2_ui_width);
+	w2_wmvaddnstr(g_w2_ui_pad_statusbar, 1, 0, temp, g_w2_ui_width);
+
 	g_w2_state.connected ? sprintf(temp, W2_UI_CONN_STAT_CONNECTED ", %ims %s", g_w2_state.ping,
 								   W2_UI_CONN_STAT_PING)
 						 : sprintf(temp, W2_UI_CONN_STAT_DISCONNECTED);
diff --git a/client/ui_modes.c b/client/ui_modes.c
index 933f47b..829f807 100644
--- a/client/ui_modes.c
+++ b/client/ui_modes.c
@@ -1,10 +1,19 @@
+#include "commands.h"
 #include "ui.h"
+#include "i18n.h"
 
 void w2_ui_onkey_modes(int ch) {
+	if (ch == '1') w2_send_mode(W2_M_MAZE);
+	if (ch == '2') w2_send_mode(W2_M_GRID);
+	if (ch == '3') w2_send_mode(W2_M_HALT);
+	if (ch == '4') w2_send_mode(W2_M_CHRG);
+	if (ch == '5') w2_send_mode(W2_M_SPIN);
+	if (ch == '6') w2_send_mode(W2_M_SCAL);
 
+	if (ch == '0') w2_send_mode(W2_M_PREV);
 }
 
 void w2_ui_tab_modes(bool first) {
-
+	w2_wmvaddstr(g_w2_ui_pad_body, 0, 0, W2_UI_MODES_INFO);
 }
 
diff --git a/robot/mode_halt.c b/robot/mode_halt.c
index 88d6183..bc501a2 100644
--- a/robot/mode_halt.c
+++ b/robot/mode_halt.c
@@ -1,3 +1,6 @@
 #include "mode_halt.h"
+#include "orangutan_shim.h"
 
-void w2_mode_halt() { return; }
+void w2_mode_halt() {
+	set_motors(0, 0);
+}
diff --git a/robot/modes.c b/robot/modes.c
index 600c9a1..4995d6f 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -46,7 +46,7 @@ void w2_modes_switch(w2_e_mode new_mode, bool replace) {
 	// forward mode change to sercomm
 	W2_CREATE_MSG_BIN(w2_s_cmd_mode_tx, msg, msg_bin);
 	msg->opcode = W2_CMD_MODE | W2_CMDDIR_TX;
-	msg->mode	= new_mode;
+	msg->mode	= g_w2_mode_history[g_w2_mode_history_index];
 
 	w2_sercomm_append_msg(msg_bin);
 	free(msg_bin);