add present counter, add state app_manual and auto

ADIS_Sumo/Sumo/Application.c

@@ -163,7 +163,9 @@ typedef enum {
 #if PL_CONFIG_APP_SUMO
   APP_STATE_RUN_SUMO, /* Sumo fight */
 #endif
-  APP_STATE_IDLE
+  APP_STATE_IDLE,
+  APP_STATE_MANUAL_MOVE,
+  APP_STATE_AUTO,
 } AppStateType;
 
 static AppStateType appState = APP_STATE_STARTUP;
@@ -211,6 +213,8 @@ static unsigned char *AppStateString(AppStateType state) {
 static void StateMachine(bool buttonPress) {
 #if PL_CONFIG_USE_LINE_SENSOR
   static uint8_t cnt;
+  static uint8_t mazeFailure;
+  static uint8_t presentCnt;
 #endif
   // TODO: send appstate via Mqtt
   switch (appState) {
@@ -357,6 +361,43 @@ static void StateMachine(bool buttonPress) {
       }
       break;
 #endif
+    case APP_STATE_MANUAL_MOVE:
+    	if(REF_GetLineKind(REF_LINE_KIND_MODE_MAZE) == REF_LINE_STRAIGHT){
+    		// send to mqtt AUTO
+    		SHELL_SendString((unsigned char*)"AUTO!!!\r\n");
+    		appState = APP_STATE_AUTO;
+    		mazeFailure = 0;
+    		MAZE_ResetPresentCount();
+    		presentCnt = MAZE_GetPresentCount();
+    		MAZE_ClearSolution();
+    		LF_StartFollowing();
+    	}
+    	break;
+    case APP_STATE_AUTO:
+    	if(!LF_IsFollowing() && MAZE_IsSolved()){
+    		//done
+    		SHELL_SendString((unsigned char*)"MAZE: done, stopped!!!\r\n");
+    		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;
+    	}
+    	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");
+    	}
+    	break;
     default:
       break;
   } /* switch */
@@ -456,6 +497,11 @@ static uint8_t AutoCalibrateReflectance(void) {
 }
 #endif
 
+static uint8_t SetManualMode(){
+	appState = APP_STATE_MANUAL_MOVE;
+	return ERR_OK;
+}
+
 #if 0 /* do not use this any more, use proper debouncing! */
 static void CheckButton(void) {
 #if PL_HAS_USER_BUTTON
@@ -1011,6 +1057,8 @@ static uint8_t APP_PrintHelp(const McuShell_StdIOType *io) {
 #if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN 
   McuShell_SendHelpStr((unsigned char*)"  autocalib", (unsigned char*)"Automatically calibrate line sensor\r\n", io->stdOut);
 #endif
+  //app manualdrive
+  McuShell_SendHelpStr((unsigned char*)"  manualdrive", (unsigned char*)"Robot can be driven manualy\r\n", io->stdOut);
   return ERR_OK;
 }
 
@@ -1048,6 +1096,9 @@ uint8_t APP_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell
   } else if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_STATUS)==0 || McuUtility_strcmp((char*)cmd, (char*)"app status")==0) {
     *handled = TRUE;
     return APP_PrintStatus(io);
+  }else if(McuUtility_strcmp((char*)cmd, (char*)"app manualdrive")==0){
+	  *handled = TRUE;
+	  return SetManualMode();
 #if PL_CONFIG_USE_LINE_SENSOR && PL_HAS_TURN 
   } else if (McuUtility_strcmp((char*)cmd, (char*)"app autocalib")==0) {
     *handled = TRUE;

ADIS_Sumo/Sumo/LineFollow.c

@@ -306,7 +306,7 @@ static void StateMachine(void) {
   #endif
         //SHELL_SendString((unsigned char *)"MAZE: Going back to start...\r\n");
         //LF_currState = STATE_FOLLOW_SEGMENT; /* go back to start */
-        MAZE_ClearSolution(); /* clear solution */
+        //MAZE_ClearSolution(); /* clear solution */
         LF_currState = STATE_STOP;
         break;
   #endif

ADIS_Sumo/Sumo/Maze.c

@@ -51,6 +51,15 @@ static TURN_Kind path[MAZE_MAX_PATH];
 static uint8_t pathLength;
 static bool isSolved = FALSE;
 static bool useLeftHandOnTheWallRule = TRUE;
+static uint8_t presentCounter = 0;
+
+uint8_t MAZE_ResetPresentCount(){
+	presentCounter = 0;
+}
+
+uint8_t MAZE_GetPresentCount(){
+	return presentCounter;
+}
 
 bool MAZE_IsLeftHandRule(void) {
   return useLeftHandOnTheWallRule;
@@ -207,6 +216,7 @@ uint8_t MAZE_EvaluteTurn(bool *finished, bool *deadEndGoBw) {
     }
 #endif
     TURN_Turn(turn, NULL); /* make turn */
+    presentCounter++;
     MAZE_AddPath(turn);
     MAZE_SimplifyPath();
     return ERR_OK; /* turn finished */

ADIS_Sumo/Sumo/Maze.h

@@ -11,6 +11,9 @@
 #if PL_CONFIG_APP_LINE_MAZE
 #include "Turn.h"
 
+uint8_t MAZE_ResetPresentCount();
+
+uint8_t MAZE_GetPresentCount();
 /*!
  * \brief Adds a new path while going forward through the maze
  * \param kind New path to be added

ADIS_Sumo/Sumo/Reflectance.c

@@ -754,7 +754,7 @@ void REF_Init(void) {
   };
 
   REF_Sensor.isEnabled = true;
-  REF_Sensor.savePower = false; /* turn off, as this extends the ref sensor task time by around 200 us */
+  REF_Sensor.savePower = true; /* 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;