/// <summary>
/// Sends diagnostic data to the dashboard
/// </summary>
/// <param name="sender">The timer</param>
/// <param name="e">The event args</param>
private static void SendDiagnosticData(object sender, System.Timers.ElapsedEventArgs e)
{
//collect and send diagnostic robot data to the dashboard
if (!ConnectionLive || !RobotController.SystemOnline)
{
return;
}
string[] diagnosticData = new string[]
{
RobotController.ControlStatus.ToString(),
RobotController.GetRobotStatus().ToString(),
RobotController.RobotAutoControlState.ToString(),
Hardware.SignalVoltageRaw.ToString(), //raw signal voltage
Hardware.SignalCurrentRaw.ToString(), //raw signal current
Hardware.PowerVoltageRaw.ToString(), //raw power voltage
Hardware.PowerVoltage.ToString(), //raw power current
Hardware.WaterLevel.ToString(), //water level
Hardware.TempatureRaw.ToString(), //raw tempature
"NC", //CH6 (unused)
"NC", //CH7 (unused)
Hardware.LeftDrive.GetSignInt().ToString(), //sign
Math.Round(Hardware.LeftDrive.GetActiveDutyCyclePercentage(), 2).ToString(), //mag
Hardware.LeftEncoder.Clicks.ToString(), //encoder
Hardware.RightDrive.GetSignInt().ToString(), //sign
Math.Round(Hardware.RightDrive.GetActiveDutyCyclePercentage(), 2).ToString(), //mag
Hardware.RightEncoder.Clicks.ToString(), //encoder
Hardware.SignalVoltage.ToString(), //signal voltage
Hardware.SignalCurrent.ToString(), //signal current
Hardware.PowerVoltage.ToString(), //power voltage
Hardware.PowerCurrent.ToString(), //power current
Hardware.AccelerationX.ToString(), //accel X
Hardware.AccelerationY.ToString(), //accel Y
Hardware.AccelerationZ.ToString(), //accel Z
Hardware.GyroX.ToString(), //gyro X
Hardware.GyroY.ToString(), //gyro Y
Hardware.GyroZ.ToString(), //gyro Z
Hardware.Augar_Output.ToString(), //collection relay
Hardware.Impeller_Output.ToString(), //another collection relay
Hardware.VelocityX.ToString(), //velocity data
Hardware.VelocityY.ToString(), //velocity data
Hardware.VelocityZ.ToString(), //velocity data
Hardware.RotationX.ToString(), //rotation data
Hardware.RotationY.ToString(), //rotation data
Hardware.RotationZ.ToString(), //rotation data
Hardware.Tempature_2.ToString(), //MPU temp data
Hardware.SideReciever == null? "null": Convert.ToByte(Hardware.SideReciever.WallDetected).ToString(), //side wall detection
Hardware.FrontReciever == null? "null": Convert.ToByte(Hardware.FrontReciever.WallDetected).ToString(), //front wall detection
Hardware.the_only_distance_sensor == null? "null" : Hardware.the_only_distance_sensor.Distance_in_cm.ToString(), //proximity
Hardware.PositionX.ToString(), //positionx
Hardware.PositionY.ToString(), //positiony
Hardware.PositionZ.ToString(), //positionz
};
LogNetwork(string.Join(',', diagnosticData), MessageType.DiagnosticData);
}
private async void InitRobot(object sender, RoutedEventArgs e)
{
if (!Hardware.InitGPIO())
{
//https://stackoverflow.com/questions/32677597/how-to-exit-or-close-an-uwp-app-programmatically-windows-10
Application.Current.Exit();
return;
}
//init the robot networking
if (!NetworkUtils.InitComms())
{
RobotController.SetRobotStatus(RobotStatus.Error);
Application.Current.Exit();
return;
}
//DEBUG: wait for dashboard logging connection
if (NetworkUtils.DEBUG_FORCE_DASHBOARD_CONNECT)
{
while (!NetworkUtils.ConnectionLive)
{
System.Threading.Thread.Sleep(100);
}
NetworkUtils.LogNetwork("dashboard connected via force wait", MessageType.Debug);
}
//encoders
NetworkUtils.LogNetwork("Initializing Encoders", MessageType.Info);
if (!Hardware.InitEncoders())
{
NetworkUtils.LogNetwork("Encoders Failed to initialize", MessageType.Error);
RobotController.SetRobotStatus(RobotStatus.Error);
Application.Current.Exit();
}
//proximity
/*
* NetworkUtils.LogNetwork("Encoders initialized, loading proximity sensor", MessageType.Info);
* if(!Hardware.InitDistanceSensor())
* {
* NetworkUtils.LogNetwork("Proximity Failed to initialize", MessageType.Error);
* RobotController.SetRobotStatus(RobotStatus.Error);
* Application.Current.Exit();
* }*/
//SPI/ADC
NetworkUtils.LogNetwork("Proximity initialized, initializing SPI interface", MessageType.Info);
//http://blog.stephencleary.com/2012/07/dont-block-on-async-code.html
//https://docs.microsoft.com/en-us/uwp/api/windows.devices.enumeration.deviceinformation.findallasync
//https://stackoverflow.com/questions/33587832/prevent-winforms-ui-block-when-using-async-await
if (!await Hardware.InitSPI())
{
NetworkUtils.LogNetwork("SPI failed to intialize", MessageType.Error);
RobotController.SetRobotStatus(RobotStatus.Error);
Application.Current.Exit();
return;
}
//pwm
NetworkUtils.LogNetwork("SPI Interface initialization complete, loading PWM", MessageType.Info);
if (!await Hardware.InitPWM())
{
NetworkUtils.LogNetwork("PWM failed to intialize", MessageType.Error);
RobotController.SetRobotStatus(RobotStatus.Error);
Application.Current.Exit();
return;
}
//I2c
NetworkUtils.LogNetwork("PWM loading complete, I2C", MessageType.Info);
if (!await Hardware.InitI2C())
{
NetworkUtils.LogNetwork("I2C Failed to Initialize", MessageType.Error);
RobotController.SetRobotStatus(RobotStatus.Error);
Application.Current.Exit();
}
//main control system
NetworkUtils.LogNetwork("I2C loading complete, Initializing Main Robot controller", MessageType.Info);
if (!RobotController.InitController(this.Dispatcher))
{
NetworkUtils.LogNetwork("Controller failed to intialize", MessageType.Error);
RobotController.SetRobotStatus(RobotStatus.Error);
Application.Current.Exit();
return;
}
NetworkUtils.LogNetwork("Controller initialized, system online", MessageType.Info);
RobotController.SystemOnline = true;
}
/// <summary>
/// The main worker method for the networking thread, UDP method
/// </summary>
/// <param name="sender">The BackgroundWorker object</param>
/// <param name="e">The event args</param>
public static void ManageConnections(object sender, DoWorkEventArgs e)
{
if (ConnectionLive)
{
ConnectionLive = false;
}
NetworkPin.Write(ConnectionLive ? GpioPinValue.High : GpioPinValue.Low);
while (true)
{
if (ConnectionManager.CancellationPending)
{
e.Cancel = true;
HeartbeatTimer.Stop();
DiagnosticTimer.Stop();
return;
}
//verify stop the timer
HeartbeatTimer.Stop();
DiagnosticTimer.Stop();
//setup and bind listener
RobotRecieverIPEndPoint = new IPEndPoint(IPAddress.Parse(RobotIPV4Address), RobotRecieverPort);
RobotRecieverUDPClient = new UdpClient();
RobotRecieverUDPClient.Client.SendTimeout = 5000;//value is miliseconds
RobotRecieverUDPClient.Client.ReceiveTimeout = 5000;
RobotRecieverUDPClient.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
RobotRecieverUDPClient.Client.Bind(RobotRecieverIPEndPoint);
//wait for receiver client to get the ip address of the dashboard
bool receivedIP = false;
string result = null;
while (!receivedIP)
{
if (ConnectionManager.CancellationPending)
{
e.Cancel = true;
HeartbeatTimer.Stop();
DiagnosticTimer.Stop();
return;
}
try
{
result = Encoding.UTF8.GetString(RobotRecieverUDPClient.Receive(ref RobotRecieverIPEndPoint));
receivedIP = true;
}
catch (Exception)
{
}
}
//parse the ip address sent by the dashboard
IPAddress address;
try
{
address = IPAddress.Parse(result);
}
catch (Exception)
{
RobotController.SetRobotStatus(RobotStatus.Exception);
e.Cancel = true;
return;
}
if (address.AddressFamily == AddressFamily.InterNetworkV6)
{
DashboardIPV6Address = address.ToString();
}
else if (address.AddressFamily == AddressFamily.InterNetwork)
{
DashboardIPV4Address = address.ToString();
}
//setup and connect sender
RobotSenderIPEndPoint = new IPEndPoint(IPAddress.Parse(string.IsNullOrWhiteSpace(DashboardIPV6Address) ? DashboardIPV4Address : DashboardIPV6Address), RobotSenderPort);
RobotSenderUDPClient = new UdpClient();
RobotSenderUDPClient.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
RobotSenderUDPClient.Client.SendTimeout = 5000;//value is miliseconds
RobotSenderUDPClient.Client.ReceiveTimeout = 5000;
bool senderConnected = false;
while (!senderConnected)
{
if (ConnectionManager.CancellationPending)
{
e.Cancel = true;
HeartbeatTimer.Stop();
DiagnosticTimer.Stop();
return;
}
try
{
RobotSenderUDPClient.Connect(RobotSenderIPEndPoint);
senderConnected = true;
}
catch (Exception)
{
}
}
//send ack
RobotSenderUDPClient.Send(Encoding.UTF8.GetBytes("ack"), Encoding.UTF8.GetByteCount("ack"));
//start to send heartbeats
NumHeartbeatsSent = 0;
HeartbeatTimer.Start();
DiagnosticTimer.Start();
ConnectionLive = true;
NetworkPin.Write(ConnectionLive ? GpioPinValue.High : GpioPinValue.Low);
if (RecoveredException != null)
{
LogNetwork("The network thread just recovered from an exception level event\n" + RecoveredException.ToString(), MessageType.Exception);
RecoveredException = null;
}
//listen for dashboard events
while (ConnectionLive)
{
if (ConnectionManager.CancellationPending)
{
e.Cancel = true;
HeartbeatTimer.Stop();
DiagnosticTimer.Stop();
return;
}
try
{
result = Encoding.UTF8.GetString(RobotRecieverUDPClient.Receive(ref RobotRecieverIPEndPoint));
}
catch (SocketException)
{
if (DEBUG_IGNORE_TIMEOUT && !DEBUG_TCP_TEST)
{
continue;
}
//the dashboard has disocnnected!
ConnectionLive = false;
NetworkPin.Write(ConnectionLive ? GpioPinValue.High : GpioPinValue.Low);
e.Cancel = true;
HeartbeatTimer.Stop();
DiagnosticTimer.Stop();
return;
}
int messageTypeInt = -1;
string messageTypeString = result.Split(',')[0];
if (int.TryParse(messageTypeString, out messageTypeInt))
{
MessageType messageType = (MessageType)messageTypeInt;
switch (messageType)
{
case MessageType.Heartbeat:
//do nothing
break;
case MessageType.Control:
string controlMessage = result.Substring(messageTypeString.Count() + 1);
if (!controlMessage.Equals(ManualControlCommands))
{
//assuming thread access is atomic...
ManualControlCommands = controlMessage;
if (ManualControlCommands.Equals("Start"))
{
RobotController.ControlStatus = ControlStatus.RequestManual;
RobotController.ControllerThread.CancelAsync();
}
else if (ManualControlCommands.Equals("Stop"))
{
RobotController.ControlStatus = ControlStatus.RelaseManual;
RobotController.ControllerThread.CancelAsync();
}
else if (ManualControlCommands.Split(',')[0].Equals("Shutdown"))
{
RobotController.Poweroff(TimeSpan.FromSeconds(int.Parse(ManualControlCommands.Split(',')[1])));
}
else if (ManualControlCommands.Split(',')[0].Equals("Reboot"))
{
RobotController.Reboot(TimeSpan.FromSeconds(int.Parse(ManualControlCommands.Split(',')[1])));
}
else if (ManualControlCommands.Split(',')[0].Equals("Cancel_Shutdown"))
{
RobotController.CancelShutdown();
}
}
break;
}
}
}
}
}
/// <summary>
/// The main worker method for the networking thread, TCP method
/// </summary>
/// <param name="sender">The BackgroundWorker object</param>
/// <param name="e">The event args</param>
public static void ManageConnections_tcp(object sender, DoWorkEventArgs e)
{
ConnectionLive = false;
//Robot only
NetworkPin.Write(ConnectionLive ? GpioPinValue.High : GpioPinValue.Low);
while (true)
{
if (ConnectionManager.CancellationPending)
{
e.Cancel = true;
HeartbeatTimer.Stop();
DiagnosticTimer.Stop();
return;
}
//verify stop the timer
HeartbeatTimer.Stop();
DiagnosticTimer.Stop();
//setup and bind listener
RobotRecieverIPEndPoint = new IPEndPoint(IPAddress.Parse(RobotIPV4Address), RobotRecieverPort);
RobotReceiverTCPListener = new TcpListener(RobotRecieverIPEndPoint);
RobotReceiverTCPListener.Server.SendTimeout = 5000;//value is miliseconds
RobotReceiverTCPListener.Server.ReceiveTimeout = 5000;
//set socket before binding!
RobotReceiverTCPListener.Server.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
RobotReceiverTCPListener.Start();//now it's bound
//wait for receiver client to get the ip address of the dashboard
IPAddress address = null;
bool recieverConnected = false;
while (!recieverConnected)
{
if (ConnectionManager.CancellationPending)
{
e.Cancel = true;
HeartbeatTimer.Stop();
DiagnosticTimer.Stop();
return;
}
try
{
//try to accept the tcp connection and parse the ip address
RobotRecieverTCPClient = RobotReceiverTCPListener.AcceptTcpClient();
address = IPAddress.Parse(TCPRecieve(RobotRecieverTCPClient));
recieverConnected = true;
}
catch (Exception)
{
//RobotController.RobotStatus = RobotStatus.Exception;
//keep trying
}
}
if (address.AddressFamily == AddressFamily.InterNetworkV6)
{
DashboardIPV6Address = address.ToString();
}
else if (address.AddressFamily == AddressFamily.InterNetwork)
{
DashboardIPV4Address = address.ToString();
}
//for now we'er just force using the IPV4 stuff
//setup and bind sender to send ack
RobotSenderIPEndPoint = new IPEndPoint(IPAddress.Parse(string.IsNullOrWhiteSpace(DashboardIPV6Address) ? DashboardIPV4Address : DashboardIPV6Address), RobotSenderPort);
RobotSenderTCPClient = new TcpClient();
RobotSenderTCPClient.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
RobotSenderTCPClient.Client.SendTimeout = 5000;//value is miliseconds
RobotSenderTCPClient.Client.ReceiveTimeout = 5000;
bool senderConnected = false;
while (!senderConnected)
{
if (ConnectionManager.CancellationPending)
{
e.Cancel = true;
HeartbeatTimer.Stop();
DiagnosticTimer.Stop();
return;
}
try
{
RobotSenderTCPClient.Connect(RobotSenderIPEndPoint);
senderConnected = true;
}
catch (SocketException)
{
//keep trying
}
}
RobotSenderTCPStream = RobotSenderTCPClient.GetStream();
//set new timeout settings for the robot receiver
if (RobotRecieverTCPClient.Client.ReceiveTimeout != 5000)
{
RobotRecieverTCPClient.Client.ReceiveTimeout = 5000;
}
if (RobotRecieverTCPClient.Client.SendTimeout != 5000)
{
RobotRecieverTCPClient.Client.SendTimeout = 5000;
}
if (RobotRecieverTCPClient.GetStream().CanTimeout)
{
if (RobotRecieverTCPClient.GetStream().ReadTimeout != 5000)
{
RobotRecieverTCPClient.GetStream().ReadTimeout = 5000;
}
if (RobotRecieverTCPClient.GetStream().WriteTimeout != 5000)
{
RobotRecieverTCPClient.GetStream().WriteTimeout = 5000;
}
}
//send ack to dashboard
if (!TCPSend(RobotSenderTCPStream, "ack"))
{
continue;
}
//start to send heartbeats
ConnectionLive = true;
NetworkPin.Write(ConnectionLive ? GpioPinValue.High : GpioPinValue.Low);
NumHeartbeatsSent = 0;
HeartbeatTimer.Start();
DiagnosticTimer.Start();
if (RecoveredException != null)
{
LogNetwork("The network thread just recovered from an exception level event\n" + RecoveredException.ToString(), MessageType.Exception);
RecoveredException = null;
}
//listen for dashboard events
string result = null;
while (ConnectionLive)
{
if (ConnectionManager.CancellationPending)
{
e.Cancel = true;
HeartbeatTimer.Stop();
DiagnosticTimer.Stop();
return;
}
result = TCPRecieve(RobotRecieverTCPClient);
if (string.IsNullOrWhiteSpace(result))
{
if (DEBUG_IGNORE_TIMEOUT && DEBUG_TCP_TEST)
{
Thread.Sleep(1000);
continue;
}
//the dashboard has disocnnected!
ConnectionLive = false;
NetworkPin.Write(ConnectionLive ? GpioPinValue.High : GpioPinValue.Low);
Disconnect();
break;
}
int messageTypeInt = -1;
string messageTypeString = result.Split(',')[0];
if (int.TryParse(messageTypeString, out messageTypeInt))
{
MessageType messageType = (MessageType)messageTypeInt;
switch (messageType)
{
case MessageType.Heartbeat:
//do nothing
break;
case MessageType.Control:
string controlMessage = result.Substring(messageTypeString.Count() + 1);
if (!controlMessage.Equals(ManualControlCommands))
{
//assuming thread access is atomic...
ManualControlCommands = controlMessage;
if (ManualControlCommands.Equals("Start"))
{
RobotController.ControlStatus = ControlStatus.RequestManual;
RobotController.ControllerThread.CancelAsync();
}
else if (ManualControlCommands.Equals("Stop"))
{
RobotController.ControlStatus = ControlStatus.RelaseManual;
RobotController.ControllerThread.CancelAsync();
}
else if (ManualControlCommands.Split(',')[0].Equals("Shutdown"))
{
RobotController.Poweroff(TimeSpan.FromSeconds(int.Parse(ManualControlCommands.Split(',')[1])));
}
else if (ManualControlCommands.Split(',')[0].Equals("Reboot"))
{
RobotController.Reboot(TimeSpan.FromSeconds(int.Parse(ManualControlCommands.Split(',')[1])));
}
else if (ManualControlCommands.Split(',')[0].Equals("Cancel_Shutdown"))
{
RobotController.CancelShutdown();
}
}
break;
}
}
}
}
}
/// <summary>
/// Occures when the application encounters an unhandled exception. The applicatoin is unloaded and all resources released
/// </summary>
/// <param name="sender">The source that caused the exception</param>
/// <param name="e">The exception object entry</param>
private void Application_UnhandledException(object sender, Windows.UI.Xaml.UnhandledExceptionEventArgs e)
{
RobotController.SetRobotStatus(RobotStatus.Exception);
NetworkUtils.LogNetwork(e.ToString(), MessageType.Exception);
Current.Exit();
}
