/// <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, got " + response.Body.CommandType);
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">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;
case 0:
//Console.WriteLine("Got 0 command");
// Do nothing
break;
default:
LogError(LogGroups.Console, "Update State command missmatch, got " + response.Body.CommandType);
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;
}