diff --git a/ADIS_Csharp/RobotClientWpf/MainWindow.xaml.cs b/ADIS_Csharp/RobotClientWpf/MainWindow.xaml.cs
index 0830e25..340fa81 100644
--- a/ADIS_Csharp/RobotClientWpf/MainWindow.xaml.cs
+++ b/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
{
diff --git a/ADIS_Csharp/RobotClientWpf/Views/ConfigView.xaml b/ADIS_Csharp/RobotClientWpf/Views/ConfigView.xaml
index 7bb255b..c218ce2 100644
--- a/ADIS_Csharp/RobotClientWpf/Views/ConfigView.xaml
+++ b/ADIS_Csharp/RobotClientWpf/Views/ConfigView.xaml
@@ -12,7 +12,7 @@
-
+
@@ -36,8 +36,18 @@
-
-
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/ADIS_Csharp/RobotClientWpf/Views/ConfigView.xaml.cs b/ADIS_Csharp/RobotClientWpf/Views/ConfigView.xaml.cs
index 08b7a69..7a6ed08 100644
--- a/ADIS_Csharp/RobotClientWpf/Views/ConfigView.xaml.cs
+++ b/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);
+ }
}
}
diff --git a/ADIS_Csharp/RobotClientWpf/Views/MainView.xaml b/ADIS_Csharp/RobotClientWpf/Views/MainView.xaml
index d3c0402..c0eddde 100644
--- a/ADIS_Csharp/RobotClientWpf/Views/MainView.xaml
+++ b/ADIS_Csharp/RobotClientWpf/Views/MainView.xaml
@@ -15,9 +15,21 @@
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/ADIS_Csharp/RobotClientWpf/Views/MainView.xaml.cs b/ADIS_Csharp/RobotClientWpf/Views/MainView.xaml.cs
index 8c9a3f5..2363df0 100644
--- a/ADIS_Csharp/RobotClientWpf/Views/MainView.xaml.cs
+++ b/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)
diff --git a/ADIS_Csharp/RobotLib/BatteryEventArgs.cs b/ADIS_Csharp/RobotLib/Battery/BatteryEventArgs.cs
similarity index 88%
rename from ADIS_Csharp/RobotLib/BatteryEventArgs.cs
rename to ADIS_Csharp/RobotLib/Battery/BatteryEventArgs.cs
index fb31787..98da738 100644
--- a/ADIS_Csharp/RobotLib/BatteryEventArgs.cs
+++ b/ADIS_Csharp/RobotLib/Battery/BatteryEventArgs.cs
@@ -1,6 +1,6 @@
using System;
-namespace RobotLib
+namespace RobotLib.Battery
{
public class BatteryEventArgs : EventArgs
{
diff --git a/ADIS_Csharp/RobotLib/Battery/DevBattery.cs b/ADIS_Csharp/RobotLib/Battery/DevBattery.cs
new file mode 100644
index 0000000..06caaa4
--- /dev/null
+++ b/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 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() { 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("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(string parameter, string message)
+ {
+ var data = JsonSerializer.Deserialize>(message);
+ data.TryGetValue(parameter, out T value);
+ return value;
+ }
+ }
+}
diff --git a/ADIS_Csharp/RobotLib/DevBattery.cs b/ADIS_Csharp/RobotLib/DevBattery.cs
deleted file mode 100644
index b141661..0000000
--- a/ADIS_Csharp/RobotLib/DevBattery.cs
+++ /dev/null
@@ -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 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;
- // }
-
-
- // }
- //}
-}
diff --git a/ADIS_Csharp/RobotLib/Movement/DevLineSensor.cs b/ADIS_Csharp/RobotLib/Movement/DevLineSensor.cs
index 695478c..7840dca 100644
--- a/ADIS_Csharp/RobotLib/Movement/DevLineSensor.cs
+++ b/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() { }) { }
+ public event EventHandler NewCalibrationDataArrived;
+
+ public DevLineSensor(IPublisherSubscriber com) : base(com, new List() { TOPIC_ROBO_CALIB_STATE }) { }
public void GetCalibrationData()
{
@@ -23,5 +26,27 @@ namespace RobotLib.Movement
string payload = JsonSerializer.Serialize(new Dictionary() { { "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("state", message);
+ var parsedValues = GetValueFromMesage("data", message);
+
+ if (parsedState == null) parsedState = "?";
+ if (parsedValues == null) parsedValues = "?";
+
+ LineSensorCalibrationDataEventArgs eventArgs = new(parsedState, parsedValues);
+ NewCalibrationDataArrived?.Invoke(this, eventArgs);
+ }
+ }
+
+ private T GetValueFromMesage(string parameter, string message)
+ {
+ var data = JsonSerializer.Deserialize>(message);
+ data.TryGetValue(parameter, out T value);
+ return value;
+ }
}
}
diff --git a/ADIS_Csharp/RobotLib/Movement/LineSensorCalibrationDataEventArgs.cs b/ADIS_Csharp/RobotLib/Movement/LineSensorCalibrationDataEventArgs.cs
new file mode 100644
index 0000000..eb01244
--- /dev/null
+++ b/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;
+ }
+ }
+}
diff --git a/ADIS_Csharp/RobotLib/Robot.cs b/ADIS_Csharp/RobotLib/Robot.cs
index 7a8ccec..1ebe3b3 100644
--- a/ADIS_Csharp/RobotLib/Robot.cs
+++ b/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();
}
}
diff --git a/ADIS_ESP32_Eclipse/main/challenge_com.c b/ADIS_ESP32_Eclipse/main/challenge_com.c
index 3a50d4e..d8ea50b 100644
--- a/ADIS_ESP32_Eclipse/main/challenge_com.c
+++ b/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);
}
diff --git a/ADIS_ESP32_Eclipse/main/robo_wrapper.c b/ADIS_ESP32_Eclipse/main/robo_wrapper.c
index 51336cf..2062442 100644
--- a/ADIS_ESP32_Eclipse/main/robo_wrapper.c
+++ b/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;
+}
diff --git a/ADIS_ESP32_Eclipse/main/robo_wrapper.h b/ADIS_ESP32_Eclipse/main/robo_wrapper.h
index a61c8c5..1ef63ce 100644
--- a/ADIS_ESP32_Eclipse/main/robo_wrapper.h
+++ b/ADIS_ESP32_Eclipse/main/robo_wrapper.h
@@ -11,6 +11,11 @@
#include
#include
+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_ */
diff --git a/ADIS_ESP32_Eclipse/main/wifi.c b/ADIS_ESP32_Eclipse/main/wifi.c
index bbe24de..89d083b 100644
--- a/ADIS_ESP32_Eclipse/main/wifi.c
+++ b/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;
}