private async void NotifyOnObstacleEventHandler(object sender, NotificationEventArgs e)
{
// new obstacle signal
var currentState = e.IsObstacleDetected;
if (currentState && !this.isObstacleDetected)
{
await this.ExecuteCommandAsync(Commands.DriveStop);
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = "🛑 Stop sign detected!"
};
this.NotifyUIEvent(notifyEventArgs);
dynamic telemetry = new System.Dynamic.ExpandoObject();
telemetry.IsCameraActive = this.isCameraActive;
telemetry.DeviceIp = this.isCameraActive ? Helpers.GetIPAddress()?.ToString() : null;
telemetry.IsObstacleDetected = true;
// send an urgent tele packet
this.OnNotifyDataEventHandler("AZURE", JObject.FromObject(telemetry), true);
this.Speak("Stop sign detected!");
}
this.isObstacleDetected = currentState;
}
private async Task SignalRejectedCommand(DeserializableCommand command)
{
if (command == null)
{
throw new ArgumentNullException("command");
}
Debug.Assert(
!string.IsNullOrEmpty(command.LockToken),
"command.LockToken is a null reference or empty string.");
try
{
await this.deviceClient.RejectAsync(command.LockToken);
}
catch (Exception ex)
{
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = string.Format("{0}{0}*** Exception: Reject Command ***{0}{0}Command Name: {1}{0}Command: {2}{0}Exception: {3}{0}{0}",
Console.Out.NewLine,
command.CommandName,
command.Command,
ex)
};
this.NotifyUIEvent(notifyEventArgs);
}
}
private async Task SendEventAsync(JObject eventData)
{
var eventId = Guid.NewGuid();
string objectType = this.GetObjectType(eventData);
var objectTypePrefix = Constants.ObjectTypePrefix;
if (!string.IsNullOrWhiteSpace(objectType) &&
!string.IsNullOrEmpty(objectTypePrefix))
{
eventData["ObjectType"] = objectTypePrefix + objectType;
}
var bytes = this.serializer.SerializeObject(eventData);
var message = new Microsoft.Azure.Devices.Client.Message(bytes);
message.Properties["EventId"] = eventId.ToString();
try
{
await this.deviceClient.SendEventAsync(message);
}
catch (Exception ex)
{
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = string.Format("{0}{0}*** Exception: SendEventAsync ***{0}{0}EventId: {1}{0}Event Data: {2}{0}Exception: {3}{0}{0}",
Console.Out.NewLine,
ex)
};
this.NotifyUIEvent(notifyEventArgs);
}
}
private async Task PurgeCommandQueueAsync(CancellationToken token)
{
try
{
Message msg = null;
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = "Purging device command queue..."
};
this.NotifyUIEvent(notifyEventArgs);
while (!token.IsCancellationRequested)
{
try
{
// Retrieve the message from the IoT Hub
msg = await this.deviceClient.ReceiveAsync();
if (msg == null)
{
notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = "Finished purging device command queue..."
};
this.NotifyUIEvent(notifyEventArgs);
return;
}
}
catch (Exception ex)
{
Debug.WriteLine(ex);
}
if (msg != null)
{
await this.deviceClient.RejectAsync(msg.LockToken);
// Pause before running through the receive loop
await Task.Delay(TimeSpan.FromMilliseconds(50));
}
}
}
catch (Exception ex)
{
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = string.Format("Unexpected exception while purging device commands queue: {0}", ex.ToString())
};
this.NotifyUIEvent(notifyEventArgs);
}
}
private async void GetDesiredTurnDirection()
{
if (this.nextWayPoint == null)
{
return;
}
this.IncrementCoordinates();
GeoCoordinate currentPosition = this.currentPosition;
var targetHeading = WayPointHelper.DegreeBearing(currentPosition, this.nextWayPoint);
// calculate where we need to turn to head to destination
var headingError = targetHeading -
this.compass.GetHeadingInDegrees(this.compass.ReadRaw());
// adjust for compass wrap
if (headingError < -180)
{
headingError += 360;
}
if (headingError > 180)
{
headingError -= 360;
}
// calculate which way to turn to intercept the targetHeading
if (Math.Abs(headingError) <= this.HEADING_TOLERANCE_DEGREES)
{
// if within tolerance, don't turn
this.turnDirection = Commands.DrivingDirection.Forward;
}
else if (headingError < 0)
{
this.turnDirection = Commands.DrivingDirection.Left;
}
else if (headingError > 0)
{
this.turnDirection = Commands.DrivingDirection.Right;
}
else
{
this.turnDirection = Commands.DrivingDirection.Forward;
}
this.dummyIndex++;
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = "Turn Direction - " + this.turnDirection
};
this.NotifyUIEvent(notifyEventArgs);
}
private void NotifyUIEventHandler(object sender, NotifyUIEventArgs e)
{
switch (e.NotificationType)
{
default:
case NotificationType.Console:
this.WriteToOutputTextBlock(e.Data);
return;
case NotificationType.ControlMode:
this.UpdateControlModeonUi(e.Name, e.Data.ToString());
return;
case NotificationType.ButtonState:
var state = (Commands.ToggleCommandState)Enum.Parse(typeof(Commands.ToggleCommandState), e.Data, true);
this.UpdateUiButtonStates(e.Name, state);
return;
}
}
public async Task StartWayPointNavigationAsync(List <GeoCoordinate> wayPoints)
{
if (wayPoints == null || wayPoints.Count < 1)
{
throw new ArgumentNullException(nameof(wayPoints));
}
this.navigationCancellationTokenSource = new CancellationTokenSource();
NotifyUIEventArgs notifyEventArgs = null;
notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.ControlMode,
Name = "navigation",
Data = "Navigation"
};
this.NotifyUIEvent(notifyEventArgs);
this.nextWayPointIndex = -1;
this.dummycounter = 0;
this.dummyIndex = 0;
this.currentPosition = null;
this.mockwayPoints = this.GetMockWayPoints();
// first waypoint
this.nextWayPoint = this.GetNextWayPoint();
if (this.nextWayPoint == null)
{
notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = "Last waypoint reached!"
};
this.NotifyUIEvent(notifyEventArgs);
}
this.StartRoverAsync();
}
private async Task <DeserializableCommand> ReceiveAsync()
{
Microsoft.Azure.Devices.Client.Message message = null;
Exception exp = null;
try
{
message = await this.deviceClient.ReceiveAsync();
}
catch (Exception exception)
{
exp = exception;
}
if (exp != null)
{
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = string.Format("{0}{0}*** Exception: ReceiveAsync ***{0}{0}{1}{0}{0}",
Console.Out.NewLine,
exp)
};
this.NotifyUIEvent(notifyEventArgs);
if (message != null)
{
await this.deviceClient.AbandonAsync(message);
}
}
if (message != null)
{
return(new DeserializableCommand(message, this.serializer));
}
return(null);
}
public void _form_UpdateUI(object sender, EventArgs e)
{
lock (syncObject)
{
NotifyUIEventArgs args = e as NotifyUIEventArgs;
if (args != null)
{
if (args.messageCollection == null)
{
Control[] changedControl = this.Controls.Find(args.name, true);
if (changedControl.Count() > 0)
{
TextBox boxContainer = (TextBox)changedControl[0];
boxContainer.BeginInvoke((MethodInvoker) delegate { boxContainer.Text = args.message; });
}
}
else
{
foreach (KeyValuePair <string, string> messageItem in args.messageCollection)
{
Control[] changedControl = this.Controls.Find(messageItem.Key, true);
string test = messageItem.Key;
if (changedControl.Count() > 0)
{
TextBox boxContainer = (TextBox)changedControl[0];
boxContainer.BeginInvoke((MethodInvoker) delegate { boxContainer.Text = messageItem.Value; });
}
}
}
}
else
{
// exception
return;
}
}
}
private void IncrementCoordinates()
{
//GeoCoordinate currentPosition = new GeoCoordinate(
// this.CurrentGpsPosition.Latitude.Value,
// this.CurrentGpsPosition.Longitude.Value);
if (this.dummyIndex < 19)
{
this.dummyIndex++;
this.currentPosition = this.GetDummyPoints();
}
else
{
this.currentPosition = this.mockwayPoints.LastOrDefault();
}
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = "Current GPS As - " + this.currentPosition.ToString()
};
this.NotifyUIEvent(notifyEventArgs);
}
private async void StartRoverAsync()
{
await this.AcquireActiveGpsConnectionAsync();
// initial direction
await this.commandProcessor.ExecuteCommandAsync(Commands.SpeedNormal);
var tasks = new List <Task>()
{
this.MoveRoverAsync(),
this.UpdateDesiredTurnDirectionAsync(),
this.LoadNextWayPointAsync()
};
// execute asynchronously till its cancelled
await Task.WhenAll(tasks);
await this.commandProcessor.ExecuteCommandAsync(Commands.SpeedStop);
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = "All waypoint completed!"
};
this.NotifyUIEvent(notifyEventArgs);
notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.ControlMode,
Name = "Parked",
Data = "Parked"
};
this.NotifyUIEvent(notifyEventArgs);
}
private async void UltrasonictimerLoopAsync()
{
while (!this.ultrasonicTimerToken.IsCancellationRequested)
{
try
{
var distance = this.ultrasonicsensor.GetDistance(1000);
if (distance.Centimeters < Constants.SafeDistanceCm)
{
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = "OBSTACLE in " + distance.Centimeters + " cm"
};
this.NotifyUIEvent(notifyEventArgs);
this.wasObstacleDetected = true;
notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = "Reversing..."
};
this.NotifyUIEvent(notifyEventArgs);
await this.ExecuteCommandAsync(Commands.DriveReverse);
await Task.Delay(TimeSpan.FromMilliseconds(500));
await this.ExecuteCommandAsync(Commands.DriveReverseLeft);
await Task.Delay(TimeSpan.FromMilliseconds(500));
}
else if (this.wasObstacleDetected)
{
this.wasObstacleDetected = false;
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = "Rover at safe distance..."
};
this.NotifyUIEvent(notifyEventArgs);
await this.ExecuteCommandAsync(Commands.DriveForward);
}
else
{
await Task.Delay(TimeSpan.FromMilliseconds(50));
}
}
catch (Exception ex)
{
Debug.Write(ex);
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = $"Autonomous mode error... {ex.Message}"
};
this.NotifyUIEvent(notifyEventArgs);
}
}
}
public async Task ExecuteCommandAsync(string commandText, object commandData = null)
{
switch (commandText)
{
case Commands.DriveForward:
{
if (this.isObstacleDetected)
{
return;
}
this.arduino.SendCommand(Commands.SpeedNormalValue);
this.arduino.SendCommand(Commands.DriveForwardValue);
this.displayManager.AppendImage(DisplayImages.TopArrow, 0, 3);
this.displayManager.AppendText("Drive", 19, 3);
//this.UpdateDisplayWithSpeed();
break;
}
case Commands.DriveReverse:
{
if (this.isObstacleDetected)
{
return;
}
this.arduino.SendCommand(Commands.SpeedNormalValue);
this.playbackService.ChangeMediaSource(PlaybackService.SoundFiles.CensorBeep);
this.playbackService.PlaySound();
this.arduino.SendCommand(Commands.DriveReverseValue);
this.displayManager.AppendImage(DisplayImages.BottomArrow, 0, 3);
this.displayManager.AppendText("Drive", 19, 3);
//this.UpdateDisplayWithSpeed();
break;
}
case Commands.DriveLeft:
{
if (this.isObstacleDetected)
{
return;
}
this.arduino.SendCommand(Commands.SpeedSlowValue);
this.arduino.SendCommand(Commands.DriveLeftValue);
this.displayManager.AppendImage(DisplayImages.LeftArrow, 0, 3);
this.displayManager.AppendText("Drive", 19, 3);
break;
}
case Commands.DriveRight:
{
if (this.isObstacleDetected)
{
return;
}
this.arduino.SendCommand(Commands.SpeedSlowValue);
this.arduino.SendCommand(Commands.DriveRightValue);
this.displayManager.AppendImage(DisplayImages.RightArrow, 0, 3);
this.displayManager.AppendText("Drive", 19, 3);
break;
}
case Commands.DriveStop:
{
this.arduino.SendCommand(Commands.DriveStopValue);
this.displayManager.ClearRow(2);
this.displayManager.AppendImage(DisplayImages.Stop, 0, 3);
this.displayManager.AppendText("Stop", 19, 3);
break;
}
case Commands.DriveReverseLeft:
{
if (this.isObstacleDetected)
{
return;
}
this.arduino.SendCommand(Commands.SpeedNormalValue);
this.arduino.SendCommand(Commands.DriveReverseLeftValue);
this.displayManager.AppendImage(DisplayImages.LeftArrow, 0, 3);
this.displayManager.AppendText("Drive", 19, 3);
//this.UpdateDisplayWithSpeed();
break;
}
case Commands.DriveReverseRight:
{
if (this.isObstacleDetected)
{
return;
}
this.arduino.SendCommand(Commands.SpeedNormalValue);
this.arduino.SendCommand(Commands.DriveReverseRightValue);
this.displayManager.AppendImage(DisplayImages.RightArrow, 0, 3);
this.displayManager.AppendText("Drive", 19, 3);
// this.UpdateDisplayWithSpeed();
break;
}
case Commands.DriveAutoModeOn:
{
this.InitializeDistanceSensor();
this.arduino.SendCommand(Commands.SpeedNormalValue);
this.arduino.SendCommand(Commands.DriveForwardValue);
this.displayManager.AppendImage(DisplayImages.Logo_16_16, 0, 1);
this.Speak("Autonomous mode on!");
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.ControlMode,
Name = "Autonomous",
Data = "Autonomous"
};
this.NotifyUIEvent(notifyEventArgs);
break;
}
case Commands.DriveAutoModeOff:
{
await TaskHelper.DispatchAsync(CoreDispatcherPriority.Normal, () =>
{
this.ultrasonicTimerToken?.Cancel();
});
this.displayManager.ClearRow(1);
this.arduino.SendCommand(Commands.DriveStopValue);
this.arduino.SendCommand(Commands.SpeedStopValue);
this.Speak("Autonomous mode off!");
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.ControlMode,
Name = "Parked",
Data = "Parked"
};
this.NotifyUIEvent(notifyEventArgs);
break;
}
case Commands.SpeedStop:
{
this.arduino.SendCommand(Commands.SpeedStopValue);
break;
}
case Commands.SpeedNormal:
{
this.arduino.SendCommand(Commands.SpeedNormalValue);
break;
}
case Commands.SpeedSlow:
{
this.arduino.SendCommand(Commands.SpeedSlowValue);
break;
}
case Commands.SpeedVerySlow:
{
this.arduino.SendCommand(Commands.SpeedVerySlowValue);
break;
}
case Commands.CameraOn:
{
var server = this.httpServer;
if (server != null)
{
await server?.Start();
}
this.OnNotifyDataEventHandler("RFCOMM", "CAMON");
this.displayManager.AppendImage(DisplayImages.Camera, 0, 2);
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.ButtonState,
Name = "camera",
Data = Commands.ToggleCommandState.On.ToString()
};
this.isCameraActive = true;
this.NotifyUIEvent(notifyEventArgs);
break;
}
case Commands.CameraOff:
{
var server = this.httpServer;
if (server != null)
{
await server?.Stop();
}
this.OnNotifyDataEventHandler("RFCOMM", "CAMOFF");
this.displayManager.ClearRow(2);
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.ButtonState,
Name = "camera",
Data = Commands.ToggleCommandState.Off.ToString()
};
this.isCameraActive = false;
this.NotifyUIEvent(notifyEventArgs);
break;
}
case Commands.TiltUp:
{
await this.panTiltServo.ExecuteCommand(Commands.TiltUpValue);
break;
}
case Commands.TiltDown:
{
await this.panTiltServo.ExecuteCommand(Commands.TiltDownValue);
break;
}
case Commands.PanLeft:
{
await this.panTiltServo.ExecuteCommand(Commands.PanLeftValue);
break;
}
case Commands.PanRight:
{
await this.panTiltServo.ExecuteCommand(Commands.PanRightValue);
break;
}
case Commands.PanTiltCenter:
{
await this.panTiltServo.ExecuteCommand(Commands.PanTiltCenterValue);
break;
}
case Commands.Horn:
{
this.playbackService.PlaySoundFromFile(PlaybackService.SoundFiles.Horn);
break;
}
case Commands.CameraLedOn:
{
this.cameraLedPin.Write(GpioPinValue.High);
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.ButtonState,
Name = "lights",
Data = Commands.ToggleCommandState.On.ToString()
};
this.NotifyUIEvent(notifyEventArgs);
break;
}
case Commands.CameraLedOff:
{
this.cameraLedPin.Write(GpioPinValue.Low);
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.ButtonState,
Name = "lights",
Data = Commands.ToggleCommandState.Off.ToString()
};
this.NotifyUIEvent(notifyEventArgs);
break;
}
default:
{
this.Speak(commandText);
break;
}
}
}
private async Task <CommandProcessingResult> HandleCommandAsync(DeserializableCommand deserializableCommand)
{
var notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = $"Received cloud command: {deserializableCommand.CommandName}"
};
this.NotifyUIEvent(notifyEventArgs);
if (deserializableCommand.CommandName == "DemoRun")
{
return(await this.ExecuteDemoCommandAsync(deserializableCommand));
}
else if (deserializableCommand.CommandName == "SendWaypoints")
{
return(await this.ExecuteSendWaypointsCommandAsync(deserializableCommand));
}
else if (deserializableCommand.CommandName == "SetLights")
{
return(await this.ExecuteSetLightsCommandAsync(deserializableCommand));
}
else if (deserializableCommand.CommandName == "SetCamera")
{
return(await this.ExecuteSetCameraCommandAsync(deserializableCommand));
}
else if (deserializableCommand.CommandName == "SendBuzzer")
{
return(await this.ExecuteSendBuzzerCommandAsync());
}
else if (deserializableCommand.CommandName == "EmergencyStop")
{
return(await this.ExecuteEmergencyStopCommandAsync());
}
else if (deserializableCommand.CommandName == "StartTelemetry")
{
return(this.ExecuteStartStopTelemetryCommandAsync(true));
}
else if (deserializableCommand.CommandName == "StopTelemetry")
{
return(this.ExecuteStartStopTelemetryCommandAsync(false));
}
else if (deserializableCommand.CommandName == "MoveVehicle")
{
return(await this.ExecuteMoveVehicleCommandAsync(deserializableCommand));
}
else if (deserializableCommand.CommandName == "MoveCamera")
{
return(await this.ExecuteMoveCameraCommandAsync(deserializableCommand));
}
else
{
notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = $"Command not registered in the system: {deserializableCommand.CommandName}"
};
this.NotifyUIEvent(notifyEventArgs);
}
return(CommandProcessingResult.CannotComplete);
}
private async void StartReceiveLoopAsync(CancellationToken token)
{
DeserializableCommand command;
Exception exception;
CommandProcessingResult processingResult;
try
{
while (!token.IsCancellationRequested)
{
command = null;
exception = null;
// Pause before running through the receive loop
await Task.Delay(TimeSpan.FromSeconds(1), token);
try
{
// Retrieve the message from the IoT Hub
command = await this.ReceiveAsync();
if (command == null)
{
continue;
}
processingResult =
await this.HandleCommandAsync(command);
switch (processingResult)
{
case CommandProcessingResult.CannotComplete:
await this.SignalRejectedCommand(command);
break;
case CommandProcessingResult.RetryLater:
await this.SignalAbandonedCommand(command);
break;
case CommandProcessingResult.Success:
await this.SignalCompletedCommand(command);
break;
}
}
catch (IotHubException ex)
{
exception = ex;
}
catch (Exception ex)
{
exception = ex;
}
if ((command != null) &&
(exception != null))
{
await this.SignalAbandonedCommand(command);
}
}
}
catch (TaskCanceledException)
{
//do nothing if the task was cancelled
}
catch (Exception ex)
{
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = string.Format("Unexpected Exception starting device receive loop: {0}", ex.ToString())
};
this.NotifyUIEvent(notifyEventArgs);
}
}