/// <summary>
/// Update state after recieving return data from robot
/// </summary>
/// <param name="response">response from scribbler robot</param>
private IEnumerator <ITask> ScribblerResponseHandler(ScribblerResponseMessage response) //private void UpdateState(ScribblerResponse response) //debug
{
//initialize notify list
List <string> notify = new List <string>();
switch ((ScribblerHelper.Commands)response.Body.CommandType)
{
case ScribblerHelper.Commands.GET_STATE:
ScribblerHelper.GetStatusDecomp parse_get_state = new ScribblerHelper.GetStatusDecomp(response.Body.Data[0], response.Body.Data[1]);
if (_state.Stall != parse_get_state.Stall)
{
notify.Add("STALL");
_state.Stall = parse_get_state.Stall;
}
if (_state.LineLeft != parse_get_state.LineLeft)
{
notify.Add("LINELEFT");
_state.LineLeft = parse_get_state.LineLeft;
}
if (_state.LineRight != parse_get_state.LineRight)
{
notify.Add("LINERIGHT");
_state.LineRight = parse_get_state.LineRight;
}
_state.LEDLeft = parse_get_state.LedLeft;
_state.LEDCenter = parse_get_state.LedCenter;
_state.LEDRight = parse_get_state.LedRight;
break;
case ScribblerHelper.Commands.GET_IR_LEFT:
bool New_Get_Open_Left = response.Body.Data[0] == 1; //NOTE: Not inverting logic here
if (_state.IRLeft != New_Get_Open_Left)
{
notify.Add("IRLEFT");
_state.IRLeft = New_Get_Open_Left;
}
break;
case ScribblerHelper.Commands.GET_IR_RIGHT:
bool New_Get_Open_Right = response.Body.Data[0] == 1; //NOTE: Not inverting logic here
if (_state.IRRight != New_Get_Open_Right)
{
notify.Add("IRRIGHT");
_state.IRRight = New_Get_Open_Right;
}
break;
case ScribblerHelper.Commands.GET_STALL:
bool New_Get_Stall = response.Body.Data[0] == 1;
if (_state.Stall != New_Get_Stall)
{
notify.Add("STALL");
_state.Stall = New_Get_Stall;
}
break;
case ScribblerHelper.Commands.GET_LIGHT_LEFT:
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Body.Data[1], response.Body.Data[0] }, 0);
notify.Add("LIGHTLEFT");
break;
case ScribblerHelper.Commands.GET_LIGHT_CENTER:
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Body.Data[1], response.Body.Data[0] }, 0);
notify.Add("LIGHTCENTER");
break;
case ScribblerHelper.Commands.GET_LIGHT_RIGHT:
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Body.Data[1], response.Body.Data[0] }, 0);
notify.Add("LIGHTRIGHT");
break;
case ScribblerHelper.Commands.GET_LINE_RIGHT:
bool New_Get_Line_Right = response.Body.Data[0] == 1;
if (_state.LineRight != New_Get_Line_Right)
{
notify.Add("LINERIGHT");
_state.LineRight = New_Get_Line_Right;
}
break;
case ScribblerHelper.Commands.GET_LINE_LEFT:
bool New_Get_Line_Left = response.Body.Data[0] == 1;
if (_state.LineLeft != New_Get_Line_Left)
{
notify.Add("LINELEFT");
_state.LineLeft = New_Get_Line_Left;
}
break;
case ScribblerHelper.Commands.GET_NAME:
Encoding.ASCII.GetChars(response.Body.Data, 0, 8);
break;
case ScribblerHelper.Commands.GET_LIGHT_ALL:
//TODO: this can be simplified if the scribbler sent low byte first
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Body.Data[1], response.Body.Data[0] }, 0);
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Body.Data[3], response.Body.Data[2] }, 0);
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Body.Data[5], response.Body.Data[4] }, 0);
_state.LightSensorsLastUpdated = DateTime.Now;
notify.Add("LIGHTLEFT");
notify.Add("LIGHTCENTER");
notify.Add("LIGHTRIGHT");
break;
case ScribblerHelper.Commands.GET_IR_ALL:
bool newirleft = response.Body.Data[0] == 1; //NOTE: Not inverting logic here
bool newirright = response.Body.Data[1] == 1;
if (_state.IRLeft != newirleft)
{
notify.Add("IRLEFT");
_state.IRLeft = newirleft;
}
if (_state.IRRight != newirright)
{
notify.Add("IRRIGHT");
_state.IRRight = newirright;
}
_state.LineSensorsLastUpdated = DateTime.Now;
_state.BumpersLastUpdated = DateTime.Now;
break;
case ScribblerHelper.Commands.GET_LINE_ALL:
bool newlineleft = response.Body.Data[0] == 1;
bool newlineright = response.Body.Data[1] == 1;
if (_state.LineLeft != newlineleft)
{
notify.Add("LINELEFT");
_state.LineLeft = newlineleft;
}
if (_state.LineRight != newlineright)
{
notify.Add("LINERIGHT");
_state.LineRight = newlineright;
}
_state.LineSensorsLastUpdated = DateTime.Now;
break;
case ScribblerHelper.Commands.GET_ALL:
case ScribblerHelper.Commands.SET_MOTORS_OFF:
case ScribblerHelper.Commands.SET_MOTORS:
case ScribblerHelper.Commands.SET_LED_LEFT_ON:
case ScribblerHelper.Commands.SET_LED_LEFT_OFF:
case ScribblerHelper.Commands.SET_LED_CENTER_ON:
case ScribblerHelper.Commands.SET_LED_CENTER_OFF:
case ScribblerHelper.Commands.SET_LED_RIGHT_ON:
case ScribblerHelper.Commands.SET_LED_RIGHT_OFF:
case ScribblerHelper.Commands.SET_SPEAKER:
case ScribblerHelper.Commands.SET_SPEAKER_2:
case ScribblerHelper.Commands.SET_NAME:
case ScribblerHelper.Commands.SET_LED_ALL_ON:
case ScribblerHelper.Commands.SET_LED_ALL_OFF:
case ScribblerHelper.Commands.SET_LED_ALL:
case ScribblerHelper.Commands.SET_LOUD:
case ScribblerHelper.Commands.SET_QUIET:
case ScribblerHelper.Commands.SET_DATA:
case ScribblerHelper.Commands.SET_ECHO_MODE:
bool New_Get_All_IR_Left = response.Body.Data[0] == 1; //NOTE: Not inverting logic here
bool New_Get_All_IR_Right = response.Body.Data[1] == 1;
if (_state.IRLeft != New_Get_All_IR_Left)
{
notify.Add("IRLEFT");
_state.IRLeft = New_Get_All_IR_Left;
}
if (_state.IRRight != New_Get_All_IR_Right)
{
notify.Add("IRRIGHT");
_state.IRRight = New_Get_All_IR_Right;
}
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Body.Data[3], response.Body.Data[2] }, 0);
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Body.Data[5], response.Body.Data[4] }, 0);
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Body.Data[7], response.Body.Data[6] }, 0);
notify.Add("LIGHTLEFT");
notify.Add("LIGHTCENTER");
notify.Add("LIGHTRIGHT");
bool New_Get_All_Line_Left = response.Body.Data[8] == 1;
bool New_Get_All_Line_Right = response.Body.Data[9] == 1;
if (_state.LineLeft != New_Get_All_Line_Left)
{
notify.Add("LINELEFT");
_state.LineLeft = New_Get_All_Line_Left;
}
if (_state.LineRight != New_Get_All_Line_Right)
{
notify.Add("LINERIGHT");
_state.LineRight = New_Get_All_Line_Right;
}
bool newstall = response.Body.Data[10] == 1;
if (_state.Stall != newstall)
{
notify.Add("STALL");
_state.Stall = newstall;
}
_state.LineSensorsLastUpdated = DateTime.Now;
_state.BumpersLastUpdated = DateTime.Now;
_state.LightSensorsLastUpdated = DateTime.Now;
break;
case ScribblerHelper.Commands.GET_ALL_BINARY:
ScribblerHelper.AllBinaryDecomp parse_all_binary = new ScribblerHelper.AllBinaryDecomp(response.Body.Data[0]);
if (_state.IRLeft != parse_all_binary.IRLeft)
{
notify.Add("IRLEFT");
_state.IRLeft = parse_all_binary.IRLeft; //NOTE: Not inverting logic here
}
if (_state.IRRight != parse_all_binary.IRRight)
{
notify.Add("IRRIGHT");
_state.IRRight = parse_all_binary.IRRight;
}
if (_state.LineLeft != parse_all_binary.LineLeft)
{
notify.Add("LINELEFT");
_state.LineLeft = parse_all_binary.LineLeft;
}
if (_state.LineRight != parse_all_binary.LineRight)
{
notify.Add("LINERIGHT");
_state.LineRight = parse_all_binary.LineRight;
}
if (_state.Stall != parse_all_binary.Stall)
{
notify.Add("STALL");
_state.Stall = parse_all_binary.Stall;
}
_state.LineSensorsLastUpdated = DateTime.Now;
_state.BumpersLastUpdated = DateTime.Now;
break;
case ScribblerHelper.Commands.GET_INFO:
Console.WriteLine(System.Text.Encoding.ASCII.GetChars(response.Body.Data));
break;
case ScribblerHelper.Commands.GET_DATA:
foreach (byte b in response.Body.Data)
{
Console.Write(b);
}
Console.Write("\n");
break;
default:
LogError(LogGroups.Console, "Update State command missmatch");
break;
}
// notify general subscribers
subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
// notify selective subscribers
submgr.Submit sub = new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest, notify.ToArray());
subMgrPort.Post(sub);
yield break;
}
/// <summary>
/// Handles incoming sensor messages
/// </summary>
private IEnumerator <ITask> SensorNotificationHandler(SensorNotification message)
{
//initialize notify list
List <string> notify = new List <string>();
//update state
switch (ScribblerHelper.SensorType((byte)message.Sensor))
{
case "IRLeft":
_state.IRLeft = (message.Status > 0);
notify.Add("IRLEFT");
break;
case "IRRight":
_state.IRRight = (message.Status > 0);
notify.Add("IRRIGHT");
break;
case "Stall":
_state.Stall = (message.Status > 0);
notify.Add("STALL");
break;
case "LineLeft":
_state.LineLeft = (message.Status > 0);
notify.Add("LINELEFT");
break;
case "LineRight":
_state.LineRight = (message.Status > 0);
notify.Add("LINERIGHT");
break;
case "LightLeft":
_state.LightLeft = message.Status;
notify.Add("LIGHTLEFT");
break;
case "LightRight":
_state.LightRight = message.Status;
notify.Add("LIGHTRIGHT");
break;
case "LightCenter":
_state.LightCenter = message.Status;
notify.Add("LIGHTCENTER");
break;
//only notify if there is a change
case "AllBinary":
ScribblerHelper.AllBinaryDecomp newState = new ScribblerHelper.AllBinaryDecomp(message.Status);
if (newState.IRLeft != _state.IRLeft)
{
notify.Add("IRLEFT");
_state.IRLeft = newState.IRLeft;
}
if (newState.IRRight != _state.IRRight)
{
notify.Add("IRRIGHT");
_state.IRRight = newState.IRRight;
}
if (newState.Stall != _state.Stall)
{
notify.Add("STALL");
_state.Stall = newState.Stall;
}
if (newState.LineLeft != _state.LineLeft)
{
notify.Add("LINELEFT");
_state.LineLeft = newState.LineLeft;
}
if (newState.LineRight != _state.LineRight)
{
notify.Add("LINERIGHT");
_state.LineRight = newState.LineRight;
}
break;
default:
LogError("Unrecognized sensor type");
//throw new ArgumentException("Sensor update error");
break;
}
// notify general subscribers
subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
// notify selective subscribers
submgr.Submit sub = new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest, notify.ToArray());
subMgrPort.Post(sub);
yield break;
}
/// <summary>
/// Update state after recieving return data from robot
/// </summary>
/// <param name="response"></param>
private void UpdateState(ScribblerResponse response)
{
//initialize notify list
List <string> notify = new List <string>();
switch ((ScribblerHelper.Commands)response.CommandType)
{
case ScribblerHelper.Commands.GET_STATE:
ScribblerHelper.GetStatusDecomp parse_get_state = new ScribblerHelper.GetStatusDecomp(response.Data[0], response.Data[1]);
if (_state.Stall != parse_get_state.Stall)
{
notify.Add("STALL");
_state.Stall = parse_get_state.Stall;
}
if (_state.LineLeft != parse_get_state.LineLeft)
{
notify.Add("LINELEFT");
_state.LineLeft = parse_get_state.LineLeft;
}
if (_state.LineRight != parse_get_state.LineRight)
{
notify.Add("LINERIGHT");
_state.LineRight = parse_get_state.LineRight;
}
_state.LEDLeft = parse_get_state.LedLeft;
_state.LEDCenter = parse_get_state.LedCenter;
_state.LEDRight = parse_get_state.LedRight;
break;
case ScribblerHelper.Commands.GET_OPEN_LEFT:
bool New_Get_Open_Left = response.Data[0] == 1; //NOTE: Not inverting logic here
if (_state.IRLeft != New_Get_Open_Left)
{
notify.Add("IRLEFT");
_state.IRLeft = New_Get_Open_Left;
}
break;
case ScribblerHelper.Commands.GET_OPEN_RIGHT:
bool New_Get_Open_Right = response.Data[0] == 1; //NOTE: Not inverting logic here
if (_state.IRRight != New_Get_Open_Right)
{
notify.Add("IRRIGHT");
_state.IRRight = New_Get_Open_Right;
}
break;
case ScribblerHelper.Commands.GET_STALL:
bool New_Get_Stall = response.Data[0] == 1;
if (_state.Stall != New_Get_Stall)
{
notify.Add("STALL");
_state.Stall = New_Get_Stall;
}
break;
case ScribblerHelper.Commands.GET_LIGHT_LEFT:
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
//if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
notify.Add("LIGHTLEFT");
break;
case ScribblerHelper.Commands.GET_LIGHT_CENTER:
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
//if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
notify.Add("LIGHTCENTER");
break;
case ScribblerHelper.Commands.GET_LIGHT_RIGHT:
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
//if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
notify.Add("LIGHTRIGHT");
break;
case ScribblerHelper.Commands.GET_LINE_RIGHT:
bool New_Get_Line_Right = response.Data[0] == 1;
if (_state.LineRight != New_Get_Line_Right)
{
notify.Add("LINERIGHT");
_state.LineRight = New_Get_Line_Right;
}
break;
case ScribblerHelper.Commands.GET_LINE_LEFT:
bool New_Get_Line_Left = response.Data[0] == 1;
if (_state.LineLeft != New_Get_Line_Left)
{
notify.Add("LINELEFT");
_state.LineLeft = New_Get_Line_Left;
}
break;
case ScribblerHelper.Commands.GET_NAME:
Encoding.ASCII.GetChars(response.Data, 0, 8);
break;
case ScribblerHelper.Commands.GET_LIGHT_ALL:
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[3], response.Data[2] }, 0);
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[5], response.Data[4] }, 0);
//if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
notify.Add("LIGHTLEFT");
//if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
notify.Add("LIGHTCENTER");
//if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
notify.Add("LIGHTRIGHT");
break;
case ScribblerHelper.Commands.GET_IR_ALL:
bool newirleft = response.Data[0] == 1; //NOTE: Not inverting logic here
bool newirright = response.Data[1] == 1;
if (_state.IRLeft != newirleft)
{
notify.Add("IRLEFT");
_state.IRLeft = newirleft;
}
if (_state.IRRight != newirright)
{
notify.Add("IRRIGHT");
_state.IRRight = newirright;
}
break;
case ScribblerHelper.Commands.GET_LINE_ALL:
bool newlineleft = response.Data[0] == 1;
bool newlineright = response.Data[1] == 1;
if (_state.LineLeft != newlineleft)
{
notify.Add("LINELEFT");
_state.LineLeft = newlineleft;
}
if (_state.LineRight != newlineright)
{
notify.Add("LINERIGHT");
_state.LineRight = newlineright;
}
break;
case ScribblerHelper.Commands.GET_ALL:
bool New_Get_All_IR_Left = response.Data[0] == 1; //NOTE: Not inverting logic here
bool New_Get_All_IR_Right = response.Data[1] == 1;
if (_state.IRLeft != New_Get_All_IR_Left)
{
notify.Add("IRLEFT");
_state.IRLeft = New_Get_All_IR_Left;
}
if (_state.IRRight != New_Get_All_IR_Right)
{
notify.Add("IRRIGHT");
_state.IRRight = New_Get_All_IR_Right;
}
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Data[3], response.Data[2] }, 0);
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[5], response.Data[4] }, 0);
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[7], response.Data[6] }, 0);
//if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
notify.Add("LIGHTLEFT");
//if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
notify.Add("LIGHTCENTER");
//if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
notify.Add("LIGHTRIGHT");
bool New_Get_All_Line_Left = response.Data[8] == 1;
bool New_Get_All_Line_Right = response.Data[9] == 1;
if (_state.LineLeft != New_Get_All_Line_Left)
{
notify.Add("LINELEFT");
_state.LineLeft = New_Get_All_Line_Left;
}
if (_state.LineRight != New_Get_All_Line_Right)
{
notify.Add("LINERIGHT");
_state.LineRight = New_Get_All_Line_Right;
}
bool newstall = response.Data[10] == 1;
if (_state.Stall != newstall)
{
notify.Add("STALL");
_state.Stall = newstall;
}
break;
case ScribblerHelper.Commands.GET_ALL_BINARY:
case ScribblerHelper.Commands.SET_MOTORS_OFF:
case ScribblerHelper.Commands.SET_MOTORS:
case ScribblerHelper.Commands.SET_LED_LEFT_ON:
case ScribblerHelper.Commands.SET_LED_LEFT_OFF:
case ScribblerHelper.Commands.SET_LED_CENTER_ON:
case ScribblerHelper.Commands.SET_LED_CENTER_OFF:
case ScribblerHelper.Commands.SET_LED_RIGHT_ON:
case ScribblerHelper.Commands.SET_LED_RIGHT_OFF:
case ScribblerHelper.Commands.SET_SPEAKER:
case ScribblerHelper.Commands.SET_SPEAKER_2:
case ScribblerHelper.Commands.SET_NAME:
case ScribblerHelper.Commands.SET_LED_ALL_ON:
case ScribblerHelper.Commands.SET_LED_ALL_OFF:
case ScribblerHelper.Commands.SET_LOUD:
case ScribblerHelper.Commands.SET_QUIET:
ScribblerHelper.AllBinaryDecomp parse_all_binary = new ScribblerHelper.AllBinaryDecomp(response.Data[0]);
if (_state.IRLeft != parse_all_binary.IRLeft)
{
notify.Add("IRLEFT");
_state.IRLeft = parse_all_binary.IRLeft; //NOTE: Not inverting logic here
}
if (_state.IRRight != parse_all_binary.IRRight)
{
notify.Add("IRRIGHT");
_state.IRRight = parse_all_binary.IRRight;
}
if (_state.LineLeft != parse_all_binary.LineLeft)
{
notify.Add("LINELEFT");
_state.LineLeft = parse_all_binary.LineLeft;
}
if (_state.LineRight != parse_all_binary.LineRight)
{
notify.Add("LINERIGHT");
_state.LineRight = parse_all_binary.LineRight;
}
if (_state.Stall != parse_all_binary.Stall)
{
notify.Add("STALL");
_state.Stall = parse_all_binary.Stall;
}
break;
default:
LogError(LogGroups.Console, "Update State command missmatch");
break;
}
// notify general subscribers
subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
// notify selective subscribers
submgr.Submit sub = new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest, notify.ToArray());
subMgrPort.Post(sub);
}
/// <summary>
/// Update state after recieving return data from robot
/// </summary>
/// <param name="response"></param>
private void UpdateState(ScribblerResponse response)
{
//initialize notify list
List<string> notify = new List<string>();
switch ((ScribblerHelper.Commands)response.CommandType)
{
case ScribblerHelper.Commands.GET_STATE:
ScribblerHelper.GetStatusDecomp parse_get_state = new ScribblerHelper.GetStatusDecomp(response.Data[0], response.Data[1]);
if (_state.Stall != parse_get_state.Stall)
{
notify.Add("STALL");
_state.Stall = parse_get_state.Stall;
}
if (_state.LineLeft != parse_get_state.LineLeft)
{
notify.Add("LINELEFT");
_state.LineLeft = parse_get_state.LineLeft;
}
if (_state.LineRight != parse_get_state.LineRight)
{
notify.Add("LINERIGHT");
_state.LineRight = parse_get_state.LineRight;
}
_state.LEDLeft = parse_get_state.LedLeft;
_state.LEDCenter = parse_get_state.LedCenter;
_state.LEDRight = parse_get_state.LedRight;
break;
case ScribblerHelper.Commands.GET_OPEN_LEFT:
bool New_Get_Open_Left = response.Data[0] == 1; //NOTE: Not inverting logic here
if (_state.IRLeft != New_Get_Open_Left)
{
notify.Add("IRLEFT");
_state.IRLeft = New_Get_Open_Left;
}
break;
case ScribblerHelper.Commands.GET_OPEN_RIGHT:
bool New_Get_Open_Right = response.Data[0] == 1; //NOTE: Not inverting logic here
if (_state.IRRight != New_Get_Open_Right)
{
notify.Add("IRRIGHT");
_state.IRRight = New_Get_Open_Right;
}
break;
case ScribblerHelper.Commands.GET_STALL:
bool New_Get_Stall = response.Data[0] == 1;
if (_state.Stall != New_Get_Stall)
{
notify.Add("STALL");
_state.Stall = New_Get_Stall;
}
break;
case ScribblerHelper.Commands.GET_LIGHT_LEFT:
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
//if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
notify.Add("LIGHTLEFT");
break;
case ScribblerHelper.Commands.GET_LIGHT_CENTER:
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
//if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
notify.Add("LIGHTCENTER");
break;
case ScribblerHelper.Commands.GET_LIGHT_RIGHT:
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
//if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
notify.Add("LIGHTRIGHT");
break;
case ScribblerHelper.Commands.GET_LINE_RIGHT:
bool New_Get_Line_Right = response.Data[0] == 1;
if (_state.LineRight != New_Get_Line_Right)
{
notify.Add("LINERIGHT");
_state.LineRight = New_Get_Line_Right;
}
break;
case ScribblerHelper.Commands.GET_LINE_LEFT:
bool New_Get_Line_Left = response.Data[0] == 1;
if (_state.LineLeft != New_Get_Line_Left)
{
notify.Add("LINELEFT");
_state.LineLeft = New_Get_Line_Left;
}
break;
case ScribblerHelper.Commands.GET_NAME:
Encoding.ASCII.GetChars(response.Data, 0, 8);
break;
case ScribblerHelper.Commands.GET_LIGHT_ALL:
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[3], response.Data[2] }, 0);
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[5], response.Data[4] }, 0);
//if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
notify.Add("LIGHTLEFT");
//if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
notify.Add("LIGHTCENTER");
//if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
notify.Add("LIGHTRIGHT");
break;
case ScribblerHelper.Commands.GET_IR_ALL:
bool newirleft = response.Data[0] == 1; //NOTE: Not inverting logic here
bool newirright = response.Data[1] == 1;
if (_state.IRLeft != newirleft)
{
notify.Add("IRLEFT");
_state.IRLeft = newirleft;
}
if (_state.IRRight != newirright)
{
notify.Add("IRRIGHT");
_state.IRRight = newirright;
}
break;
case ScribblerHelper.Commands.GET_LINE_ALL:
bool newlineleft = response.Data[0] == 1;
bool newlineright = response.Data[1] == 1;
if (_state.LineLeft != newlineleft)
{
notify.Add("LINELEFT");
_state.LineLeft = newlineleft;
}
if (_state.LineRight != newlineright)
{
notify.Add("LINERIGHT");
_state.LineRight = newlineright;
}
break;
case ScribblerHelper.Commands.GET_ALL:
bool New_Get_All_IR_Left = response.Data[0] == 1; //NOTE: Not inverting logic here
bool New_Get_All_IR_Right = response.Data[1] == 1;
if (_state.IRLeft != New_Get_All_IR_Left)
{
notify.Add("IRLEFT");
_state.IRLeft = New_Get_All_IR_Left;
}
if (_state.IRRight != New_Get_All_IR_Right)
{
notify.Add("IRRIGHT");
_state.IRRight = New_Get_All_IR_Right;
}
_state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Data[3], response.Data[2] }, 0);
_state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[5], response.Data[4] }, 0);
_state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[7], response.Data[6] }, 0);
//if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
notify.Add("LIGHTLEFT");
//if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
notify.Add("LIGHTCENTER");
//if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
notify.Add("LIGHTRIGHT");
bool New_Get_All_Line_Left = response.Data[8] == 1;
bool New_Get_All_Line_Right = response.Data[9] == 1;
if (_state.LineLeft != New_Get_All_Line_Left)
{
notify.Add("LINELEFT");
_state.LineLeft = New_Get_All_Line_Left;
}
if (_state.LineRight != New_Get_All_Line_Right)
{
notify.Add("LINERIGHT");
_state.LineRight = New_Get_All_Line_Right;
}
bool newstall = response.Data[10] == 1;
if (_state.Stall != newstall)
{
notify.Add("STALL");
_state.Stall = newstall;
}
break;
case ScribblerHelper.Commands.GET_ALL_BINARY:
case ScribblerHelper.Commands.SET_MOTORS_OFF:
case ScribblerHelper.Commands.SET_MOTORS:
case ScribblerHelper.Commands.SET_LED_LEFT_ON:
case ScribblerHelper.Commands.SET_LED_LEFT_OFF:
case ScribblerHelper.Commands.SET_LED_CENTER_ON:
case ScribblerHelper.Commands.SET_LED_CENTER_OFF:
case ScribblerHelper.Commands.SET_LED_RIGHT_ON:
case ScribblerHelper.Commands.SET_LED_RIGHT_OFF:
case ScribblerHelper.Commands.SET_SPEAKER:
case ScribblerHelper.Commands.SET_SPEAKER_2:
case ScribblerHelper.Commands.SET_NAME:
case ScribblerHelper.Commands.SET_LED_ALL_ON:
case ScribblerHelper.Commands.SET_LED_ALL_OFF:
case ScribblerHelper.Commands.SET_LOUD:
case ScribblerHelper.Commands.SET_QUIET:
ScribblerHelper.AllBinaryDecomp parse_all_binary = new ScribblerHelper.AllBinaryDecomp(response.Data[0]);
if (_state.IRLeft != parse_all_binary.IRLeft)
{
notify.Add("IRLEFT");
_state.IRLeft = parse_all_binary.IRLeft; //NOTE: Not inverting logic here
}
if (_state.IRRight != parse_all_binary.IRRight)
{
notify.Add("IRRIGHT");
_state.IRRight = parse_all_binary.IRRight;
}
if (_state.LineLeft != parse_all_binary.LineLeft)
{
notify.Add("LINELEFT");
_state.LineLeft = parse_all_binary.LineLeft;
}
if (_state.LineRight != parse_all_binary.LineRight)
{
notify.Add("LINERIGHT");
_state.LineRight = parse_all_binary.LineRight;
}
if (_state.Stall != parse_all_binary.Stall)
{
notify.Add("STALL");
_state.Stall = parse_all_binary.Stall;
}
break;
default:
LogError(LogGroups.Console, "Update State command missmatch");
break;
}
// notify general subscribers
subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
// notify selective subscribers
submgr.Submit sub = new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest, notify.ToArray());
subMgrPort.Post(sub);
}