//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 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; }
/// <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> SetNameHandler(SetName command) { if (command.Body == null || string.IsNullOrEmpty(command.Body.NewName)) { 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 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; }
public IEnumerator<ITask> GetObstacleHandler(GetObstacle get) { if (!_state.Connected) { get.ResponsePort.Post(new Fault() { Reason = new ReasonText[] { new ReasonText() { Value = "Not connected" } } }); yield break; } ScribblerCommand cmd; switch (get.Body.Value) { case 0: cmd = new ScribblerCommand(ScribblerHelper.Commands.GET_DONGLE_L_IR); break; case 1: cmd = new ScribblerCommand(ScribblerHelper.Commands.GET_DONGLE_C_IR); break; case 2: cmd = new ScribblerCommand(ScribblerHelper.Commands.GET_DONGLE_R_IR); break; default: get.ResponsePort.Post(RSUtils.FaultOfException( new ArgumentOutOfRangeException("DONGLE_IR", get.Body, "Dongle IR sensor must be 0, 1, or 2"))); yield break; break; } SendScribblerCommand sendcmd = new SendScribblerCommand(cmd); _scribblerComPort.Post(sendcmd); yield return Arbiter.Choice(sendcmd.ResponsePort, delegate(ScribblerResponse r) { try { get.ResponsePort.Post(new UInt16Body(ScribblerHelper.GetShort(r.Data, 0))); } catch (Exception e) { get.ResponsePort.Post(RSUtils.FaultOfException(e)); } }, delegate(Fault f) { get.ResponsePort.Post(f); }); yield break; }
public IEnumerator<ITask> SetLEDBackHandler(SetLEDBack set) { if (!_state.Connected) { LogError("Trying to set LED, but not connected"); set.ResponsePort.Post(new Fault()); yield break; } ScribblerCommand cmd = new ScribblerCommand(ScribblerHelper.Commands.SET_DIMMER_LED, set.Body.BackLED); SendScribblerCommand sendcmd = new SendScribblerCommand(cmd); _scribblerComPort.Post(sendcmd); _state.LEDBack = set.Body.BackLED; yield return Arbiter.Choice(sendcmd.ResponsePort, delegate(ScribblerResponse r) { set.ResponsePort.Post(DefaultUpdateResponseType.Instance); }, delegate(Fault f) { set.ResponsePort.Post(f); }); yield break; }
private PortSet<ScribblerResponse, Fault> _scribblerComPortPost(ScribblerCommand cmd) { SendScribblerCommand sendcmd = new SendScribblerCommand(cmd); _scribblerComPort.Post(sendcmd); return sendcmd.ResponsePort; }
public IEnumerator<ITask> SetLoudHandler(SetLoud message) { if (!_state.Connected) { LogError("trying to set loudness, but not connected"); message.ResponsePort.Post(new Fault()); yield break; } ScribblerCommand cmd = new ScribblerCommand( (message.Body.IsLoud ? ScribblerHelper.Commands.SET_LOUD : ScribblerHelper.Commands.SET_QUIET)); SendScribblerCommand sendcmd = new SendScribblerCommand(cmd); _scribblerComPort.Post(sendcmd); yield return Arbiter.Choice(sendcmd.ResponsePort, delegate(ScribblerResponse f) { message.ResponsePort.Post(DefaultUpdateResponseType.Instance); }, delegate(Fault f) { message.ResponsePort.Post(f); }); yield break; }
public IEnumerator<ITask> SetLEDFrontHandler(SetLEDFront set) { if (!_state.Connected) { LogError("Trying to set LED, but not connected"); set.ResponsePort.Post(new Fault()); yield break; } ScribblerCommand cmd; if (set.Body.FrontLED == true) cmd = new ScribblerCommand(ScribblerHelper.Commands.SET_DONGLE_LED_ON); else cmd = new ScribblerCommand(ScribblerHelper.Commands.SET_DONGLE_LED_OFF); SendScribblerCommand sendcmd = new SendScribblerCommand(cmd); _scribblerComPort.Post(sendcmd); _state.LEDFront = set.Body.FrontLED; yield return Arbiter.Choice(sendcmd.ResponsePort, delegate(ScribblerResponse r) { set.ResponsePort.Post(DefaultUpdateResponseType.Instance); }, delegate(Fault f) { set.ResponsePort.Post(f); }); yield break; }
/// <summary> /// Handles incoming play tone requests /// </summary> //[ServiceHandler(ServiceHandlerBehavior.Exclusive)] public 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(ScribblerHelper.Commands.SET_SPEAKER_2, (int)(message.Body.Duration * 1000.0), message.Body.Frequency1, message.Body.Frequency2); SendScribblerCommand sendcmd = new SendScribblerCommand(cmd); _scribblerComPort.Post(sendcmd); yield return Arbiter.Choice(sendcmd.ResponsePort, delegate(ScribblerResponse response) { //reply to sender message.ResponsePort.Post(DefaultUpdateResponseType.Instance); }, delegate(Fault f) { message.ResponsePort.Post(f); } ); yield break; }
private IEnumerator<ITask> SendScribblerCommandHandlerExternal(SendScribblerCommand command) { _scribblerComPort.Post(command); yield break; }
/// <summary> /// Send a command to the Scribbler and wait for a response. /// </summary> //[ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_scribblerComPort")] private IEnumerator<ITask> SendScribblerCommandHandler(SendScribblerCommand command) { // Send command to robot and wait for echo and response ScribblerResponse validResponse; try { validResponse = _scribblerCom.SendCommand(command.Body); if (validResponse == null) throw new Exception("Send Scribbler Command null response"); } catch (TimeoutException e) { Console.WriteLine("Serial port timeout"); command.ResponsePort.Post(RSUtils.FaultOfException(e)); validResponse = null; } catch (Exception e) { Console.WriteLine("SendCommand exception: " + e.ToString()); command.ResponsePort.Post(RSUtils.FaultOfException(e)); validResponse = null; } if (validResponse != null) { //reset timer PollTimer.Enabled = false; PollTimer.Enabled = true; //Update our state with the scribbler's response //UpdateState(validResponse); _mainPort.PostUnknownType(new ScribblerResponseMessage(validResponse)); command.ResponsePort.Post(validResponse); } //Console.WriteLine("Finished command; " + command.Body.CommandType); // Ready to process another command Activate(Arbiter.ReceiveWithIterator<SendScribblerCommand>(false, _scribblerComPort, SendScribblerCommandHandler)); yield break; }
/// <summary> /// Handles incoming SetLED requests /// </summary> private IEnumerator<ITask> SetLEDHandler(SetLED message) { ScribblerCommand cmd; switch (message.Body.LED) { case 0: //left LED 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 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 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 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 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(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 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> /// Http Post Handler. Handles http form inputs /// </summary> //[ServiceHandler(ServiceHandlerBehavior.Concurrent)] public IEnumerator<ITask> HttpPostHandler(HttpPost httpPost) { // Use helper to read form data ReadFormData readForm = new ReadFormData(httpPost); _httpUtilities.Post(readForm); // Read form data NameValueCollection parameters = null; yield return Arbiter.Choice(readForm.ResultPort, delegate(NameValueCollection p) { parameters = p; }, delegate(Exception e) { throw new Exception("Error reading form data", e); }); // Act on form data 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.PostUnknownType(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; //HttpPostSuccess(httpPost); } 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 LogInfo("connecting to scribbler..."); Reconnect rec = new Reconnect(); _mainPort.PostUnknownType(rec); yield return Arbiter.Choice(rec.ResponsePort, delegate(DefaultUpdateResponseType r) { LogInfo("connected, sending http reply"); HttpPostSuccess(httpPost); LogInfo("http reply sent"); }, delegate(Fault f) { httpPost.ResponsePort.Post(f); }); } } else if (!string.IsNullOrEmpty(parameters["Action"]) && parameters["Action"] == "ScribblerSensors") { if (parameters["buttonOk"] == "Poll" && _state.Connected) { ScribblerCommand cmd = new ScribblerCommand(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.PostUnknownType(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.PostUnknownType(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.PostUnknownType(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.PostUnknownType(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"); } yield break; }