/// <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> /// 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); }
/// <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((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> 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((byte) (message.Body.IsLoud ? ScribblerHelper.Commands.SET_LOUD : ScribblerHelper.Commands.SET_QUIET)); SendScribblerCommand sendcmd = new SendScribblerCommand(cmd); _scribblerComPort.Post(sendcmd); yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort, delegate(ScribblerResponse response) { message.ResponsePort.Post(DefaultUpdateResponseType.Instance); })); }
/// <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 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> /// DO NOT USE THIS COMMAND DIRECTLY. /// In Scribbler.cs, post a message to _scribblerComPort /// </summary> /// <param name="cmd"></param> /// <returns></returns> internal ScribblerResponse SendCommand(ScribblerCommand cmd) { ScribblerResponse echo = null; ScribblerResponse response = null; byte[] buffer = new byte[outMessageSize]; if (buffer != null) { int ix = 0; //buffer = cmd.ToByteArray(); buffer[ix++] = cmd.CommandType; if (cmd.Data != null && cmd.Data.Length > 0) { foreach (byte b in cmd.Data) { buffer[ix++] = b; } } //fill to standard size while (ix < outMessageSize) { buffer[ix++] = 0; } #if DEBUG Console.Write("\nSent: "); foreach (byte b in buffer) { if (b != 0) { Console.Write(b + " "); } else { Console.Write("` "); } } Console.Write("\n"); #endif try { // When requesting a response, clear the inbound buffer if (_serialPort.BytesToRead > 0) { _serialPort.DiscardInBuffer(); } _serialPort.Write(buffer, 0, ix); } catch { Console.WriteLine("Serial Port Timeout. Lost connection with Scribbler."); //throw new IOException(); } echo = GetEcho(buffer); response = GetCommandResponse(helper.ReturnSize((ScribblerHelper.Commands)cmd.CommandType)); } return(response); }
/// <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(ScribblerHelper.Commands.GET_ALL); SendScribblerCommand sendcmd = new SendScribblerCommand(cmd); _scribblerComPort.Post(sendcmd); }
/// <summary> /// DO NOT USE THIS COMMAND DIRECTLY. /// In Scribbler.cs, post a message to _scribblerComPort /// </summary> /// <param name="cmd"></param> /// <returns></returns> internal ScribblerResponse SendCommand(ScribblerCommand cmd) { ScribblerResponse echo = null; ScribblerResponse response = null; int outMessageSize = cmd.Data.Length + 1; byte[] buffer = new byte[outMessageSize]; if (buffer != null) { int ix = 0; //buffer = cmd.ToByteArray(); buffer[ix++] = cmd.CommandType; // Changed this so it doesn't copy entire command (Fluke commands are shorter than 8 bytes) int len = Math.Min(cmd.Data.Length, (outMessageSize - 1)); if (cmd.Data != null && cmd.Data.Length > 0) { Array.Copy(cmd.Data, 0, buffer, 1, len); } ix += len; //foreach (byte b in cmd.Data) // buffer[ix++] = b; //fill to standard size while (ix < outMessageSize) { buffer[ix++] = 0; } #if DEBUG Console.Write("\nSent: "); foreach (byte b in buffer) { if (b != 0) { Console.Write(b + " "); } else { Console.Write("` "); } } Console.Write("\n"); #endif //try //{ // When requesting a response, clear the inbound buffer //Console.WriteLine(_serialPort.BytesToRead + " bytes left over"); while (_serialPort.BytesToRead > 0) { _serialPort.DiscardInBuffer(); } //Console.WriteLine(((ScribblerHelper.Commands)cmd.CommandType).ToString()); //Console.WriteLine("Command: " + cmd.Data.Length); //Console.WriteLine("Response: " + cmd.ResponseLength); //Console.WriteLine("Echo: " + cmd.HasEcho); _serialPort.Write(buffer, 0, ix); //} //catch //{ //Console.WriteLine("Serial Port Timeout. Lost connection with Scribbler."); //throw new IOException(); //} if (cmd.HasEcho) { echo = GetEcho(buffer); } response = GetCommandResponse(cmd); } return(response); }
/// <summary> /// Read Serial Port for a number of bytes and put into ScribblerResponse /// </summary> /// <param name="nBytes">number of bytes to read (includes the command type byte)</param> /// <returns>ScribblerResponse</returns> private ScribblerResponse GetCommandResponse(ScribblerCommand cmd) { int nBytes = cmd.ResponseLength; bool cmdEcho = cmd.HasEcho; //Console.WriteLine("GetCommandResponse: creating buffer"); byte[] inBuff = new Byte[Math.Abs(nBytes)]; //Console.WriteLine("Check 1"); ScribblerResponse response = null; //Console.WriteLine("Check 2"); int read = 0; bool error = false, done = false; // Set the default EOM character to a linefeed if (cmd.EndMarker1 == null) { cmd.EndMarker1 = 0x0A; cmd.EndMarker2 = null; } while (read < Math.Abs(nBytes) && !done) { int canread; int count = 0; while (((canread = _serialPort.BytesToRead) == 0) && count++ < 100) { Thread.Sleep(20); //spin } if (canread == 0) { break; } //Console.WriteLine("Spun for " + (count*10) + " ms, got chunk of " + canread); if (nBytes < 0) { //if (cmd.CommandType == (byte)ScribblerHelper.Commands.GET_JPEG_COLOR_SCAN) //{ // Console.WriteLine("JPEG"); //} //Console.WriteLine("Reading variable length (buffer size " + inBuff.Length + ")"); for (int i = 0; i < canread; i++) { _serialPort.Read(inBuff, read++, 1); // NOTE: This is a hack to get the header length from the first // two bytes of the return from GET_JPEG_HEADER. if (read == 2 && (cmd.CommandType == (byte)ScribblerHelper.Commands.GET_JPEG_COLOR_HEADER || cmd.CommandType == (byte)ScribblerHelper.Commands.GET_JPEG_GRAY_HEADER)) { nBytes = (int)inBuff[0] + ((int)inBuff[1] << 8) + 2; //Console.WriteLine("Looking for JPEG header of " + nBytes + " bytes"); } else if (nBytes < 0) // Have to check this again because of the JPEG hack, changed nBytes { //Console.WriteLine(" Got " + inBuff[read - 1] + " at " + (read - 1)); if (cmd.EndMarker2 == null) { if (inBuff[read - 1] == cmd.EndMarker1) { done = true; } } else if (read >= 2) { if (inBuff[read - 2] == cmd.EndMarker1 && inBuff[read - 1] == cmd.EndMarker2) { done = true; } } } } } else { int needtoread = nBytes - read; //Console.WriteLine("Reading fixed length of " + needtoread + ", buffer " + nBytes); if (canread > needtoread) { _serialPort.Read(inBuff, read, needtoread); read += needtoread; } else { _serialPort.Read(inBuff, read, canread); read += canread; } } } //Console.WriteLine("GetCommandResponse: " + _serialPort.BytesToRead + " left over"); if (read < nBytes) { throw new ScribblerProtocolException("Command response of " + read + " was too short: " + (ScribblerHelper.Commands)cmd.CommandType); } int dataBytes = (cmdEcho ? read - 1 : read); response = new ScribblerResponse(Math.Max(0, dataBytes)); response.CommandType = (cmdEcho ? inBuff[inBuff.Length - 1] : (byte)0); Array.Copy(inBuff, response.Data, dataBytes); #if DEBUG Console.Write("Got: "); foreach (byte b in response.Data) { if (b != 0) { Console.Write(b + " "); } else { Console.Write("` "); } } Console.Write("\n"); #endif return(response); }
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; }
private PortSet<ScribblerResponse, Fault> _scribblerComPortPost(ScribblerCommand cmd) { SendScribblerCommand sendcmd = new SendScribblerCommand(cmd); _scribblerComPort.Post(sendcmd); return sendcmd.ResponsePort; }
/// <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> 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(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 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; }
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; }
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 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(ScribblerHelper.Commands.SET_LED_LEFT_ON); else cmd = new ScribblerCommand(ScribblerHelper.Commands.SET_LED_LEFT_OFF); break; case 1: //center LED _state.LEDCenter = message.Body.State; if (message.Body.State) cmd = new ScribblerCommand(ScribblerHelper.Commands.SET_LED_CENTER_ON); else cmd = new ScribblerCommand(ScribblerHelper.Commands.SET_LED_CENTER_OFF); break; case 2: //right LED _state.LEDRight = message.Body.State; if (message.Body.State) cmd = new ScribblerCommand(ScribblerHelper.Commands.SET_LED_RIGHT_ON); else cmd = new ScribblerCommand(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(ScribblerHelper.Commands.SET_LED_ALL_ON); else cmd = new ScribblerCommand(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; }
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; }
/// <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; }
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; }
/// <summary> /// DO NOT USE THIS COMMAND DIRECTLY. /// In Scribbler.cs, post a message to _scribblerComPort /// </summary> /// <param name="cmd"></param> /// <returns></returns> internal ScribblerResponse SendCommand(ScribblerCommand cmd) { ScribblerResponse echo = null; ScribblerResponse response = null; int outMessageSize = cmd.Data.Length + 1; byte[] buffer = new byte[outMessageSize]; if (buffer != null) { int ix = 0; //buffer = cmd.ToByteArray(); buffer[ix++] = cmd.CommandType; // Changed this so it doesn't copy entire command (Fluke commands are shorter than 8 bytes) int len = Math.Min(cmd.Data.Length, (outMessageSize - 1)); if (cmd.Data != null && cmd.Data.Length > 0) Array.Copy(cmd.Data, 0, buffer, 1, len); ix += len; //foreach (byte b in cmd.Data) // buffer[ix++] = b; //fill to standard size while (ix < outMessageSize) buffer[ix++] = 0; #if DEBUG Console.Write("\nSent: "); foreach (byte b in buffer) { if (b != 0) Console.Write(b + " "); else Console.Write("` "); } Console.Write("\n"); #endif //try //{ // When requesting a response, clear the inbound buffer //Console.WriteLine(_serialPort.BytesToRead + " bytes left over"); while (_serialPort.BytesToRead > 0) _serialPort.DiscardInBuffer(); //Console.WriteLine(((ScribblerHelper.Commands)cmd.CommandType).ToString()); //Console.WriteLine("Command: " + cmd.Data.Length); //Console.WriteLine("Response: " + cmd.ResponseLength); //Console.WriteLine("Echo: " + cmd.HasEcho); _serialPort.Write(buffer, 0, ix); //} //catch //{ //Console.WriteLine("Serial Port Timeout. Lost connection with Scribbler."); //throw new IOException(); //} if (cmd.HasEcho) echo = GetEcho(buffer); response = GetCommandResponse(cmd); } return response; }
/// <summary> /// Read Serial Port for a number of bytes and put into ScribblerResponse /// </summary> /// <param name="nBytes">number of bytes to read (includes the command type byte)</param> /// <returns>ScribblerResponse</returns> private ScribblerResponse GetCommandResponse(ScribblerCommand cmd) { int nBytes = cmd.ResponseLength; bool cmdEcho = cmd.HasEcho; //Console.WriteLine("GetCommandResponse: creating buffer"); byte[] inBuff = new Byte[Math.Abs(nBytes)]; //Console.WriteLine("Check 1"); ScribblerResponse response = null; //Console.WriteLine("Check 2"); int read = 0; bool error = false, done = false; // Set the default EOM character to a linefeed if (cmd.EndMarker1 == null) { cmd.EndMarker1 = 0x0A; cmd.EndMarker2 = null; } while (read < Math.Abs(nBytes) && !done) { int canread; int count = 0; while (((canread = _serialPort.BytesToRead) == 0) && count++ < 100) Thread.Sleep(20); //spin if (canread == 0) break; //Console.WriteLine("Spun for " + (count*10) + " ms, got chunk of " + canread); if (nBytes < 0) { //if (cmd.CommandType == (byte)ScribblerHelper.Commands.GET_JPEG_COLOR_SCAN) //{ // Console.WriteLine("JPEG"); //} //Console.WriteLine("Reading variable length (buffer size " + inBuff.Length + ")"); for (int i = 0; i < canread; i++) { _serialPort.Read(inBuff, read++, 1); // NOTE: This is a hack to get the header length from the first // two bytes of the return from GET_JPEG_HEADER. if (read == 2 && (cmd.CommandType == (byte)ScribblerHelper.Commands.GET_JPEG_COLOR_HEADER || cmd.CommandType == (byte)ScribblerHelper.Commands.GET_JPEG_GRAY_HEADER)) { nBytes = (int)inBuff[0] + ((int)inBuff[1] << 8) + 2; //Console.WriteLine("Looking for JPEG header of " + nBytes + " bytes"); } else if (nBytes < 0) // Have to check this again because of the JPEG hack, changed nBytes { //Console.WriteLine(" Got " + inBuff[read - 1] + " at " + (read - 1)); if (cmd.EndMarker2 == null) { if (inBuff[read - 1] == cmd.EndMarker1) done = true; } else if (read >= 2) { if (inBuff[read - 2] == cmd.EndMarker1 && inBuff[read - 1] == cmd.EndMarker2) done = true; } } } } else { int needtoread = nBytes - read; //Console.WriteLine("Reading fixed length of " + needtoread + ", buffer " + nBytes); if (canread > needtoread) { _serialPort.Read(inBuff, read, needtoread); read += needtoread; } else { _serialPort.Read(inBuff, read, canread); read += canread; } } } //Console.WriteLine("GetCommandResponse: " + _serialPort.BytesToRead + " left over"); if (read < nBytes) throw new ScribblerProtocolException("Command response of " + read + " was too short: " + (ScribblerHelper.Commands)cmd.CommandType); int dataBytes = (cmdEcho ? read - 1 : read); response = new ScribblerResponse(Math.Max(0, dataBytes)); response.CommandType = (cmdEcho ? inBuff[inBuff.Length - 1] : (byte)0); Array.Copy(inBuff, response.Data, dataBytes); #if DEBUG Console.Write("Got: "); foreach (byte b in response.Data) { if (b != 0) Console.Write(b + " "); else Console.Write("` "); } Console.Write("\n"); #endif return response; }
public SendScribblerCommand(ScribblerCommand cmd) { this.Body = cmd; }
/// <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; }