implemented line sensor calibration in ESP and UI,

implemented battery voltage display

ADIS_Csharp/RobotClientWpf/MainWindow.xaml.cs

@@ -6,7 +6,7 @@ using System.Windows;
 using System.Windows.Media;
 using RobotClientWpf.Properties;
 using RobotClientWpf.Utilities;
-using RobotLib;
+using RobotLib.Battery;
 
 namespace RobotClientWpf
 {

ADIS_Csharp/RobotClientWpf/Views/ConfigView.xaml

@@ -12,7 +12,7 @@
         </Grid.ColumnDefinitions>
         <Grid.RowDefinitions>
             <RowDefinition Height="120"/>
-            <RowDefinition Height="120"/>
+            <RowDefinition Height="150"/>
             <RowDefinition Height="*"/>
         </Grid.RowDefinitions>
 
@@ -36,8 +36,18 @@
                     <Button x:Name="btnStartCalibration" Content="Start Calibration" Margin="5" Width="200" Height="30" Click="btnStartCalibration_Click" />
                     <Button x:Name="btnEndCalibration" Content="End Calibration" Margin="5" Width="200" Height="30" Click="btnEndCalibration_Click" />
                 </StackPanel>
-                
+
-                <Button DockPanel.Dock="Top" x:Name="btnGetCalibrationData" Content="Get Calibration Data" Margin="5" Width="200" Height="30" Click="btnGetCalibrationData_Click"/>
+                <StackPanel DockPanel.Dock="Top" Orientation="Horizontal" Margin="5" >
+                    <Label Content="Calibration state:"/>
+                    <TextBox x:Name="tbCalibrationState" IsReadOnly="True" Width="200" Height="30"/>
+                </StackPanel>
+
+                <StackPanel DockPanel.Dock="Top" Orientation="Horizontal" Margin="5" >
+                    <Label Content="Calibration data:"/>
+                    <TextBox x:Name="tbCalibrationData" IsReadOnly="True" Width="500" Height="30"/>
+                </StackPanel>
+
+                <Button x:Name="btnGetCalibrationData" DockPanel.Dock="Right" Content="Get State" Margin="10 0" Width="100" Height="30" Click="btnGetCalibrationData_Click"/>
             </DockPanel>
         </GroupBox>
     </Grid>

ADIS_Csharp/RobotClientWpf/Views/ConfigView.xaml.cs

@@ -22,6 +22,7 @@ namespace RobotClientWpf.Views
         public void SetChallengeFactory(ChallengeFactory challenge)
         {
             this.challenge = challenge;
+            this.challenge.RobotMobile.LineSensor.NewCalibrationDataArrived += LineSensor_NewCalibrationDataArrived;
         }
 
         private void btnApplyConfiguration_Click(object sender, RoutedEventArgs e)
@@ -70,11 +71,18 @@ namespace RobotClientWpf.Views
         private void btnEndCalibration_Click(object sender, RoutedEventArgs e)
         {
             this.challenge?.RobotMobile.LineSensor.StartCalibration(false);
+            this.challenge?.RobotMobile.LineSensor.GetCalibrationData();
         }
 
         private void btnGetCalibrationData_Click(object sender, RoutedEventArgs e)
         {
             this.challenge?.RobotMobile.LineSensor.GetCalibrationData();
         }
+
+        private void LineSensor_NewCalibrationDataArrived(object? sender, RobotLib.Movement.LineSensorCalibrationDataEventArgs e)
+        {
+            UIAccessHelpers.SetTextboxText(this.tbCalibrationData, e.CalibrationValues);
+            UIAccessHelpers.SetTextboxText(this.tbCalibrationState, e.State);
+        }
     }
 }

ADIS_Csharp/RobotClientWpf/Views/MainView.xaml

@@ -15,9 +15,21 @@
             <RowDefinition Height="150"/>
             <RowDefinition Height="*"/>
         </Grid.RowDefinitions>
-        <GroupBox Grid.Row="0" Grid.Column="1" Header="Splitflap Display" Margin="10" HorizontalAlignment="Right">
+
-            <TextBox x:Name="tbSplitflapText" Height="70" Width="150" FontSize="30" IsReadOnly="True" TextWrapping="NoWrap"/>
+        <!-- RIGHT SIDE -->
-        </GroupBox>
+        <StackPanel Orientation="Vertical" Grid.Row="0" Grid.Column="1" Grid.RowSpan="2">
+            <GroupBox Header="Splitflap Display" Margin="10" HorizontalAlignment="Right" Width="180">
+                <TextBox x:Name="tbSplitflapText" Height="70" Width="150" FontSize="30" IsReadOnly="True" TextWrapping="NoWrap" HorizontalAlignment="Left"/>
+            </GroupBox>
+            <GroupBox Header="Robot battery" Margin="10" HorizontalAlignment="Right" Width="180">
+                <StackPanel Orientation="Horizontal">
+                    <TextBox x:Name="tbRoboVoltage" Height="25" Width="150" FontSize="30" IsReadOnly="True" TextWrapping="NoWrap" HorizontalAlignment="Left"/>
+                    <Label Content="V" Margin="10 0"/>
+                </StackPanel>
+            </GroupBox>
+        </StackPanel>
+
+        <!-- LEFT SIDE -->
         <DockPanel>
             <GroupBox DockPanel.Dock="Left" Grid.Column="0" Header="Robot Control" Width="200">
                 <StackPanel Orientation="Vertical" Margin="5">

ADIS_Csharp/RobotClientWpf/Views/MainView.xaml.cs

@@ -25,6 +25,12 @@ namespace RobotClientWpf.Views
             this.challenge = challenge;
             // subscribe to events
             this.challenge.RobotStationary.SplitFlap.SplitFlapDisplayChanged += this.SplitFlap_SplitFlapDisplayChanged;
+            this.challenge.RobotMobile.Battery.BatteryChanged += Battery_BatteryChanged;
+        }
+
+        private void Battery_BatteryChanged(object? sender, RobotLib.Battery.BatteryEventArgs e)
+        {
+            UIAccessHelpers.SetTextboxText(tbRoboVoltage, e.Voltage.ToString());
         }
 
         public void SplitFlap_SplitFlapDisplayChanged(object? sender, RobotLib.SplitFlap.SplitFlapDisplayEventArgs e)

ADIS_Csharp/RobotLib/Battery/BatteryEventArgs.cs

@@ -1,6 +1,6 @@
 using System;
 
-namespace RobotLib
+namespace RobotLib.Battery
 {
     public class BatteryEventArgs : EventArgs
     {

ADIS_Csharp/RobotLib/Battery/DevBattery.cs

@@ -0,0 +1,66 @@
+using RobotLib.Communication;
+using System;
+using System.Collections.Generic;
+using System.Text.Json;
+
+namespace RobotLib.Battery
+{
+    public class DevBattery : DevBase
+    {
+        private float voltage;
+
+        private const string TOPIC_ROBO_REQ_BATTERY = "/mobile/cmd/battery/get_volt";
+        private const string TOPIC_ROBO_RESP_BATTERY = "/mobile/state/battery/voltage";
+
+        public event EventHandler<BatteryEventArgs> BatteryChanged;
+
+        public float Voltage
+        {
+            get { return voltage; }
+            private set
+            {
+                // value changed?
+                if (voltage != value)
+                {
+                    // set new voltage and raise event
+                    voltage = value;
+                    BatteryChanged?.Invoke(this, new BatteryEventArgs(voltage));
+                }
+            }
+        }
+
+        public DevBattery(IPublisherSubscriber com) : base(com, new List<string>() { TOPIC_ROBO_RESP_BATTERY }) { }
+
+        public override void Refresh()
+        {
+            this.RequestBatteryVoltage();
+        }
+
+        public void RequestBatteryVoltage()
+        {
+            base.SendMessage(TOPIC_ROBO_REQ_BATTERY, true.ToString());
+        }
+
+        protected override void ParseMessage(string fromTopic, string message)
+        {
+            if (fromTopic == TOPIC_ROBO_RESP_BATTERY)
+            {
+                var parsedVoltageString = GetValueFromMesage<string>("voltage", message);
+                if (parsedVoltageString == null) parsedVoltageString = "?";
+
+                // example message = "Battery: 1.25 V"
+                var valueUnit = message.Trim().Split(' ');
+                string voltage = valueUnit[0].Trim();
+
+                this.Voltage = float.Parse(voltage);
+            }
+        }
+
+        private T GetValueFromMesage<T>(string parameter, string message)
+        {
+            var data = JsonSerializer.Deserialize<Dictionary<string, T>>(message);
+            data.TryGetValue(parameter, out T value);
+            return value;
+        }
+    }
+}

ADIS_Csharp/RobotLib/DevBattery.cs

@@ -1,59 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Linq;
-using System.Text;
-using System.Threading.Tasks;
-
-namespace RobotLib
-{
-    //public class DevBattery : DevBase
-    //{
-    //    private float voltage;
-
-    //    public event EventHandler<BatteryEventArgs> BatteryChanged;
-
-    //    public float Voltage 
-    //    { 
-    //        get { return voltage; }
-    //        private set
-    //        {
-    //            // value changed?
-    //            if(voltage != value)
-    //            {
-    //                // set new voltage and raise event
-    //                voltage = value;
-    //                BatteryChanged?.Invoke(this, new BatteryEventArgs(voltage));
-    //            }
-    //        }
-    //    }
-
-    //    public DevBattery(Com com) : base(com, "battery") { }
-
-    //    public override void Refresh()
-    //    {
-    //        SendMessage("get battery status!");
-    //    }
-
-    //    protected override void ParseMessage(string message)
-    //    {
-    //        // example message = "Battery: 1.25 V"
-    //        var keyValue = message.Trim().Split(':');
-    //        string key = keyValue[0].Trim();
-    //        string value = keyValue[1].Trim();
-
-    //        switch (key)
-    //        {
-    //            case "battery":
-    //                value = value.Trim(' ', 'V');
-    //                this.Voltage = float.Parse(value);
-    //                break;
-
-    //            default:
-    //                log.Warn($"Unkown element {key}: {value} in message {message}");
-    //                break;
-    //        }
-            
-
-    //    }
-    //}
-}

ADIS_Csharp/RobotLib/Movement/DevLineSensor.cs

@@ -1,4 +1,5 @@
 using RobotLib.Communication;
+using RobotLib.SplitFlap;
 using System;
 using System.Collections.Generic;
 using System.Text.Json;
@@ -11,7 +12,9 @@ namespace RobotLib.Movement
         private const string TOPIC_ROBO_GET_CALIB = "/mobile/cmd/line_sens/get_calib";
         private const string TOPIC_ROBO_CALIB_STATE = "/mobile/state/line_sens/calib_data";
 
-        public DevLineSensor(IPublisherSubscriber com) : base(com, new List<string>() { }) { }
+        public event EventHandler<LineSensorCalibrationDataEventArgs> NewCalibrationDataArrived;
+
+        public DevLineSensor(IPublisherSubscriber com) : base(com, new List<string>() { TOPIC_ROBO_CALIB_STATE }) { }
 
         public void GetCalibrationData()
         {
@@ -23,5 +26,27 @@ namespace RobotLib.Movement
             string payload = JsonSerializer.Serialize(new Dictionary<string, bool>() { { "start", start } });
             base.SendMessage(TOPIC_ROBO_CALIB_CMD, payload);
         }
+
+        protected override void ParseMessage(string fromTopic, string message)
+        {
+            if (fromTopic == TOPIC_ROBO_CALIB_STATE)
+            {
+                var parsedState = GetValueFromMesage<string>("state", message);
+                var parsedValues = GetValueFromMesage<string>("data", message);
+
+                if (parsedState == null) parsedState = "?";
+                if (parsedValues == null) parsedValues = "?";
+
+                LineSensorCalibrationDataEventArgs eventArgs = new(parsedState, parsedValues);
+                NewCalibrationDataArrived?.Invoke(this, eventArgs);
+            }
+        }
+
+        private T GetValueFromMesage<T>(string parameter, string message)
+        {
+            var data = JsonSerializer.Deserialize<Dictionary<string, T>>(message);
+            data.TryGetValue(parameter, out T value);
+            return value;
+        }
     }
 }

ADIS_Csharp/RobotLib/Movement/LineSensorCalibrationDataEventArgs.cs

@@ -0,0 +1,14 @@
+namespace RobotLib.Movement
+{
+    public class LineSensorCalibrationDataEventArgs
+    {
+        public string State { get; }
+        public string CalibrationValues { get; }
+
+        public LineSensorCalibrationDataEventArgs(string state, string calibrationValues)
+        {
+            this.State = state;
+            this.CalibrationValues = calibrationValues;
+        }
+    }
+}

ADIS_Csharp/RobotLib/Robot.cs

@@ -1,6 +1,7 @@
 using RobotLib.Communication;
 using RobotLib.SplitFlap;
 using RobotLib.Movement;
+using RobotLib.Battery;
 using System.Threading;
 
 namespace RobotLib
@@ -11,7 +12,7 @@ namespace RobotLib
         {
             Com = com;
             //Buzzer = new DevBuzzer(Com);
-            //Battery = new DevBattery(Com);
+            Battery = new DevBattery(Com);
             SplitFlap = new DevSplitFlap(com);
             LineSensor = new DevLineSensor(com);
             Movement = new DevMovement(com);
@@ -23,7 +24,7 @@ namespace RobotLib
         public IPublisherSubscriber Com { get; }
 
         //public DevBuzzer Buzzer { get; }
-        //public DevBattery Battery { get; }
+        public DevBattery Battery { get; }
         public DevSplitFlap SplitFlap { get; }
         public DevMovement Movement { get; }
         public DevLineSensor LineSensor { get; }
@@ -42,7 +43,7 @@ namespace RobotLib
         {
             if (Com.IsConnected)
             {
-                //Battery.Refresh();
+                Battery.Refresh();
             }
         }
 

ADIS_ESP32_Eclipse/main/challenge_com.c

@@ -28,6 +28,11 @@ const char MQTT_TOPIC_ROBO_MODE[] = "/mobile/cmd/mode/";
 const char MQTT_TOPIC_ROBO_NAV_TURN[] = "/mobile/cmd/nav/turn/";
 const char MQTT_TOPIC_ROBO_NAV_MOVE[] = "/mobile/cmd/nav/move/";
 const char MQTT_TOPIC_ROBO_NAV_STOP[] = "/mobile/cmd/nav/stop/";
+const char MQTT_TOPIC_ROBO_CALIB_CMD[] = "/mobile/cmd/line_sens/calib";
+const char MQTT_TOPIC_ROBO_GET_CALIB[] = "/mobile/cmd/line_sens/get_calib";
+const char MQTT_TOPIC_ROBO_CALIB_STATE[] = "/mobile/state/line_sens/calib_data";
+const char MQTT_TOPIC_ROBO_REQ_BATTERY[] = "/mobile/cmd/battery/get_volt";
+const char MQTT_TOPIC_ROBO_RESP_BATTERY[] = "/mobile/state/battery/voltage";
 
 void Challenge_Com_ParseMqtt(void *handler_args, esp_event_base_t base, int32_t event_id, void *event_data){
 	/* INFO
@@ -158,6 +163,54 @@ void Challenge_Com_ParseMqtt(void *handler_args, esp_event_base_t base, int32_t
         	handled = true;
         	Robo_Wrapper_Nav_Stop();
     	}
+    	// CMD: ROBO_CALIB_CMD
+    	else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_CALIB_CMD)==0){
+        	handled = true;
+			// parse json
+			bool start = false;
+			struct json_attr_t json_attrs[] = {
+				{"start",    t_boolean, .addr.boolean = &start},
+				{NULL},
+			};
+			int err = json_read_object(data, json_attrs, NULL);
+			if(err != 0){
+				ESP_LOGE(TAG, "Parsing JSON data of ROBO_CALIB_CMD message failed. Event data was = %s", data);
+				ESP_LOGE(TAG, "Parse error was: %s", json_error_string(err));
+			} else{  // successfully parsed
+				Robo_Wrapper_Calibration(start);
+			}
+    	}
+    	// CMD: ROBO_GET_CALIB
+    	else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_GET_CALIB)==0){
+        	handled = true;
+        	LineSensorCalibData_t calibData;
+        	Robo_Wrapper_GetCalibrationValues(&calibData);
+        	ESP_LOGI(TAG, "Read calibration data: state=%s", calibData.state);
+        	ESP_LOGI(TAG, "Read calibration data: values=%s", calibData.values);
+
+        	// build json and respond
+        	unsigned char payload[100] = "{\"state\": \"";
+        	McuUtility_strcat(payload, sizeof(payload), calibData.state);
+        	McuUtility_strcat(payload, sizeof(payload), (unsigned char*)"\", \"data\": \"");
+        	McuUtility_strcat(payload, sizeof(payload), calibData.values);
+        	McuUtility_strcat(payload, sizeof(payload), (unsigned char*)"\"}");
+
+        	MyMqtt_Publish(MQTT_TOPIC_ROBO_CALIB_STATE, (char*)payload);
+    	}
+    	// CMD: ROBO_REQ_BATTERY
+    	else if(McuUtility_strcmp(topic, MQTT_TOPIC_ROBO_REQ_BATTERY)==0){
+        	handled = true;
+        	unsigned char voltage[20] = "";
+        	Robo_Wrapper_GetBatteryVoltage(voltage);
+        	ESP_LOGI(TAG, "Read battery voltage=%s", voltage);
+
+        	// build json and respond
+        	unsigned char payload[100] = "{\"voltage\": \"";
+        	McuUtility_strcat(payload, sizeof(payload), voltage);
+        	McuUtility_strcat(payload, sizeof(payload), (unsigned char*)"\"}");
+
+        	MyMqtt_Publish(MQTT_TOPIC_ROBO_RESP_BATTERY, (char*)payload);
+    	}
     }
 
     /* both robot modes allowed commands */
@@ -178,4 +231,7 @@ void Challenge_Com_SubscribeToAllTopics(void){
 	MyMqtt_Subscribe(MQTT_TOPIC_ROBO_NAV_MOVE);
 	MyMqtt_Subscribe(MQTT_TOPIC_ROBO_NAV_TURN);
 	MyMqtt_Subscribe(MQTT_TOPIC_ROBO_NAV_STOP);
+	MyMqtt_Subscribe(MQTT_TOPIC_ROBO_CALIB_CMD);
+	MyMqtt_Subscribe(MQTT_TOPIC_ROBO_GET_CALIB);
+	MyMqtt_Subscribe(MQTT_TOPIC_ROBO_REQ_BATTERY);
 }

ADIS_ESP32_Eclipse/main/robo_wrapper.c

@@ -8,11 +8,17 @@
 #include "McuUtility.h"
 #include "McuShell.h"
 #include "Shell.h"
+#include "esp_log.h"
 
 #define RS_CMD_DRIVE_PREFIX "drive "
 #define RS_CMD_TURN_PREFIX "turn "
 #define BUF_SIZE 50
 
+#define TAG "ROBO_WRAPPER" /* tag for logging with ESP_LOG */
+
+static bool getValueOfStatusResponse (unsigned char* response, const unsigned char* key, unsigned char* valuevalue, size_t valueStringLen);
+
+
 /* Sets the robot to automatic / manual mode */
 bool Robo_Wrapper_SetMode(bool automatic){
 	if(automatic){
@@ -66,3 +72,94 @@ bool Robo_Wrapper_Nav_Stop(void){
 	McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
 	return true;
 }
+
+bool Robo_Wrapper_Calibration(bool start){
+	if(start){
+		/* start calibration */
+		unsigned char response[128];
+		SHELL_SendToRobotAndGetResponse((const unsigned char*)"ref calib start", response, sizeof(response));
+		McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
+	}else{
+		/* end calibration */
+		unsigned char response[128];
+		SHELL_SendToRobotAndGetResponse((const unsigned char*)"ref calib stop", response, sizeof(response));
+		McuShell_SendStr(response, McuShell_GetStdio()->stdOut);
+	}
+	return true;
+}
+
+bool Robo_Wrapper_GetCalibrationValues(LineSensorCalibData_t *data){
+	unsigned char response[500];
+	// set default values
+	McuUtility_strcpy(data->state, sizeof(data->state), (unsigned char*)"n/a");
+	McuUtility_strcpy(data->values, sizeof(data->values), (unsigned char*)"0 0 0 0 0 0");
+
+	SHELL_SendToRobotAndGetResponse((const unsigned char*)"ref status", response, sizeof(response));
+	/* get calibration values */
+	getValueOfStatusResponse(response, (unsigned char*)"state", (unsigned char*)&data->state, 20);
+	getValueOfStatusResponse(response, (unsigned char*)"calib val", (unsigned char*)&data->values, 50);
+
+	return true;
+}
+
+bool Robo_Wrapper_GetBatteryVoltage(unsigned char *voltage){
+	unsigned char response[200];
+	// set default value
+	McuUtility_strcpy(voltage, sizeof(voltage), (unsigned char*)"n/a");
+	SHELL_SendToRobotAndGetResponse((const unsigned char*)"battery status", response, sizeof(response));
+	/* get value */
+	getValueOfStatusResponse(response, (unsigned char*)"Battery", (unsigned char*)voltage, 20);
+
+	return true;
+}
+
+static bool getValueOfStatusResponse(unsigned char* response, const unsigned char* key, unsigned char* value, size_t valueStringLen){
+	int16_t pos = McuUtility_strFind(response, (unsigned char*)key);
+	unsigned char extractedString[50] = "";
+
+	if(pos == -1){   // error string not found = -1
+		ESP_LOGE(TAG, "Could not find key %s in response.", key);
+		return false;
+	}
+
+	unsigned char *p;
+	p = (unsigned char*)response + pos;
+
+	// skip first line (if the keyword would also appear in the headline, this is an issue)
+	while(*p!='\n'){
+		p++;
+	}
+	p+=1; // skip newline
+
+	// skip until colon
+	while(true){
+		if(*p==':'){
+			p+=2;  // colon found, skip colon + 2 space and proceed
+			break;
+		}
+		else if(*p=='\n'){
+			ESP_LOGE(TAG, "Reached end of line while skipping colon after key %s.", key);
+			return false;  // error, end of line
+		}
+		else{
+			p++;  // check next character
+		}
+	}
+
+	// extract value
+	uint8_t i = 0;
+	while(true){
+		if(*p == '\n'){
+			break;   // end of value reached
+		}
+		else{
+			extractedString[i++] = *p;
+			p++;  // next char
+		}
+	}
+
+	// copy to destination
+	McuUtility_strcpy(value, sizeof(unsigned char) * valueStringLen, extractedString);
+
+	return true;
+}

ADIS_ESP32_Eclipse/main/robo_wrapper.h

@@ -11,6 +11,11 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+typedef struct {
+  unsigned char state[20];
+  unsigned char values[50];
+} LineSensorCalibData_t;
+
 /* Sets the robot to automatic / manual mode */
 bool Robo_Wrapper_SetMode(bool automatic);
 
@@ -23,4 +28,13 @@ bool Robo_Wrapper_Nav_Move(int16_t speed);
 /* Stops the movement of the robot */
 bool Robo_Wrapper_Nav_Stop(void);
 
+/* Starts / stops robo line sensor calibration. true = start, false = stop */
+bool Robo_Wrapper_Calibration(bool start);
+
+/* gets calibration value of line sensor */
+bool Robo_Wrapper_GetCalibrationValues(LineSensorCalibData_t *data);
+
+/* gets battery voltage of robo */
+bool Robo_Wrapper_GetBatteryVoltage(unsigned char *voltage);
+
 #endif /* MAIN_ROBO_WRAPPER_H_ */

ADIS_ESP32_Eclipse/main/wifi.c

@@ -304,6 +304,10 @@ static void WiFiTask(void *pv) {
 bool WiFi_isConnected(void) {
   bool isConnected;
 
+  // wifi not initialized yet => return false
+  if(s_wifi_event_group == 0) return false;
+
+  // check event bits
   isConnected = xEventGroupGetBits(s_wifi_event_group)&WIFI_CONNECTED_BIT;
   return isConnected;
 }