publish status and presentcount

ADIS_Sumo/Sumo/Application.c

@@ -144,7 +144,9 @@
 #if PL_HAS_MIDI
   #include "MidiMusic.h"
 #endif
-
+#if PL_CONFIG_USE_ESP32
+#include "McuESP32.h"
+#endif
 typedef enum {
   APP_STATE_STARTUP,
   APP_STATE_INIT,
@@ -371,23 +373,28 @@ static void StateMachine(bool buttonPress) {
     		presentCnt = MAZE_GetPresentCount();
     		MAZE_ClearSolution();
     		LF_StartFollowing();
+    		McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"AUTO\"", McuESP32_GetTxToESPStdio()->stdOut);
+			McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
     	}
     	break;
     case APP_STATE_AUTO:
     	if(!LF_IsFollowing() && MAZE_IsSolved()){
     		//done
     		SHELL_SendString((unsigned char*)"MAZE: done, stopped!!!\r\n");
+    		McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"FINAL\"", McuESP32_GetTxToESPStdio()->stdOut);
+			McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
     		appState = APP_STATE_IDLE;
     	}else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure < 5){
     		// failed
     		mazeFailure++;
-    		SHELL_SendString((unsigned char*)"MAZE: Failure, stopped OMG!!!\r\n");
     		MAZE_ClearSolution();
     		DRV_SetSpeed(0, 0);
     		DRV_SetMode(DRV_MODE_SPEED);
 			LF_StartFollowing();
     	}else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure >= 5){
     		appState = APP_STATE_IDLE;
+    		McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"FAILURE\"", McuESP32_GetTxToESPStdio()->stdOut);
+			McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
     	}
     	if(MAZE_GetPresentCount() != presentCnt){
     		presentCnt = MAZE_GetPresentCount();
@@ -396,6 +403,8 @@ static void StateMachine(bool buttonPress) {
     		McuUtility_Num8uToStr(counter, sizeof(counter), presentCnt);
     		SHELL_SendString((unsigned char*)counter);
     		SHELL_SendString((unsigned char*)"\r\n");
+    		McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/present/\" \"true\"", McuESP32_GetTxToESPStdio()->stdOut);
+    		McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
     	}
     	break;
     default:
@@ -499,6 +508,8 @@ static uint8_t AutoCalibrateReflectance(void) {
 
 static uint8_t SetManualMode(){
 	appState = APP_STATE_MANUAL_MOVE;
+	McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"MANUAL\"", McuESP32_GetTxToESPStdio()->stdOut);
+	McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
 	return ERR_OK;
 }