added hex files (Sumo and SplitFlap project)

.gitignore

@@ -62,7 +62,6 @@ ProjectInfo.xml
 *.app
 *.i*86
 *.x86_64
-*.hex
 
 # Debug files
 *.dSYM/

ADIS_Csharp/RaspiControl/Application.cs

@@ -32,28 +32,33 @@ namespace RaspiControl {
 
         public void Status_StatusChanged(object? sender, StatusEventArgs e) {
             try {
+                if (e.Status == RoboStatus.Manual) {
+                    this.sm.MoveNext(Command.manualMode);
+                    this.smHandler();
+                }
                 if (e.Status == RoboStatus.Auto) {
                     this.sm.MoveNext(Command.autoMode);
                     this.smHandler();
                 }
-                if(e.Status == RoboStatus.Final) {
+                if (e.Status == RoboStatus.Final) {
                     this.sm.MoveNext(Command.finished);
                     this.smHandler();
                 }
-                if(e.Status == RoboStatus.Failure) {
+                if (e.Status == RoboStatus.Failure) {
                     this.sm.MoveNext(Command.failure);
                     this.smHandler();
                 }
-            }catch(Exception ex) {
+            } catch (Exception ex) {
                 this.log.Error(ex.Message);
             }
         }
 
-        public void Status_PresentChanged(object? sender, PresentEventArgs e) { 
+        public void Status_PresentChanged(object? sender, PresentEventArgs e) {
-            if(this.sm.CurrentState == ProcessState.Auto) {
+            //if(this.sm.CurrentState == ProcessState.Auto) {
-                this.PresentCounter++;
+            this.challenge.RobotStationary.SplitFlap.Display(e.Present.ToString().PadLeft(4, '0'));
-                this.challenge.RobotStationary.SplitFlap.Display(this.PresentCounter.ToString().PadLeft(4,'0'));
+            log.Debug(e.Present.ToString());
-            }
+            log.Debug(e.Present.ToString().PadLeft(4, '0'));
+            //}
         }
 
         public void Run() {
@@ -137,7 +142,7 @@ namespace RaspiControl {
                     this.PresentCounter = 0;
                     this.stopwatch.Stop();
                     long elapsedTime = stopwatch.ElapsedMilliseconds / 1000;
-                    this.challenge.RobotStationary.SplitFlap.Display(elapsedTime.ToString().PadLeft(4,'0'));
+                    this.challenge.RobotStationary.SplitFlap.Display($"{elapsedTime.ToString().PadLeft(3, '0')}S");
                     break;
                 case ProcessState.Error:
                     //this.PresentCounter = 0;
@@ -166,7 +171,7 @@ namespace RaspiControl {
                         this.sm.MoveNext(Command.JoystickCenter);
                         break;
                 }
-                if(e.Button != JoystickButton.None) {
+                if (e.Button != JoystickButton.None) {
                     this.smHandler();
                 }
             } catch (Exception ex) {

ADIS_Csharp/RaspiControl/NavigationConstants.cs

@@ -1,9 +1,9 @@
 namespace RaspiControl {
     public static class NavigationConstants {
-        public const int TURN_ANGLE_LEFT = -10;
+        public const int TURN_ANGLE_LEFT = -15;
-        public const int TURN_ANGLE_RIGHT = 10;
+        public const int TURN_ANGLE_RIGHT = 15;
-        public const int SPEED_FORWARD = 100;
+        public const int SPEED_FORWARD = 350;
-        public const int SPEED_BACKWARD = -100;
+        public const int SPEED_BACKWARD = -350;
         public const bool STOP = true;
     }
 }

ADIS_Csharp/RaspiControl/RaspiControl.csproj

@@ -8,7 +8,7 @@
   </PropertyGroup>
 
   <Target Name="PostBuild" AfterTargets="PostBuildEvent">
-    <Exec Command="cd &quot;$(TargetDir)&quot;&#xD;&#xA;&quot;$(SolutionDir)\SecureUpload.exe&quot; . pi-home:ADIS/$(ProjectName)" />
+    <Exec Command="cd &quot;$(TargetDir)&quot;&#xD;&#xA;&quot;$(SolutionDir)\SecureUpload.exe&quot; . pi-hslu:ADIS/$(ProjectName)" />
   </Target>
 
   <ItemGroup>

ADIS_Csharp/RaspiControl/StateMachine.cs

@@ -33,7 +33,8 @@ namespace RaspiControl {
         autoMode,
         finished,
         failure,
-        none
+        none,
+        manualMode
     }
     internal class StateMachine {
 
@@ -70,10 +71,13 @@ namespace RaspiControl {
                     {new StateTransition(ProcessState.Ready,Command.JoystickDown), ProcessState.StartMoveManual },
                     {new StateTransition(ProcessState.Ready,Command.JoystickLeft), ProcessState.StartMoveManual },
                     {new StateTransition(ProcessState.Ready,Command.JoystickRight), ProcessState.StartMoveManual },
+                    {new StateTransition(ProcessState.Ready,Command.autoMode), ProcessState.Auto },
+                    {new StateTransition(ProcessState.Ready,Command.manualMode), ProcessState.Ready },
 
 
                     {new StateTransition(ProcessState.StartMoveManual,Command.none), ProcessState.MoveManual },
                     {new StateTransition(ProcessState.StartMoveManual, Command.autoMode), ProcessState.Auto },
+                    {new StateTransition(ProcessState.StartMoveManual, Command.manualMode), ProcessState.StartMoveManual },
 
                     {new StateTransition(ProcessState.MoveManualF, Command.none), ProcessState.MoveManual },
                     {new StateTransition(ProcessState.MoveManualF, Command.autoMode), ProcessState.Auto },
@@ -90,29 +94,30 @@ namespace RaspiControl {
                     {new StateTransition(ProcessState.MoveManual, Command.JoystickRight), ProcessState.MoveManualR },
                     {new StateTransition(ProcessState.MoveManual, Command.JoystickCenter), ProcessState.StopManual },
                     {new StateTransition(ProcessState.MoveManual, Command.autoMode), ProcessState.Auto },
+                    {new StateTransition(ProcessState.MoveManual, Command.finished), ProcessState.Final },
 
                     {new StateTransition(ProcessState.StopManual,Command.JoystickUp), ProcessState.ResumeMoveManual },
                     {new StateTransition(ProcessState.StopManual,Command.JoystickDown), ProcessState.ResumeMoveManual },
                     {new StateTransition(ProcessState.StopManual,Command.JoystickLeft), ProcessState.ResumeMoveManual },
                     {new StateTransition(ProcessState.StopManual,Command.JoystickRight), ProcessState.ResumeMoveManual },
                     {new StateTransition(ProcessState.StopManual, Command.autoMode), ProcessState.Auto },
+                    {new StateTransition(ProcessState.StopManual, Command.finished), ProcessState.Final },
 
                     {new StateTransition(ProcessState.ResumeMoveManual, Command.none),ProcessState.MoveManual },
                     {new StateTransition(ProcessState.ResumeMoveManual, Command.autoMode), ProcessState.Auto },
 
                     {new StateTransition(ProcessState.Auto, Command.finished), ProcessState.Final },
-                    {new StateTransition(ProcessState.Auto, Command.JoystickUp), ProcessState.StartMoveManual },
-                    {new StateTransition(ProcessState.Auto, Command.JoystickDown), ProcessState.StartMoveManual },
-                    {new StateTransition(ProcessState.Auto, Command.JoystickLeft), ProcessState.StartMoveManual },
-                    {new StateTransition(ProcessState.Auto, Command.JoystickRight), ProcessState.StartMoveManual },
-                    {new StateTransition(ProcessState.Auto, Command.JoystickCenter), ProcessState.StartMoveManual },
                     {new StateTransition(ProcessState.Auto, Command.failure), ProcessState.Error },
+                    {new StateTransition(ProcessState.Auto, Command.manualMode), ProcessState.ResumeMoveManual },
+
+                    {new StateTransition(ProcessState.Error, Command.JoystickUp), ProcessState.ResumeMoveManual },
+                    {new StateTransition(ProcessState.Error, Command.JoystickDown), ProcessState.ResumeMoveManual },
+                    {new StateTransition(ProcessState.Error, Command.JoystickLeft), ProcessState.ResumeMoveManual },
+                    {new StateTransition(ProcessState.Error, Command.JoystickRight), ProcessState.ResumeMoveManual },
+                    {new StateTransition(ProcessState.Error, Command.JoystickCenter), ProcessState.ResumeMoveManual },
+                    {new StateTransition(ProcessState.Error, Command.finished), ProcessState.Final },
+                    {new StateTransition(ProcessState.Error, Command.manualMode), ProcessState.ResumeMoveManual },
 
-                    {new StateTransition(ProcessState.Error, Command.JoystickUp), ProcessState.StartMoveManual },
-                    {new StateTransition(ProcessState.Error, Command.JoystickDown), ProcessState.StartMoveManual },
-                    {new StateTransition(ProcessState.Error, Command.JoystickLeft), ProcessState.StartMoveManual },
-                    {new StateTransition(ProcessState.Error, Command.JoystickRight), ProcessState.StartMoveManual },
-                    {new StateTransition(ProcessState.Error, Command.JoystickCenter), ProcessState.StartMoveManual },
                 };
         }
 

ADIS_Csharp/RobotClientWpf/MainWindow.xaml

@@ -12,7 +12,7 @@
         <Grid.RowDefinitions>
             <RowDefinition Height="40"/>
             <RowDefinition Height="*"/>
-            <RowDefinition Height="260"/>
+            <RowDefinition Height="220"/>
             <!-- <RowDefinition Height="30"/>  Bottom message (NOT USED) -->
         </Grid.RowDefinitions>
 

ADIS_Csharp/RobotClientWpf/MainWindow.xaml.cs

@@ -61,7 +61,7 @@ namespace RobotClientWpf
             // set color according to assessed state
             if(e.Online == false)
             {
-                log.Warn("Robot Stationary seems to be offline.");
+                log.Warn($"Robot Stationary seems to be offline. No response in {e.Seconds}s.");
             }
             else
             {
@@ -75,7 +75,7 @@ namespace RobotClientWpf
             // set color according to assessed state
             if (e.Online == false)
             {
-                log.Warn("Robot Mobile seems to be offline.");
+                log.Warn($"Robot Mobile seems to be offline. No response in {e.Seconds}s.");
             }
             else
             {

ADIS_Csharp/RobotClientWpf/Views/MainView.xaml

@@ -5,7 +5,7 @@
              xmlns:d="http://schemas.microsoft.com/expression/blend/2008" 
              xmlns:local="clr-namespace:RobotClientWpf.Views"
              mc:Ignorable="d" 
-             d:DesignHeight="430" d:DesignWidth="1200" FontSize="15" KeyDown="UserControl_KeyDown">
+             d:DesignHeight="430" d:DesignWidth="1200" FontSize="15">
     <Grid>
         <Grid.ColumnDefinitions>
             <ColumnDefinition Width="*"/>
@@ -19,7 +19,10 @@
         <!-- RIGHT SIDE -->
         <StackPanel Orientation="Vertical" Grid.Row="0" Grid.Column="1" Grid.RowSpan="2">
             <GroupBox Header="Splitflap Display" Margin="10" HorizontalAlignment="Right">
-                <TextBox x:Name="tbSplitflapText" Height="100" Width="300" FontSize="40" IsReadOnly="True" TextWrapping="NoWrap" HorizontalAlignment="Left"/>
+                <TextBox x:Name="tbSplitflapText" Height="100" Width="250" FontSize="70" IsReadOnly="True" TextWrapping="NoWrap" TextAlignment="Center"/>
+            </GroupBox>
+            <GroupBox Header="Presents Counted" Margin="10" HorizontalAlignment="Right">
+                <TextBox x:Name="tbPresentCount" Height="100" Width="250" FontSize="70" IsReadOnly="True" TextWrapping="NoWrap" TextAlignment="Center"/>
             </GroupBox>
         </StackPanel>
 

ADIS_Csharp/RobotClientWpf/Views/MainView.xaml.cs

@@ -1,4 +1,5 @@
 using RobotClientWpf.Utilities;
+using RobotLib.Status;
 using System.Windows.Controls;
 using System.Windows.Input;
 
@@ -12,7 +13,7 @@ namespace RobotClientWpf.Views
         private static readonly NLog.Logger log = NLog.LogManager.GetCurrentClassLogger();
         private ChallengeFactory? challenge;
         private MainWindow? mainWindow;
-        private const int AMOUNT_SPEED_ADDED_PER_CLICK = 200;
+        private const int AMOUNT_SPEED_ADDED_PER_CLICK = 350;
         private const int TURN_ANGLE_PER_CLICK = 30;
 
         public MainView()
@@ -27,6 +28,22 @@ namespace RobotClientWpf.Views
             // subscribe to events
             this.challenge.RobotStationary.SplitFlap.SplitFlapDisplayChanged += this.SplitFlap_SplitFlapDisplayChanged;
             this.challenge.RobotMobile.Battery.BatteryChanged += Battery_BatteryChanged;
+            this.challenge.RobotMobile.Status.PresentChanged += Status_PresentChanged;
+            this.challenge.RobotMobile.Status.StatusChanged += Status_StatusChanged;
+        }
+
+        private void Status_StatusChanged(object? sender, RobotLib.Status.StatusEventArgs e)
+        {
+            // if status manual is set => reset present counter
+            if(e.Status == RoboStatus.Manual || e.Status == RoboStatus.Auto)
+            {
+                UIAccessHelpers.SetTextboxText(tbPresentCount, "");
+            }
+        }
+
+        private void Status_PresentChanged(object? sender, RobotLib.Status.PresentEventArgs e)
+        {
+            UIAccessHelpers.SetTextboxText(tbPresentCount, e.Present.ToString());
         }
 
         private void Battery_BatteryChanged(object? sender, RobotLib.Battery.BatteryEventArgs e)
@@ -101,10 +118,5 @@ namespace RobotClientWpf.Views
                 e.Handled = true;
             }
         }
-
-        private void UserControl_KeyDown(object sender, KeyEventArgs e)
-        {
-            this.HandleKeyDownEvent(sender, e);
-        }
     }
 }

ADIS_Csharp/RobotLib/Battery/DevBattery.cs

@@ -11,7 +11,7 @@ namespace RobotLib.Battery
     {
         private float voltage;
         private int lastResponseS = 1000;  // seconds since last response
-        private const int offlineTresholdS = 11;   // treshold when to assess a robot as offline (no response for x sec)
+        private const int offlineTresholdS = 25;   // treshold when to assess a robot as offline (no response for x sec)
         private Stopwatch stopwatchLastResponse = new();
 
         private const string TOPIC_ROBO_REQ_BATTERY = "/both/cmd/battery/get_volt";

ADIS_Csharp/RobotLib/Status/DevStatus.cs

@@ -23,8 +23,14 @@ namespace RobotLib.Status {
                 StatusEventArgs eventArgs = new(message);
                 StatusChanged?.Invoke(this, eventArgs);
             }else if(fromTopic == TOPIC_STATUS_PRESENT) {
-                PresentEventArgs presentEventArgs = new(true);
+                Int16 presentCnt = 0;
-                PresentChanged?.Invoke(this, presentEventArgs);
+                if (Int16.TryParse(message, out presentCnt)) {
+                    PresentEventArgs presentEventArgs = new(presentCnt);
+                    PresentChanged?.Invoke(this, presentEventArgs);
+                } else {
+                    log.Error($"Invalid payload received {message}");
+                }
+                
             }
         }
 

ADIS_Csharp/RobotLib/Status/PresentEventArgs.cs

@@ -6,10 +6,11 @@ using System.Threading.Tasks;
 
 namespace RobotLib.Status {
     public class PresentEventArgs {
-        public bool Present { get; }
+        public int Present { get; }
 
-        public PresentEventArgs(bool present) {
+        public PresentEventArgs(int presentCount) {
-            Present = present;
+            Present = presentCount;
+           
         }
     }
 }

ADIS_ESP32_Eclipse/main/myMqtt.c

@@ -20,6 +20,7 @@ static const char *TAG = "MY_MQTT";
 
 // local cars
 char _brokerIp[] = "255.255.255.255";  // max length of ip
+SemaphoreHandle_t semaphoreMqtt = NULL;
 
 // declarations
 static void log_error_if_nonzero(const char *message, int error_code);
@@ -29,8 +30,16 @@ static void mqtt_event_handler(void *handler_args, esp_event_base_t base, int32_
 esp_mqtt_client_handle_t client;
 
 bool MyMqtt_Init(void){
+	// initialize semaphore
+	semaphoreMqtt = xSemaphoreCreateRecursiveMutex();
+	if(semaphoreMqtt == NULL){
+		ESP_LOGE(TAG, "Failed to initialize semaphore for MyMqtt!");
+	}else{
+		vQueueAddToRegistry(semaphoreMqtt, "mymqtt_semphr");
+	}
+
+	// get mqtt broker IP from NVS
 	char brokerUri[50];
-	// get from NVS
 	Challenge_Nvs_GetBrokerIpFromNVS(_brokerIp, sizeof(_brokerIp));
 	McuUtility_strcpy((unsigned char*)brokerUri, sizeof(brokerUri), (unsigned char*)"mqtt://");
 	McuUtility_strcat((unsigned char*)brokerUri, sizeof(brokerUri), (unsigned char*)_brokerIp);
@@ -54,14 +63,35 @@ void MyMqtt_Deinit(void){
 	esp_mqtt_client_destroy(client);
 }
 
+/* HELPERS FOR SEMAPHORE */
+static bool MyMqtt_AquireMutex(void){
+	if(xSemaphoreTakeRecursive(semaphoreMqtt, pdMS_TO_TICKS(1000)) != pdTRUE){
+		/* timeout */
+		ESP_LOGW(TAG, "Timemout while aquiring myMqtt semaphore.");
+		return false;
+	}
+	return true;
+}
+
+static void MyMqtt_ReleaseMutex(void){
+	xSemaphoreGive(semaphoreMqtt);
+}
+
+
 void MyMqtt_Publish(const char *topic, const char *data){
-    int msg_id = esp_mqtt_client_publish(client, topic, data, 0, 1, 0);
+	if(MyMqtt_AquireMutex()){
-    ESP_LOGI(TAG, "sent publish successful, msg_id=%d", msg_id);
+		int msg_id = esp_mqtt_client_publish(client, topic, data, 0, 2, 0);
+		ESP_LOGI(TAG, "sent publish successful, msg_id=%d", msg_id);
+		MyMqtt_ReleaseMutex();
+	}
 }
 
 void MyMqtt_Subscribe(const char *topic){
-    int msg_id = esp_mqtt_client_subscribe(client, topic, 0);
+	if(MyMqtt_AquireMutex()){
-    ESP_LOGI(TAG, "sent subscribe successful, msg_id=%d, success=%s", msg_id, msg_id==-1 ? "false" : "true");
+		int msg_id = esp_mqtt_client_subscribe(client, topic, 0);
+		ESP_LOGI(TAG, "sent subscribe successful, msg_id=%d, success=%s", msg_id, msg_id==-1 ? "false" : "true");
+		MyMqtt_ReleaseMutex();
+	}
 }
 
 char* MyMqtt_GetBrokerIP(void){

ADIS_Sumo/.cproject

@@ -18,7 +18,7 @@
 				</extensions>
 			</storageModule>
 			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration artifactExtension="axf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="Debug build" errorParsers="org.eclipse.cdt.core.CWDLocator;org.eclipse.cdt.core.GmakeErrorParser;org.eclipse.cdt.core.GCCErrorParser;org.eclipse.cdt.core.GLDErrorParser;org.eclipse.cdt.core.GASErrorParser" id="com.crt.advproject.config.exe.debug.962273250" name="Debug" parent="com.crt.advproject.config.exe.debug" postannouncebuildStep="Performing post-build steps" postbuildStep="arm-none-eabi-size &quot;${BuildArtifactFileName}&quot;; # arm-none-eabi-objcopy -v -O binary &quot;${BuildArtifactFileName}&quot; &quot;${BuildArtifactFileBaseName}.bin&quot; ; # checksum -p ${TargetChip} -d &quot;${BuildArtifactFileBaseName}.bin&quot;;  ">
+				<configuration artifactExtension="axf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="Debug build" errorParsers="org.eclipse.cdt.core.CWDLocator;org.eclipse.cdt.core.GmakeErrorParser;org.eclipse.cdt.core.GCCErrorParser;org.eclipse.cdt.core.GLDErrorParser;org.eclipse.cdt.core.GASErrorParser" id="com.crt.advproject.config.exe.debug.962273250" name="Debug" parent="com.crt.advproject.config.exe.debug" postannouncebuildStep="Performing post-build steps" postbuildStep="arm-none-eabi-size &quot;${BuildArtifactFileName}&quot; ; arm-none-eabi-objcopy -v -O binary &quot;${BuildArtifactFileName}&quot; &quot;${BuildArtifactFileBaseName}.bin&quot; ; arm-none-eabi-objcopy -v -O srec &quot;${BuildArtifactFileName}&quot; &quot;${BuildArtifactFileBaseName}.s19&quot; --srec-len=64 ; arm-none-eabi-objcopy -v -O ihex &quot;${BuildArtifactFileName}&quot; &quot;${BuildArtifactFileBaseName}.hex&quot; ; # arm-none-eabi-objcopy -v -O binary &quot;${BuildArtifactFileName}&quot; &quot;${BuildArtifactFileBaseName}.bin&quot; ; # checksum -p ${TargetChip} -d &quot;${BuildArtifactFileBaseName}.bin&quot;">
 					<folderInfo id="com.crt.advproject.config.exe.debug.962273250." name="/" resourcePath="">
 						<toolChain id="com.crt.advproject.toolchain.exe.debug.1592834671" name="NXP MCU Tools" superClass="com.crt.advproject.toolchain.exe.debug">
 							<targetPlatform binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.GNU_ELF" id="com.crt.advproject.platform.exe.debug.1631684109" name="ARM-based MCU (Debug)" superClass="com.crt.advproject.platform.exe.debug"/>

ADIS_Sumo/Sumo/Application.c

@@ -215,7 +215,7 @@ 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 mazeFailure;
   static uint8_t presentCnt;
 #endif
   // TODO: send appstate via Mqtt
@@ -366,16 +366,18 @@ static void StateMachine(bool buttonPress) {
     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");
+//    		SHELL_SendString((unsigned char*)"AUTO!!!\r\n");
-    		appState = APP_STATE_AUTO;
+
-    		mazeFailure = 0;
+//    		mazeFailure = 0;
     		MAZE_ResetPresentCount();
     		presentCnt = 0;
     		MAZE_ClearSolution();
     		LF_StartFollowing();
-    		BUZ_Beep(1000, 1000);
+//    		BUZ_Beep(2000, 1000);
+    		BUZ_PlayTune(BUZ_TUNE_WELCOME);
     		McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/mode/\" \"AUTO\"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
-//			McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
+			appState = APP_STATE_AUTO;
+//    		McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
     	}
     	vTaskDelay(pdMS_TO_TICKS(10));
     	break;
@@ -386,25 +388,31 @@ static void StateMachine(bool buttonPress) {
     		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;
+//    		mazeFailure=0;
-    	}else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure < 5){
+    	}else if(!LF_IsFollowing() && !MAZE_IsSolved()){// && mazeFailure < 5){
     		// failed
-    		mazeFailure++;
+//    		mazeFailure++;
     		MAZE_ClearSolution();
     		DRV_SetSpeed(0, 0);
     		DRV_SetMode(DRV_MODE_SPEED);
 			LF_StartFollowing();
-    	}else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure >= 5){
-    		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;
     	}
+//    	}else if(!LF_IsFollowing() && !MAZE_IsSolved() && mazeFailure >= 5){
+//    		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
     		BUZ_Beep(1000, 1000);
-    		McuShell_SendStr((unsigned char*)"mqtt publish \"/mobile/status/present/\" \"true\"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
+    		unsigned char payload[50] = "mqtt publish \"/mobile/status/present/\" \"";
+    		unsigned char number[4] = "";
+    		McuUtility_Num8uToStr(number, sizeof(number), presentCnt);
+    		McuUtility_strcat(payload, sizeof(payload), number);
+    		McuUtility_strcat(payload, sizeof(payload), (unsigned char*)"\"\r\n");
+    		McuShell_SendStr(payload, McuESP32_GetTxToESPStdio()->stdOut);
 //    		McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
     	}
     	vTaskDelay(pdMS_TO_TICKS(10));
@@ -510,8 +518,7 @@ 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*)"mqtt publish \"/mobile/status/mode/\" \"MANUAL\"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
-	McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
 	return ERR_OK;
 }
 

ADIS_Sumo/Sumo/Turn.c

@@ -63,11 +63,11 @@
     #define TURN_STEPS_POST_LINE_TIMEOUT_MS 500
     #define TURN_STEPS_STOP_TIMEOUT_MS      150
   #elif PL_IS_INTRO_ZUMO_K22 /* no LiPo */
-    #define TURN_STEPS_90         650//720
+    #define TURN_STEPS_90         720
       /*!< number of steps for a 90 degree turn */
-    #define TURN_STEPS_LINE       170//100 /*230*/
+    #define TURN_STEPS_LINE       100 /*230*/
       /*!< number of steps stepping over the line */
-    #define TURN_STEPS_POST_LINE  240//150
+    #define TURN_STEPS_POST_LINE  150
       /*!< number of steps after the line, before making a turn */
     #define TURN_STEPS_90_TIMEOUT_MS        1000
     #define TURN_STEPS_LINE_TIMEOUT_MS      200
@@ -575,7 +575,7 @@ uint8_t TURN_ParseCommand(const unsigned char *cmd, bool *handled, const McuShel
     if (McuUtility_xatoi(&p, &angle)==ERR_OK) {
       TURN_TurnAngle((int16_t)angle, NULL);
       TURN_Turn(TURN_STOP, NULL);
-      DRV_SetSpeed(100, 100);
+      DRV_SetSpeed(400, 400);
       DRV_SetMode(DRV_MODE_SPEED);
       *handled = TRUE;
     } else {

ADIS_tinyK22_SplitFlap/.cproject

@@ -18,7 +18,7 @@
 				</extensions>
 			</storageModule>
 			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration artifactExtension="axf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="Debug build" errorParsers="org.eclipse.cdt.core.CWDLocator;org.eclipse.cdt.core.GmakeErrorParser;org.eclipse.cdt.core.GCCErrorParser;org.eclipse.cdt.core.GLDErrorParser;org.eclipse.cdt.core.GASErrorParser" id="com.crt.advproject.config.exe.debug.198189932" name="Debug" parent="com.crt.advproject.config.exe.debug" postannouncebuildStep="Performing post-build steps" postbuildStep="arm-none-eabi-size &quot;${BuildArtifactFileName}&quot;; # arm-none-eabi-objcopy -v -O binary &quot;${BuildArtifactFileName}&quot; &quot;${BuildArtifactFileBaseName}.bin&quot; ; # checksum -p ${TargetChip} -d &quot;${BuildArtifactFileBaseName}.bin&quot;;  ">
+				<configuration artifactExtension="axf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="Debug build" errorParsers="org.eclipse.cdt.core.CWDLocator;org.eclipse.cdt.core.GmakeErrorParser;org.eclipse.cdt.core.GCCErrorParser;org.eclipse.cdt.core.GLDErrorParser;org.eclipse.cdt.core.GASErrorParser" id="com.crt.advproject.config.exe.debug.198189932" name="Debug" parent="com.crt.advproject.config.exe.debug" postannouncebuildStep="Performing post-build steps" postbuildStep="arm-none-eabi-size &quot;${BuildArtifactFileName}&quot; ; arm-none-eabi-objcopy -v -O binary &quot;${BuildArtifactFileName}&quot; &quot;${BuildArtifactFileBaseName}.bin&quot; ; arm-none-eabi-objcopy -v -O srec &quot;${BuildArtifactFileName}&quot; &quot;${BuildArtifactFileBaseName}.s19&quot; --srec-len=64 ; arm-none-eabi-objcopy -v -O ihex &quot;${BuildArtifactFileName}&quot; &quot;${BuildArtifactFileBaseName}.hex&quot; ; # arm-none-eabi-objcopy -v -O binary &quot;${BuildArtifactFileName}&quot; &quot;${BuildArtifactFileBaseName}.bin&quot; ; # checksum -p ${TargetChip} -d &quot;${BuildArtifactFileBaseName}.bin&quot;">
 					<folderInfo id="com.crt.advproject.config.exe.debug.198189932." name="/" resourcePath="">
 						<toolChain id="com.crt.advproject.toolchain.exe.debug.1408014601" name="NXP MCU Tools" superClass="com.crt.advproject.toolchain.exe.debug">
 							<targetPlatform binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.GNU_ELF" id="com.crt.advproject.platform.exe.debug.1933628153" name="ARM-based MCU (Debug)" superClass="com.crt.advproject.platform.exe.debug"/>