From bdf8e1c4e6669ee2254dd3401a91956e0ae6bb15 Mon Sep 17 00:00:00 2001
From: Simon Frei <simon-frei@hotmail.com>
Date: Thu, 22 Dec 2022 21:20:57 +0100
Subject: [PATCH] ref power save off

---
 ADIS_Sumo/Sumo/Application.c | 44 +++++++++++++++++++-----------------
 ADIS_Sumo/Sumo/Reflectance.c |  7 +++---
 2 files changed, 27 insertions(+), 24 deletions(-)

diff --git a/ADIS_Sumo/Sumo/Application.c b/ADIS_Sumo/Sumo/Application.c
index 201a71c..5952c52 100644
--- a/ADIS_Sumo/Sumo/Application.c
+++ b/ADIS_Sumo/Sumo/Application.c
@@ -291,11 +291,11 @@ static void StateMachine(bool buttonPress) {
       if (buttonPress) {
 #if (PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE) && !PL_APP_MINT /* Mint application handles buttons itself */
         if (LF_IsFollowing()) {
-          LF_StopFollowing();
-          appState = APP_STATE_IDLE;
+//          LF_StopFollowing();
+//          appState = APP_STATE_IDLE;
         } else {
-          LF_StartFollowing();
-          appState = APP_STATE_FOLLOW_LINE;
+//          LF_StartFollowing();
+//          appState = APP_STATE_FOLLOW_LINE;
         }
 #elif PL_APP_FOLLOW_OBSTACLE
         followObstacle = TRUE;
@@ -354,9 +354,9 @@ static void StateMachine(bool buttonPress) {
       McuLED_Off(LEDS_Left);
       McuLED_Off(LEDS_Right);
 #endif
-      if (!LF_IsFollowing()) {
-        appState = APP_STATE_IDLE;
-      }
+//      if (!LF_IsFollowing()) {
+//        appState = APP_STATE_IDLE;
+//      }
       if (buttonPress) {
         LF_StopFollowing(); 
         appState = APP_STATE_IDLE;
@@ -370,20 +370,23 @@ static void StateMachine(bool buttonPress) {
     		appState = APP_STATE_AUTO;
     		mazeFailure = 0;
     		MAZE_ResetPresentCount();
-    		presentCnt = MAZE_GetPresentCount();
+    		presentCnt = 0;
     		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);
+    		BUZ_Beep(1000, 1000);
+    		McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"AUTO\"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
+//			McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
     	}
+    	vTaskDelay(pdMS_TO_TICKS(10));
     	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);
+    		McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"FINAL\"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
+//			McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
     		appState = APP_STATE_IDLE;
+    		mazeFailure=0;
     	}else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure < 5){
     		// failed
     		mazeFailure++;
@@ -392,20 +395,19 @@ static void StateMachine(bool buttonPress) {
     		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);
+    		BUZ_Beep(1000, 1000);
+    		McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"FAILURE\"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
+//			McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
+			appState = APP_STATE_IDLE;
     	}
     	if(MAZE_GetPresentCount() != presentCnt){
     		presentCnt = MAZE_GetPresentCount();
     		//publish
-    		uint8_t counter[5] = "";
-    		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);
+    		BUZ_Beep(1000, 1000);
+    		McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/present/\" \"true\"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
+//    		McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
     	}
+    	vTaskDelay(pdMS_TO_TICKS(10));
     	break;
     default:
       break;
diff --git a/ADIS_Sumo/Sumo/Reflectance.c b/ADIS_Sumo/Sumo/Reflectance.c
index eec3c92..7f50d0d 100644
--- a/ADIS_Sumo/Sumo/Reflectance.c
+++ b/ADIS_Sumo/Sumo/Reflectance.c
@@ -605,11 +605,12 @@ uint8_t REF_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell
     return ERR_OK;
   } else if (McuUtility_strcmp((char*)cmd, "ref save power on")==0) {
     REF_Sensor.savePower = true;
-    McuGPIO_SetLow(REF_Sensor.irOn);; /* turn IR LED's off */
+    McuGPIO_SetLow(REF_Sensor.irOn); /* turn IR LED's off */
     *handled = TRUE;
     return ERR_OK;
   } else if (McuUtility_strcmp((char*)cmd, "ref save power off")==0) {
     REF_Sensor.savePower = false;
+    McuGPIO_SetHigh(REF_Sensor.irOn);
     *handled = TRUE;
     return ERR_OK;
   }
@@ -754,14 +755,14 @@ void REF_Init(void) {
   };
 
   REF_Sensor.isEnabled = true;
-  REF_Sensor.savePower = true; /* turn off, as this extends the ref sensor task time by around 200 us */
+  REF_Sensor.savePower = false; /* turn off, as this extends the ref sensor task time by around 200 us */
   McuGPIO_GetDefaultConfig(&gpioConfig);
   /* IR on/off: PTD1, high active */
   gpioConfig.hw.gpio = GPIOD;
   gpioConfig.hw.port = PORTD;
   gpioConfig.hw.pin = 1;
   gpioConfig.isInput = false;
-  gpioConfig.isHighOnInit = false;
+  gpioConfig.isHighOnInit = true;
   REF_Sensor.irOn = McuGPIO_InitGPIO(&gpioConfig);
   REF_LedOn = false;
   /* IR sensors */