public void SetMotors(double leftPower, double rightPower)
{
if (!motorsOn)
{
EnableMotors();
}
//drive.SetDrivePowerRequest drivePowerReq = new drive.SetDrivePowerRequest();
//drivePowerReq.LeftWheelPower = leftPower;
//drivePowerReq.RightWheelPower = rightPower;
//drive.SetDrivePower setDrivePower = new drive.SetDrivePower(drivePowerReq);
//drivePort.Post(setDrivePower);
//ManualResetEvent signal = new ManualResetEvent(false);
//Arbiter.Activate(DssEnvironment.TaskQueue,
// Arbiter.Choice<DefaultUpdateResponseType, Fault>(
// drivePort.SetDrivePower((double)leftPower, (double)rightPower),
// delegate(DefaultUpdateResponseType state)
// {
// signal.Set();
// },
// delegate(Fault failure)
// {
// Console.WriteLine("*** Fault in SetMotors: ");
// foreach (var r in failure.Reason)
// Console.WriteLine("*** " + r.Value);
// signal.Set();
// }));
//signal.WaitOne();
RSUtils.ReceiveSync(taskQueue, drivePort.SetDrivePower(leftPower, rightPower), Myro.Utilities.Params.DefaultRecieveTimeout);
}
/// <summary>
/// Set the motor power.
/// </summary>
/// <param name="leftPower"></param>
/// <param name="rightPower"></param>
public void SetMotors(double leftPower, double rightPower)
{
if (!motorsOn)
{
EnableMotors();
}
RSUtils.ReceiveSync(taskQueue, drivePort.SetDrivePower(leftPower, rightPower), Myro.Utilities.Params.DefaultRecieveTimeout);
}
/// <summary>
/// Set a single element in the vector by name.
/// </summary>
/// <param name="tag"></param>
/// <param name="value"></param>
public void Set(string key, double value)
{
RSUtils.ReceiveSync <DefaultUpdateResponseType>(taskQueue, opPort.SetByKey(new List <string>()
{
key
}, new List <double>()
{
value
}, DateTime.Now), Myro.Utilities.Params.DefaultRecieveTimeout);
}
/// <summary>
/// Set a single element in the vector by index.
/// </summary>
/// <param name="index"></param>
/// <param name="value"></param>
public void Set(int index, double value)
{
RSUtils.ReceiveSync <DefaultUpdateResponseType>(taskQueue, opPort.SetByIndex(new List <int>()
{
index
}, new List <double>()
{
value
}, DateTime.Now), Myro.Utilities.Params.DefaultRecieveTimeout);
}
/// <summary>
/// Set a group of elements in the vector by name.
/// </summary>
/// <param name="index"></param>
/// <param name="value"></param>
public void Set(List <string> keys, List <double> values)
{
//try
//{
RSUtils.ReceiveSync <DefaultUpdateResponseType>(taskQueue, opPort.SetByKey(keys, values, DateTime.Now), Myro.Utilities.Params.defaultRecieveTimeout);
//}
//catch (KeyNotFoundException)
//{
// throw new AdapterArgumentException(Strings.KeyNotFound());
//}
}
protected virtual void Initialize()
{
// Initialize the port and subscribe to the service to know when the motors are enabled
motorsOn = false;
drivePort = DssEnvironment.ServiceForwarder <drive.DriveOperations>(new Uri(ServiceInfo.Service));
drive.DriveOperations driveNotificationPort = new drive.DriveOperations();
RSUtils.ReceiveSync(taskQueue, drivePort.Subscribe(driveNotificationPort), Params.DefaultRecieveTimeout);
// Set up notifications
Arbiter.Activate(DssEnvironment.TaskQueue,
Arbiter.Receive <drive.Update>(true, driveNotificationPort, NotifyDriveUpdate));
}
/// <summary>
/// Retrieve a single element from the state vector, with full safety
/// checks. Throws AdapterArgumentException and
/// AdapterOperationException.
/// </summary>
/// <param name="index"></param>
/// <returns></returns>
public double Get(int index)
{
//try
//{
var resp = RSUtils.ReceiveSync <vector.GetElementResponseType>(taskQueue, opPort.GetByIndex(index), Myro.Utilities.Params.defaultRecieveTimeout);
return(resp.Value);
//}
//catch (ArgumentOutOfRangeException)
//{
// throw new AdapterArgumentException(Strings.IndexOutOfBounds(index));
//}
}
/// <summary>
/// Retrieve a single element from the state vetor, by name, with full
/// safety checks. Throws AdapterArgumentException and
/// AdapterOperationException.
/// </summary>
/// <param name="tag"></param>
/// <returns></returns>
public double Get(string key)
{
//try
//{
var resp = RSUtils.ReceiveSync <vector.GetElementResponseType>(taskQueue, opPort.GetByKey(key), Myro.Utilities.Params.defaultRecieveTimeout);
return(resp.Value);
//}
//catch (KeyNotFoundException)
//{
// throw new AdapterArgumentException(Strings.KeyNotFound(key));
//}
}
public void QueryFrame(MyroImageType type, out int Width, out int Height, out byte[] Image)
{
var r = RSUtils.ReceiveSync(queue,
opPort.QueryFrame(new webcam.QueryFrameRequest()
{
Format = type.Guid
}),
Params.DefaultRecieveTimeout);
Width = r.Size.Width;
Height = r.Size.Height;
Image = r.Frame;
}
/// <summary>
/// "Darken" the camera, so that it only sees bright lights. This
/// turns off automatic exposure, gain, and white balance.
/// </summary>
/// <param name="level"></param>
public void DarkenCamera(byte level)
{
RSUtils.ReceiveSync(queue, opPort.SetCamera(new fluke.CamControlState()
{
Darkness = level,
Val1 = 0,
Val2 = 0,
Brightness = 0,
Exposure = 0,
AutoExposure = false,
AutoGain = false,
AutoWhiteBalance = false
}), Params.DefaultRecieveTimeout);
}
/// <summary>
/// Return the camera to automatic mode, auto exposure, gain, and WB.
/// </summary>
public void AutoCamera()
{
RSUtils.ReceiveSync(queue, opPort.SetCamera(new fluke.CamControlState()
{
Darkness = 0,
Val1 = 0x80,
Val2 = 0x80,
Brightness = b.ScribblerHelper.CameraParams.CAM_BRT_DEFAULT,
Exposure = b.ScribblerHelper.CameraParams.CAM_EXP_DEFAULT,
AutoExposure = true,
AutoGain = true,
AutoWhiteBalance = true
}), Params.DefaultRecieveTimeout);
}
/// <summary>
/// Set a group of elements in the vector by index.
/// </summary>
/// <param name="index"></param>
/// <param name="value"></param>
public void Set(List <int> indices, List <double> values)
{
//try
//{
RSUtils.ReceiveSync <DefaultUpdateResponseType>(taskQueue, opPort.SetByIndex(indices, values, DateTime.Now), Myro.Utilities.Params.defaultRecieveTimeout);
//}
//catch (ArgumentOutOfRangeException e)
//{
// throw new AdapterArgumentException(
// e.ActualValue is Int32 ?
// Strings.IndexOutOfBounds((Int32)e.ActualValue) :
// Strings.IndexOutOfBounds());
//}
}
/// <summary>
/// Set a single element in the vector by index.
/// </summary>
/// <param name="index"></param>
/// <param name="value"></param>
public void Set(int index, double value)
{
//try
//{
RSUtils.ReceiveSync <DefaultUpdateResponseType>(taskQueue, opPort.SetByIndex(new List <int>()
{
index
}, new List <double>()
{
value
}, DateTime.Now), Myro.Utilities.Params.defaultRecieveTimeout);
//}
//catch (ArgumentOutOfRangeException)
//{
// throw new AdapterArgumentException(Strings.IndexOutOfBounds(index));
//}
}
/// <summary>
/// Set a single element in the vector by name.
/// </summary>
/// <param name="tag"></param>
/// <param name="value"></param>
public void Set(string key, double value)
{
//try
//{
RSUtils.ReceiveSync <DefaultUpdateResponseType>(taskQueue, opPort.SetByKey(new List <string>()
{
key
}, new List <double>()
{
value
}, DateTime.Now), Myro.Utilities.Params.DefaultRecieveTimeout);
//}
//catch (KeyNotFoundException)
//{
// throw new AdapterArgumentException(Strings.KeyNotFound(key));
//}
}
/// <summary>
/// This is a helper mehtod that connects the Scribbler on the specified
/// COM port. It waits for the connection to complete, and re-throws any
/// exceptions generated by the Scribbler service.
/// </summary>
/// <param name="comPort"></param>
private static void connectWaitForScribbler(string comPort)
{
ManualResetEvent evt = new ManualResetEvent(false);
waitForService(new ServiceInfoType(scribbler.Contract.Identifier),
delegate(ServiceInfoType info) { brickService = info; evt.Set(); });
evt.WaitOne(Params.DefaultRecieveTimeout, false);
if (brickService == null)
{
throw new MyroInitException("Could not find Scribbler service");
}
var scribPort = DssEnvironment.ServiceForwarder <scribbler.ScribblerOperations>(new Uri(brickService.Service));
DispatcherQueue queue = new DispatcherQueue("init", new Dispatcher());
try
{
if (comPort != null)
{
int comNumber;
if (comPort.ToLower().StartsWith("com"))
{
comNumber = Int32.Parse(comPort.Substring(3));
}
else
{
throw new MyroInitException("COM port string must be of the format com2, com5, etc.");
}
RSUtils.ReceiveSync(queue, scribPort.Replace(new scribbler.ScribblerState()
{
ComPort = comNumber
}), Params.DefaultRecieveTimeout);
}
DssEnvironment.LogInfo("calling reconnect...");
RSUtils.ReceiveSync(queue, scribPort.Reconnect(), Params.DefaultRecieveTimeout);
DssEnvironment.LogInfo("reconnect returned");
}
catch (Exception)
{
throw;
}
finally
{
queue.Dispose();
}
}
private T tryCreateAdapter()
{
lock (this)
{
try
{
// Query the service initially
PortSet <LookupResponse, Fault> responsePort = new PortSet <LookupResponse, Fault>();
DssEnvironment.ServiceForwarderUnknownType(new Uri("dssp.tcp://localhost/" + Name)).PostUnknownType(
new DsspDefaultLookup()
{
Body = new LookupRequestType(), ResponsePort = responsePort
});
ServiceInfoType responseRecord = RSUtils.ReceiveSync(responsePort, Myro.Utilities.Params.DefaultRecieveTimeout);
// Try to find a working contract for each adapter
foreach (var factory in adapterFactories)
{
try
{
ServiceInfoType serviceRecord = RSUtils.FindCompatibleContract(responseRecord, factory.SupportedContracts);
T ret = (T)factory.Create(serviceRecord);
Console.WriteLine("Attached to " + serviceRecord.Service + " as \"" + Name + "\"");
return(ret);
}
catch (NoContractFoundException) { }
}
// If we haven't returned already in the loop, we didn't find
// an adapter that works.
throw new NoAdapterFoundException(responseRecord);
}
catch (NoAdapterFoundException)
{
throw;
}
catch (Exception e)
{
DssEnvironment.LogError("Error querying or attaching to " + "dssp.tcp://localhost/" + Name + ": " + e.ToString());
throw;
}
}
}
/// <summary>
/// Modify the robot name.
/// </summary>
/// <param name="name"></param>
public void SetName(string name)
{
RSUtils.ReceiveSync(queue, opPort.SetName(name), Params.DefaultRecieveTimeout);
}
public drive.DriveDifferentialTwoWheelState Get()
{
return(RSUtils.ReceiveSync <drive.DriveDifferentialTwoWheelState>(taskQueue, drivePort.Get(), Myro.Utilities.Params.DefaultRecieveTimeout));
}
//public override bool Equals(Object obj)
//{
// if (obj is String)
// {
// string truncUri = ServiceInfo.Service.Substring(ServiceInfo.Service.IndexOf('/', ServiceInfo.Service.IndexOf("//") + 2));
// return String.Equals(truncUri, obj);
// }
// else
// return String.Equals(this.ServiceInfo.Service, ((DriveAdapter)obj).ServiceInfo.Service);
//}
public void Stop()
{
RSUtils.ReceiveSync <DefaultUpdateResponseType>(taskQueue, drivePort.AllStop(), Myro.Utilities.Params.DefaultRecieveTimeout);
}
/// <summary>
/// Set the fluke IR obstacle sensor power.
/// </summary>
/// <param name="power"></param>
public void SetIRPower(byte power)
{
RSUtils.ReceiveSync(queue, opPort.SetIRPower(power), Params.DefaultRecieveTimeout);
}
protected void EnableMotors()
{
RSUtils.ReceiveSync(taskQueue, drivePort.EnableDrive(true), Params.DefaultRecieveTimeout);
}
/// <summary>
/// Retrieve a single element from the state vetor, by name, with full
/// safety checks. Throws AdapterArgumentException and
/// AdapterOperationException.
/// </summary>
/// <param name="tag"></param>
/// <returns></returns>
public double Get(string key)
{
var resp = RSUtils.ReceiveSync <vector.GetElementResponseType>(taskQueue, opPort.GetByKey(key), Myro.Utilities.Params.DefaultRecieveTimeout);
return(resp.Value);
}
/// <summary>
/// Retrieve the entire vector state. State members may be null.
/// </summary>
/// <returns></returns>
public vector.VectorState GetState()
{
return(RSUtils.ReceiveSync <vector.VectorState>(taskQueue, opPort.Get(), Myro.Utilities.Params.defaultRecieveTimeout));
}
/// <summary>
/// Set all elements in the vector.
/// </summary>
/// <param name="values"></param>
public void SetAllElements(List <double> values)
{
RSUtils.ReceiveSync <DefaultUpdateResponseType>(taskQueue, opPort.SetAllElements(values, DateTime.Now), Myro.Utilities.Params.defaultRecieveTimeout);
}
/// <summary>
/// Set a group of elements in the vector by name.
/// </summary>
/// <param name="index"></param>
/// <param name="value"></param>
public void Set(List <string> keys, List <double> values)
{
RSUtils.ReceiveSync <DefaultUpdateResponseType>(taskQueue, opPort.SetByKey(keys, values, DateTime.Now), Myro.Utilities.Params.DefaultRecieveTimeout);
}
/// <summary>
/// Return the robot name.
/// </summary>
/// <returns></returns>
public string GetName()
{
return(RSUtils.ReceiveSync(queue, opPort.GetName(), Params.DefaultRecieveTimeout).Value);
}
/// <summary>
/// Retrieve all vector elements at once.
/// </summary>
/// <returns></returns>
public List <double> GetAllElements()
{
return(RSUtils.ReceiveSync <vector.GetAllElementsResponseType>(taskQueue, opPort.GetAllElements(), Myro.Utilities.Params.defaultRecieveTimeout).Values);
}