jonas
/
ADIS_Projects
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

implemented line sensor calibration in ESP and UI,

Browse Source 
implemented battery voltage display
main
Jonas Arnold 4 years ago
parent 1e9ee6aea3
commit 8cc965ef51
15 changed files with 325 additions and 71 deletions
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Show Stats Download Patch File Download Diff File
  1. 2
      ADIS_Csharp/RobotClientWpf/MainWindow.xaml.cs
  2. 16
      ADIS_Csharp/RobotClientWpf/Views/ConfigView.xaml
  3. 8
      ADIS_Csharp/RobotClientWpf/Views/ConfigView.xaml.cs
  4. 18
      ADIS_Csharp/RobotClientWpf/Views/MainView.xaml
  5. 6
      ADIS_Csharp/RobotClientWpf/Views/MainView.xaml.cs
  6. 2
      ADIS_Csharp/RobotLib/Battery/BatteryEventArgs.cs
  7. 66
      ADIS_Csharp/RobotLib/Battery/DevBattery.cs
  8. 59
      ADIS_Csharp/RobotLib/DevBattery.cs
  9. 27
      ADIS_Csharp/RobotLib/Movement/DevLineSensor.cs
  10. 14
      ADIS_Csharp/RobotLib/Movement/LineSensorCalibrationDataEventArgs.cs
  11. 7
      ADIS_Csharp/RobotLib/Robot.cs
  12. 56
      ADIS_ESP32_Eclipse/main/challenge_com.c
  13. 97
      ADIS_ESP32_Eclipse/main/robo_wrapper.c
  14. 14
      ADIS_ESP32_Eclipse/main/robo_wrapper.h
  15. 4
      ADIS_ESP32_Eclipse/main/wifi.c

2
ADIS_Csharp/RobotClientWpf/MainWindow.xaml.cs
Unescape View File

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

16
ADIS_Csharp/RobotClientWpf/Views/ConfigView.xaml
Unescape View File

@ -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>

8
ADIS_Csharp/RobotClientWpf/Views/ConfigView.xaml.cs
Unescape View File

@ -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);
}
}
}

18
ADIS_Csharp/RobotClientWpf/Views/MainView.xaml
Unescape View File

@ -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"/>
</GroupBox>
<!-- RIGHT SIDE -->
<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">

6
ADIS_Csharp/RobotClientWpf/Views/MainView.xaml.cs
Unescape View File

@ -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)

2
ADIS_Csharp/RobotLib/BatteryEventArgs.cs → ADIS_Csharp/RobotLib/Battery/BatteryEventArgs.cs
Unescape View File

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

66
ADIS_Csharp/RobotLib/Battery/DevBattery.cs
Unescape View File

@ -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;
}
}
}

59
ADIS_Csharp/RobotLib/DevBattery.cs
Unescape View File

@ -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;
// }
// }
//}
}

27
ADIS_Csharp/RobotLib/Movement/DevLineSensor.cs
Unescape View File

@ -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;
}
}
}

14
ADIS_Csharp/RobotLib/Movement/LineSensorCalibrationDataEventArgs.cs
Unescape View File

@ -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;
}
}
}

7
ADIS_Csharp/RobotLib/Robot.cs
Unescape View File

@ -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();
}
}

56
ADIS_ESP32_Eclipse/main/challenge_com.c
Unescape View File

@ -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);
}

97
ADIS_ESP32_Eclipse/main/robo_wrapper.c
Unescape View File

@ -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;
}

14
ADIS_ESP32_Eclipse/main/robo_wrapper.h
Unescape View File

@ -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_ */

4
ADIS_ESP32_Eclipse/main/wifi.c
Unescape View File

@ -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;
}

Write Preview
Loading…
Cancel
Save