/// <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; }