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