/// <summary>
/// Connect to a Animated Head.
/// If no configuration exists, search for the connection.
/// The code here is close to GPS service code from Microsoft RDS R3.
/// </summary>
private IEnumerator <ITask> ConnectToAnimatedHead()
{
try
{
//_state.Quaternion = null;
_state.Connected = false;
if (_state.AnimatedHeadServiceConfig.CommPort != 0 && _state.AnimatedHeadServiceConfig.BaudRate != 0)
{
_state.AnimatedHeadServiceConfig.ConfigurationStatus = "Opening Animated Head on Port " + _state.AnimatedHeadServiceConfig.CommPort.ToString();
_state.Connected = _ahConnection.Open(_state.AnimatedHeadServiceConfig.CommPort, _state.AnimatedHeadServiceConfig.BaudRate);
if (_state.Connected)
{
_state.AnimatedHeadServiceConfig.ConfigurationStatus = "Connected at saved Port " + _ahConnection.getPortName();
}
}
else
{
_state.AnimatedHeadServiceConfig.ConfigurationStatus = "Searching for the Animated Head Port";
_state.Connected = _ahConnection.FindAnimatedHead();
if (_state.Connected)
{
_state.AnimatedHeadServiceConfig.ConfigurationStatus = "Connected at found Port " + _ahConnection.getPortName();
_state.AnimatedHeadServiceConfig = _ahConnection.AnimatedHeadConfig;
SaveState(_state);
}
}
if (_state.Connected)
{
brickConnector.Open(_ahConnection);
//SafePosture();
}
}
catch (UnauthorizedAccessException ex)
{
LogError(ex);
}
catch (IOException ex)
{
LogError(ex);
}
catch (ArgumentOutOfRangeException ex)
{
LogError(ex);
}
catch (ArgumentException ex)
{
LogError(ex);
}
catch (InvalidOperationException ex)
{
LogError(ex);
}
if (!_state.Connected)
{
_state.AnimatedHeadServiceConfig.ConfigurationStatus = "Not Connected";
string msg = "The Animated Head is not detected.\r\n* To configure the Animated Head, navigate to: ";
LogInfo(LogGroups.Console, msg);
Console.WriteLine(msg);
}
else
{
string msg = "The Animated Head connected at Port " + _ahConnection.getPortName();
LogInfo(LogGroups.Console, msg);
Console.WriteLine(msg);
}
yield break;
}
private void OpenBrickConnector()
{
//brickConnector.Open("COM6", 115200); // hand
brickConnector.Open("COM11", 115200); // shoulder
}
private void OpenBrickConnector()
{
brickConnector.Open("COM12");
}