Merge branch 'main' of gitlab.enterpriselab.ch:adis_team_gueti_roaster/adis_hs2022_team_4 into main

ADIS_Csharp/RaspiControl/Application.cs

@@ -32,6 +32,10 @@ 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();

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 },
@@ -101,18 +105,22 @@ namespace RaspiControl {
                     {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.JoystickUp), ProcessState.StartMoveManual },
-                    {new StateTransition(ProcessState.Auto, Command.JoystickDown), ProcessState.StartMoveManual },
+                    //{new StateTransition(ProcessState.Auto, Command.JoystickDown), ProcessState.StartMoveManual },
-                    {new StateTransition(ProcessState.Auto, Command.JoystickLeft), ProcessState.StartMoveManual },
+                    //{new StateTransition(ProcessState.Auto, Command.JoystickLeft), ProcessState.StartMoveManual },
-                    {new StateTransition(ProcessState.Auto, Command.JoystickRight), ProcessState.StartMoveManual },
+                    //{new StateTransition(ProcessState.Auto, Command.JoystickRight), ProcessState.StartMoveManual },
-                    {new StateTransition(ProcessState.Auto, Command.JoystickCenter), 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_ESP32_Eclipse/main/myMqtt.c

@@ -55,7 +55,7 @@ void MyMqtt_Deinit(void){
 }
 
 void MyMqtt_Publish(const char *topic, const char *data){
-    int msg_id = esp_mqtt_client_publish(client, topic, data, 0, 1, 0);
+    int msg_id = esp_mqtt_client_publish(client, topic, data, 0, 2, 0);
     ESP_LOGI(TAG, "sent publish successful, msg_id=%d", msg_id);
 }
 

ADIS_Sumo/Sumo/Application.c

@@ -366,15 +366,17 @@ 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;
     		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);
+			appState = APP_STATE_AUTO;
 //    		McuShell_SendStr((unsigned char*)"\r\n", McuESP32_GetTxToESPStdio()->stdOut);
     	}
     	vTaskDelay(pdMS_TO_TICKS(10));