/// <summary>
/// Send a command to the Scribbler and wait for a response.
/// </summary>
private IEnumerator <ITask> SendScribblerCommandHandler(SendScribblerCommand command)
{
// Send command to robot and wait for echo and response
ScribblerResponse validResponse = _scribblerCom.SendCommand(command.Body);
if (validResponse == null)
{
LogError(LogGroups.Console, "Send Scribbler Command null response");
command.ResponsePort.Post(new Fault());
}
else
{
//reset timer
PollTimer.Enabled = false;
PollTimer.Enabled = true;
//Update our state with the scribbler's response
//UpdateState(validResponse);
_mainPort.Post(new ScribblerResponseMessage(validResponse));
command.ResponsePort.Post(validResponse);
}
// Ready to process another command
Activate(Arbiter.ReceiveWithIterator <SendScribblerCommand>(false, _scribblerComPort, SendScribblerCommandHandler));
yield break;
}
/// <summary>
/// Handles incoming SetAllLEDs requests
/// </summary>
private IEnumerator <ITask> SetAllLEDsHandler(SetAllLEDs message)
{
//error check
if (!_state.Connected)
{
message.ResponsePort.Post(new Fault());
yield break;
}
//update state
_state.LEDLeft = message.Body.LeftLED;
_state.LEDCenter = message.Body.CenterLED;
_state.LEDRight = message.Body.RightLED;
//send command
ScribblerCommand cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_ALL,
_state.LEDLeft,
_state.LEDCenter,
_state.LEDRight);
SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);
_scribblerComPort.Post(sendcmd);
yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
delegate(ScribblerResponse response)
{
}
));
//reply to sender
message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
/// <summary>
/// Handles incoming play tone requests
/// </summary>
private IEnumerator <ITask> PlayToneHandler(PlayTone message)
{
if (!_state.Connected)
{
LogError("trying to play tone, but not connected");
message.ResponsePort.Post(new Fault());
yield break;
}
ScribblerCommand cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_SPEAKER_2,
message.Body.Duration,
message.Body.Frequency1,
message.Body.Frequency2);
SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);
_scribblerComPort.Post(sendcmd);
yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
delegate(ScribblerResponse response)
{
//reply to sender
message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
));
yield break;
}
/// <summary>
/// This will poll the scribbler at a minimum frequency
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void PollTimer_Elapsed(object sender, EventArgs e)
{
ScribblerCommand cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.GET_ALL);
SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);
_scribblerComPort.Post(sendcmd);
}
//private IEnumerator<ITask> ConfigureSensorHandler(ConfigureSensor command)
//{
// if (command.Body == null)
// {
// command.ResponsePort.Post(new Fault());
// yield break;
// }
// if (command.Body.Sensor == null || command.Body.Configuration == null)
// {
// command.ResponsePort.Post(new Fault());
// yield break;
// }
// switch (command.Body.Sensor.ToUpper())
// {
// case "LEFT":
// _state.LightLeftConfig = command.Body.Configuration;
// break;
// case "CENTER":
// _state.LightCenterConfig = command.Body.Configuration;
// break;
// case "RIGHT":
// _state.LightRightConfig = command.Body.Configuration;
// break;
// }
// command.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// yield break;
//}
/// <summary>
/// Send a command to the Scribbler and wait for a response.
/// </summary>
/// <param name="ready"></param>
/// <param name="legoCommand"></param>
/// <returns></returns>
private IEnumerator <ITask> SendScribblerCommandHandler(SendScribblerCommand command)
{
// Send command to robot and wait for echo and response
ScribblerResponse validResponse = _scribblerCom.SendCommand(command.Body);
if (validResponse == null)
{
LogError(LogGroups.Console, "Send Scribbler Command null response");
command.ResponsePort.Post(new Fault());
}
//else if (validResponse.GetType() == typeof(LegoResponseException))
//{
// // Pull exception text from response
// string errorMessage = "LEGO command: " + command.Body.LegoCommandCode.ToString() + " response generated an error: " + ((LegoResponseException)validResponse).ErrorMessage;
// command.ResponsePort.Post(new SoapFaultContext(new InvalidOperationException(errorMessage)).SoapFault);
//}
else
{
// Check to see if we need to update state
// based on the response received from LEGO.
// PortSet<DefaultUpdateResponseType, Fault> responsePort = UpdateCurrentState(validResponse);
// if (responsePort != null)
// {
// yield return Arbiter.Choice(responsePort,
// delegate(DefaultUpdateResponseType response) { },
// delegate(Fault fault)
// {
// LogError(LogGroups.Console, "Failed to update LEGO NXT service state", fault);
// });
// }
// PostCommandProcessing(validResponse);
//reset timer
PollTimer.Enabled = false;
PollTimer.Enabled = true;
//Update our state with the scribbler's response
UpdateState(validResponse);
command.ResponsePort.Post(validResponse);
}
// Ready to process another command
Activate(Arbiter.ReceiveWithIterator <SendScribblerCommand>(false, _scribblerComPort, SendScribblerCommandHandler));
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;
}
private IEnumerator <ITask> SetNameHandler(SetName command)
{
if (string.IsNullOrEmpty(command.Body.NewName))
{
LogError("New name is null");
command.ResponsePort.Post(new Fault());
yield break;
}
if (!_state.Connected)
{
command.ResponsePort.Post(new Fault());
yield break;
}
string shortenedname;
if (command.Body.NewName.Length > 8)
{
shortenedname = command.Body.NewName.Substring(0, 8);
}
else
{
shortenedname = command.Body.NewName;
}
_state.RobotName = shortenedname;
ScribblerCommand cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_NAME, shortenedname);
SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);
_scribblerComPort.Post(sendcmd);
yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
delegate(ScribblerResponse response)
{
SaveState(_state);
}
));
//reply to sender
command.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
/// <summary>
/// Handles incoming play tone requests
/// </summary>
private IEnumerator <ITask> PlayToneHandler(PlayTone message)
{
ScribblerCommand cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_SPEAKER_2,
message.Body.Duration,
message.Body.Frequency1,
message.Body.Frequency2);
SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);
_scribblerComPort.Post(sendcmd);
yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
delegate(ScribblerResponse response)
{
}
));
//reply to sender
message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
public IEnumerator <ITask> HttpPostHandler(HttpPost httpPost)
{
// Use helper to read form data
ReadFormData readForm = new ReadFormData(httpPost.Body.Context);
_httpUtilities.Post(readForm);
// Wait for result
Activate(Arbiter.Choice(readForm.ResultPort,
delegate(NameValueCollection parameters)
{
if (!string.IsNullOrEmpty(parameters["Action"]) &&
parameters["Action"] == "ScribblerConfig")
{
if (parameters["buttonOk"] == "Change" && _state.Connected)
{
SetNameBody newname = new SetNameBody(parameters["Name"]);
SetName newnamemessage = new SetName(newname);
_mainPort.Post(newnamemessage);
Activate(
Arbiter.Choice(
Arbiter.Receive <DefaultUpdateResponseType>(false, newnamemessage.ResponsePort,
delegate(DefaultUpdateResponseType response)
{
HttpPostSuccess(httpPost);
}),
Arbiter.Receive <Fault>(false, newnamemessage.ResponsePort,
delegate(Fault f)
{
HttpPostFailure(httpPost, f.Reason[0].Value);
})
)
);
}
else if (parameters["buttonOk"] == "Connect" && _state.Connected)
{
//close down this connection to make a new connection below
PollTimer.Close();
System.Threading.Thread.Sleep(100);
_scribblerCom.Close();
_state.Connected = false;
}
if (parameters["buttonOk"] == "Connect" && !_state.Connected)
{
int port = 0;
int.TryParse(parameters["ComPort"], out port);
string name = parameters["Name"];
if (!string.IsNullOrEmpty(name) && name.Length > 8)
{
name = name.Substring(0, 8);
}
_state.ComPort = port;
_state.RobotName = name;
//open Scribbler Communications port
if (ConnectToScribbler())
{
// Listen for a single Serial port request with an acknowledgement
Activate(Arbiter.ReceiveWithIterator <SendScribblerCommand>(false, _scribblerComPort, SendScribblerCommandHandler));
PollTimer = new System.Timers.Timer();
PollTimer.Interval = TimerDelay;
PollTimer.AutoReset = true;
PollTimer.Elapsed += new System.Timers.ElapsedEventHandler(PollTimer_Elapsed);
PollTimer.Start();
//play startup tone
PlayToneBody startTone = new PlayToneBody(200, 1000, 2000);
PlayTone playToneMessage = new PlayTone(startTone);
_mainPort.Post(playToneMessage);
Activate(
Arbiter.Choice(
Arbiter.Receive <DefaultUpdateResponseType>(false, playToneMessage.ResponsePort,
delegate(DefaultUpdateResponseType response)
{
HttpPostSuccess(httpPost);
}),
Arbiter.Receive <Fault>(false, playToneMessage.ResponsePort,
delegate(Fault f)
{
HttpPostFailure(httpPost, f.Reason[0].Value);
})
)
);
}
else
{
HttpPostFailure(httpPost, "Connection to Scribbler failed");
}
}
}
else if (!string.IsNullOrEmpty(parameters["Action"]) &&
parameters["Action"] == "ScribblerSensors")
{
if (parameters["buttonOk"] == "Poll" && _state.Connected)
{
ScribblerCommand cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.GET_ALL);
SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);
_scribblerComPort.Post(sendcmd);
Activate(
Arbiter.Choice(
Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
delegate(ScribblerResponse response)
{
HttpPostSuccess(httpPost);
}),
Arbiter.Receive <Fault>(false, sendcmd.ResponsePort,
delegate(Fault f)
{
HttpPostFailure(httpPost, f.Reason[0].Value);
})
)
);
}
}
else if (!string.IsNullOrEmpty(parameters["Action"]) &&
parameters["Action"] == "ScribblerMotors")
{
if (parameters["buttonOk"] == "Set" && _state.Connected)
{
int left = _state.MotorLeft;
int right = _state.MotorRight;
int.TryParse(parameters["LeftMotor"], out left);
int.TryParse(parameters["RightMotor"], out right);
SetMotorsBody setMotorsBody = new SetMotorsBody(left, right);
SetMotors setMotorsRequest = new SetMotors(setMotorsBody);
_mainPort.Post(setMotorsRequest);
Activate(
Arbiter.Choice(
Arbiter.Receive <DefaultUpdateResponseType>(false, setMotorsRequest.ResponsePort,
delegate(DefaultUpdateResponseType response)
{
HttpPostSuccess(httpPost);
}),
Arbiter.Receive <Fault>(false, setMotorsRequest.ResponsePort,
delegate(Fault f)
{
HttpPostFailure(httpPost, f.Reason[0].Value);
})
)
);
}
else if (parameters["buttonOk"] == "All Stop" && _state.Connected)
{
SetMotorsBody setMotorsBody = new SetMotorsBody(100, 100);
SetMotors setMotorsRequest = new SetMotors(setMotorsBody);
_mainPort.Post(setMotorsRequest);
Activate(
Arbiter.Choice(
Arbiter.Receive <DefaultUpdateResponseType>(false, setMotorsRequest.ResponsePort,
delegate(DefaultUpdateResponseType response)
{
HttpPostSuccess(httpPost);
}),
Arbiter.Receive <Fault>(false, setMotorsRequest.ResponsePort,
delegate(Fault f)
{
HttpPostFailure(httpPost, f.Reason[0].Value);
})
)
);
}
}
else if (!string.IsNullOrEmpty(parameters["Action"]) &&
parameters["Action"] == "ScribblerLEDs")
{
if (parameters["buttonOk"] == "Set" && _state.Connected)
{
bool left = ((parameters["LeftLED"] ?? "off") == "on");
bool center = ((parameters["CenterLED"] ?? "off") == "on");
bool right = ((parameters["RightLED"] ?? "off") == "on");
SetAllLedsBody leds = new SetAllLedsBody(left, center, right);
SetAllLEDs setAllLeds = new SetAllLEDs(leds);
_mainPort.Post(setAllLeds);
Activate(
Arbiter.Choice(
Arbiter.Receive <DefaultUpdateResponseType>(false, setAllLeds.ResponsePort,
delegate(DefaultUpdateResponseType response)
{
HttpPostSuccess(httpPost);
}),
Arbiter.Receive <Fault>(false, setAllLeds.ResponsePort,
delegate(Fault f)
{
HttpPostFailure(httpPost, f.Reason[0].Value);
})
)
);
}
}
else if (!string.IsNullOrEmpty(parameters["Action"]) &&
parameters["Action"] == "ScribblerSpeaker")
{
if (parameters["buttonOk"] == "Play" && _state.Connected)
{
int tone1 = 0;
int tone2 = 0;
int duration = 0;
int.TryParse(parameters["Tone1"], out tone1);
int.TryParse(parameters["Tone2"], out tone2);
int.TryParse(parameters["Duration"], out duration);
PlayToneBody playTone = new PlayToneBody(duration, tone1, tone2);
PlayTone playToneMessage = new PlayTone(playTone);
_mainPort.Post(playToneMessage);
Activate(
Arbiter.Choice(
Arbiter.Receive <DefaultUpdateResponseType>(false, playToneMessage.ResponsePort,
delegate(DefaultUpdateResponseType response)
{
HttpPostSuccess(httpPost);
}),
Arbiter.Receive <Fault>(false, playToneMessage.ResponsePort,
delegate(Fault f)
{
HttpPostFailure(httpPost, f.Reason[0].Value);
})
)
);
}
}
else
{
HttpPostFailure(httpPost, "Unknown Http Post");
}
},
delegate(Exception Failure)
{
LogError(Failure.Message);
})
);
yield break;
}
/// <summary>
/// Handles incoming SetLED requests
/// </summary>
private IEnumerator <ITask> SetLEDHandler(SetLED message)
{
if (!_state.Connected)
{
LogError("Trying to set LED, but not connected");
message.ResponsePort.Post(new Fault());
yield break;
}
ScribblerCommand cmd;
switch (message.Body.LED)
{
case 0: //left LED
_state.LEDLeft = message.Body.State;
if (message.Body.State)
{
cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_LEFT_ON);
}
else
{
cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_LEFT_OFF);
}
break;
case 1: //center LED
_state.LEDCenter = message.Body.State;
if (message.Body.State)
{
cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_CENTER_ON);
}
else
{
cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_CENTER_OFF);
}
break;
case 2: //right LED
_state.LEDRight = message.Body.State;
if (message.Body.State)
{
cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_RIGHT_ON);
}
else
{
cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_RIGHT_OFF);
}
break;
case 3: //all LEDs
_state.LEDLeft = message.Body.State;
_state.LEDCenter = message.Body.State;
_state.LEDRight = message.Body.State;
if (message.Body.State)
{
cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_ALL_ON);
}
else
{
cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_ALL_OFF);
}
break;
default:
LogError("LED number set incorrect");
cmd = new ScribblerCommand();
break;
}
SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);
_scribblerComPort.Post(sendcmd);
yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
delegate(ScribblerResponse response)
{
}
));
//reply to sender
message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
/// <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;
}