/// <summary>
/// Handles incoming set motor messages
/// </summary>
private IEnumerator <ITask> SetMotorHandler(SetMotor message)
{
// Requests come too fast, so dump ones that come in too fast.
if (RequestPending > 0)
{
message.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
RequestPending++;
if (message.Body.Motor.ToUpper().Contains("LEFT"))
{
_state.MotorLeft = message.Body.Speed;
}
else if (message.Body.Motor.ToUpper().Contains("RIGHT"))
{
_state.MotorRight = message.Body.Speed;
}
else
{
LogError("Motor name not set properly");
}
_scribblerComm.SendCommand(new ScribblerCommand((byte)ScribblerHelper.Commands.SET_MOTORS, (byte)_state.MotorRight, (byte)_state.MotorLeft));
yield return(Arbiter.Receive <ScribblerCommand>(false, _scribblerComPort,
delegate(ScribblerCommand response)
{
//if (response.Data[0] != (byte)ScribblerHelper.Commands.SET_MOTORS)
// LogError("SetMotor picked up a wrong echo");
//initialize notify list
List <string> notify = new List <string>();
notify.Add("MOTORS");
// 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);
//reply to say we are done
message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
RequestPending--;
}
));
yield break;
}
/// <summary>
/// Handles incoming set motor messages
/// </summary>
private IEnumerator <ITask> SetMotorHandler(SetMotors message)
{
if (!_state.Connected)
{
message.ResponsePort.Post(new Fault());
yield break;
}
//debug
if (message.Body.LeftSpeed < 0 || message.Body.LeftSpeed > 200 || message.Body.RightSpeed < 0 || message.Body.RightSpeed > 200)
{
LogError("Scribbler SetMotorHandler: target power set incorrect");
}
//update state
_state.MotorLeft = message.Body.LeftSpeed;
_state.MotorRight = message.Body.RightSpeed;
//send command
ScribblerCommand cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_MOTORS, (byte)_state.MotorRight, (byte)_state.MotorLeft);
SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);
_scribblerComPort.Post(sendcmd);
yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
delegate(ScribblerResponse response)
{
//initialize notify list
List <string> notify = new List <string>();
notify.Add("MOTORS");
// 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);
//reply to say we are done
message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
));
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;
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"></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>
/// 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>
/// Handles incoming set motor messages
/// </summary>
private IEnumerator<ITask> SetMotorHandler(SetMotor message)
{
if (message.Body == null)
{
message.ResponsePort.Post(new Fault());
yield break;
}
if (message.Body.Motor == null)
{
message.ResponsePort.Post(new Fault());
yield break;
}
// Requests come too fast, so dump ones that come in too fast.
if (RequestPending > 0 && message.Body.Speed != 100)
{
message.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
RequestPending++;
if (message.Body.Motor.ToUpper().Contains("LEFT"))
{
_state.MotorLeft = message.Body.Speed;
}
else if (message.Body.Motor.ToUpper().Contains("RIGHT"))
{
_state.MotorRight = message.Body.Speed;
}
else
{
LogError("Motor name not set properly");
}
ScribblerCommand cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_MOTORS, (byte)_state.MotorRight, (byte)_state.MotorLeft);
SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);
_scribblerComPort.Post(sendcmd);
yield return Arbiter.Receive<ScribblerResponse>(false, sendcmd.ResponsePort,
delegate(ScribblerResponse response)
{
//if (response.Data[0] != (byte)ScribblerHelper.Commands.SET_MOTORS)
// LogError("SetMotor picked up a wrong echo");
//initialize notify list
List<string> notify = new List<string>();
notify.Add("MOTORS");
// 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);
//reply to say we are done
message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
RequestPending--;
}
);
yield break;
}
/// <summary>
/// Handles incoming set motor messages
/// </summary>
private IEnumerator<ITask> SetMotorHandler(SetMotors message)
{
if (!_state.Connected)
{
message.ResponsePort.Post(new Fault());
yield break;
}
//debug
if (message.Body.LeftSpeed < 0 || message.Body.LeftSpeed > 200 || message.Body.RightSpeed < 0 || message.Body.RightSpeed > 200)
{
//LogError("Scribbler SetMotorHandler: target power set incorrect");
message.ResponsePort.Post(RSUtils.FaultOfException(new ArgumentOutOfRangeException("Motor speed", "Motor speed out of range")));
yield break;
}
//update state
_state.MotorLeft = message.Body.LeftSpeed;
_state.MotorRight = message.Body.RightSpeed;
//send command
ScribblerCommand cmd = new ScribblerCommand(ScribblerHelper.Commands.SET_MOTORS, (byte)_state.MotorRight, (byte)_state.MotorLeft);
SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);
_scribblerComPort.Post(sendcmd);
yield return Arbiter.Receive<ScribblerResponse>(false, sendcmd.ResponsePort,
delegate(ScribblerResponse response)
{
//initialize notify list
List<string> notify = new List<string>();
notify.Add("MOTORS");
// 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);
//reply to say we are done
message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
);
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;
}
