public virtual IEnumerator <ITask> ConnectToBrickHandler(ConnectToBrick update)
{
// Validate the sensor port.
if ((update.Body.SensorPort & NxtSensorPort.AnySensorPort)
!= update.Body.SensorPort)
{
update.ResponsePort.Post(
Fault.FromException(
new ArgumentException(
string.Format("Invalid Sensor Port: {0}",
((LegoNxtPort)update.Body.SensorPort)))));
yield break;
}
_state.SensorPort = update.Body.SensorPort;
_state.Connected = false;
if (!string.IsNullOrEmpty(update.Body.Name))
{
_state.Name = update.Body.Name;
}
_state.PollingFrequencyMs = update.Body.PollingFrequencyMs;
Fault fault = null;
brick.Registration registration = new brick.Registration(
new LegoNxtConnection((LegoNxtPort)_state.SensorPort),
LegoDeviceType.DigitalSensor,
Contract.DeviceModel,
Contract.Identifier,
ServiceInfo.Service,
_state.Name);
// Reserve the port
yield return(Arbiter.Choice(_legoBrickPort.ReserveDevicePort(registration),
delegate(brick.AttachResponse reserveResponse)
{
if (reserveResponse.DeviceModel == registration.DeviceModel)
{
registration.Connection = reserveResponse.Connection;
}
},
delegate(Fault f)
{
fault = f;
LogError(fault);
registration.Connection.Port = LegoNxtPort.NotConnected;
}));
if (registration.Connection.Port == LegoNxtPort.NotConnected && update.Body.SensorPort != NxtSensorPort.NotConnected)
{
if (fault == null)
{
fault = Fault.FromException(new Exception("Failure Configuring NXT Ultrasonic Sensor on Port " + update.Body.ToString()));
}
update.ResponsePort.Post(fault);
yield break;
}
_state.Connected = true;
brick.AttachRequest attachRequest = new brick.AttachRequest(registration);
brick.AttachResponse response = null;
byte[] requestSingleShotReading = { 0x02, 0x41, 0x01 };
byte[] startContinuousReadings = { 0x02, 0x41, 0x02 };
byte[] setContinuousReadingInterval = { 0x02, 0x40, 0x010 };
attachRequest.InitializationCommands = new nxtcmd.NxtCommandSequence(
new nxtcmd.LegoLSGetStatus((NxtSensorPort)registration.Connection.Port),
new nxtcmd.LegoLSRead((NxtSensorPort)registration.Connection.Port),
new nxtcmd.LegoSetInputMode((NxtSensorPort)registration.Connection.Port, LegoSensorType.I2C_9V, LegoSensorMode.RawMode),
new nxtcmd.LegoLSWrite((NxtSensorPort)registration.Connection.Port, startContinuousReadings, 0));
attachRequest.PollingCommands = new nxtcmd.NxtCommandSequence(_state.PollingFrequencyMs,
new nxtcmd.I2CReadSonarSensor(_state.SensorPort, nxtcmd.UltraSonicPacket.ReadMeasurement1));
yield return(Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp) { response = rsp; },
delegate(Fault f) { fault = f; }));
if (response == null)
{
if (fault == null)
{
fault = Fault.FromException(new Exception("Failure Configuring NXT Ultrasonic Sensor"));
}
update.ResponsePort.Post(fault);
yield break;
}
if ((LegoNxtPort)_state.SensorPort != response.Connection.Port)
{
_state.SensorPort = (NxtSensorPort)response.Connection.Port;
}
// Set the motor name
if (string.IsNullOrEmpty(_state.Name) ||
_state.Name.StartsWith("Ultrasonic Sensor on "))
{
_state.Name = "Ultrasonic Sensor on " + response.Connection.ToString();
}
// Send a notification of the connected port
update.Body.Name = _state.Name;
update.Body.PollingFrequencyMs = _state.PollingFrequencyMs;
update.Body.SensorPort = _state.SensorPort;
SendNotification <ConnectToBrick>(_subMgrPort, update);
// Send the message response
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
public virtual IEnumerator<ITask> ConnectToBrickHandler(ConnectToBrick update)
{
// Validate the sensor port.
if ((update.Body.SensorPort & NxtSensorPort.AnySensorPort)
!= update.Body.SensorPort)
{
update.ResponsePort.Post(
Fault.FromException(
new ArgumentException(
string.Format("Invalid Sensor Port: {0}",
((LegoNxtPort)update.Body.SensorPort)))));
yield break;
}
_state.SensorPort = update.Body.SensorPort;
_state.Connected = false;
if (!string.IsNullOrEmpty(update.Body.Name))
_state.Name = update.Body.Name;
_state.PollingFrequencyMs = update.Body.PollingFrequencyMs;
Fault fault = null;
brick.Registration registration = new brick.Registration(
new LegoNxtConnection((LegoNxtPort)_state.SensorPort),
LegoDeviceType.DigitalSensor,
Contract.DeviceModel,
Contract.Identifier,
ServiceInfo.Service,
_state.Name);
// Reserve the port
yield return Arbiter.Choice(_legoBrickPort.ReserveDevicePort(registration),
delegate(brick.AttachResponse reserveResponse)
{
if (reserveResponse.DeviceModel == registration.DeviceModel)
{
registration.Connection = reserveResponse.Connection;
}
},
delegate(Fault f)
{
fault = f;
LogError(fault);
registration.Connection.Port = LegoNxtPort.NotConnected;
});
if (registration.Connection.Port == LegoNxtPort.NotConnected && update.Body.SensorPort != NxtSensorPort.NotConnected)
{
if (fault == null)
fault = Fault.FromException(new Exception("Failure Configuring NXT Ultrasonic Sensor on Port " + update.Body.ToString()));
update.ResponsePort.Post(fault);
yield break;
}
_state.Connected = true;
brick.AttachRequest attachRequest = new brick.AttachRequest(registration);
brick.AttachResponse response = null;
byte[] requestSingleShotReading = { 0x02, 0x41, 0x01 };
byte[] startContinuousReadings = { 0x02, 0x41, 0x02 };
byte[] setContinuousReadingInterval = { 0x02, 0x40, 0x010 };
attachRequest.InitializationCommands = new nxtcmd.NxtCommandSequence(
new nxtcmd.LegoLSGetStatus((NxtSensorPort)registration.Connection.Port),
new nxtcmd.LegoLSRead((NxtSensorPort)registration.Connection.Port),
new nxtcmd.LegoSetInputMode((NxtSensorPort)registration.Connection.Port, LegoSensorType.I2C_9V, LegoSensorMode.RawMode),
new nxtcmd.LegoLSWrite((NxtSensorPort)registration.Connection.Port, startContinuousReadings, 0));
attachRequest.PollingCommands = new nxtcmd.NxtCommandSequence(_state.PollingFrequencyMs,
new nxtcmd.I2CReadSonarSensor(_state.SensorPort, nxtcmd.UltraSonicPacket.ReadMeasurement1));
yield return Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp) { response = rsp; },
delegate(Fault f) { fault = f; });
if (response == null)
{
if (fault == null)
fault = Fault.FromException(new Exception("Failure Configuring NXT Ultrasonic Sensor"));
update.ResponsePort.Post(fault);
yield break;
}
if ((LegoNxtPort)_state.SensorPort != response.Connection.Port)
_state.SensorPort = (NxtSensorPort)response.Connection.Port;
// Set the motor name
if (string.IsNullOrEmpty(_state.Name)
|| _state.Name.StartsWith("Ultrasonic Sensor on "))
_state.Name = "Ultrasonic Sensor on " + response.Connection.ToString();
// Send a notification of the connected port
update.Body.Name = _state.Name;
update.Body.PollingFrequencyMs = _state.PollingFrequencyMs;
update.Body.SensorPort = _state.SensorPort;
SendNotification<ConnectToBrick>(_subMgrPort, update);
// Send the message response
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
public virtual IEnumerator<ITask> ConnectToBrickHandler(ConnectToBrick update)
{
// Validate the motor port.
if ((update.Body.MotorPort & NxtMotorPort.AnyMotorPort)
!= update.Body.MotorPort)
{
update.ResponsePort.Post(
Fault.FromException(
new ArgumentException(
string.Format("Invalid Motor Port: {0}",
((LegoNxtPort)update.Body.MotorPort)))));
yield break;
}
Fault fault = null;
_state.Connected = false;
MotorState currentState = _state.Clone();
// Registration
brick.Registration registration = new brick.Registration(
new LegoNxtConnection((LegoNxtPort)update.Body.MotorPort),
LegoDeviceType.Actuator,
Contract.DeviceModel,
Contract.Identifier,
ServiceInfo.Service,
_state.Name);
// Reserve the port
yield return Arbiter.Choice(_legoBrickPort.ReserveDevicePort(registration),
delegate(brick.AttachResponse reserveResponse)
{
if (reserveResponse.DeviceModel == registration.DeviceModel)
registration.Connection = reserveResponse.Connection;
else
registration.Connection.Port = LegoNxtPort.NotConnected;
},
delegate(Fault f)
{
fault = f;
LogError(fault);
registration.Connection.Port = LegoNxtPort.NotConnected;
});
if (registration.Connection.Port == LegoNxtPort.NotConnected)
{
string genericError = string.Format("Failure Configuring Motor on Port {0}", update.Body.MotorPort);
if (fault == null)
fault = Fault.FromException(new Exception(genericError));
else
{
if (fault.Reason == null) fault.Reason = new ReasonText[1];
if (fault.Reason[0] == null) fault.Reason[0] = new ReasonText();
if (string.IsNullOrEmpty(fault.Reason[0].Value)) fault.Reason[0].Value = genericError;
}
LogError(LogGroups.Console, fault.Reason[0].Value);
update.ResponsePort.Post(fault);
yield break;
}
brick.AttachRequest attachRequest = new brick.AttachRequest(registration);
attachRequest.InitializationCommands = new NxtCommandSequence(
new LegoResetMotorPosition((NxtMotorPort)registration.Connection.Port, false));
attachRequest.PollingCommands = new NxtCommandSequence(currentState.PollingFrequencyMs,
new LegoGetOutputState((NxtMotorPort)registration.Connection.Port));
brick.AttachResponse response = null;
yield return Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp) { response = rsp; },
delegate(Fault f) { fault = f; });
if (response == null)
{
string genericError = string.Format("Failure Configuring Motor on Port {0}", update.Body.MotorPort);
if (fault == null)
fault = Fault.FromException(new Exception(genericError));
else
{
if (fault.Reason == null) fault.Reason = new ReasonText[1];
if (fault.Reason[0] == null) fault.Reason[0] = new ReasonText();
if (string.IsNullOrEmpty(fault.Reason[0].Value)) fault.Reason[0].Value = genericError;
}
LogError(LogGroups.Console, fault.Reason[0].Value);
update.ResponsePort.Post(fault);
yield break;
}
_state.ReversePolarity = update.Body.ReversePolarity;
_state.PollingFrequencyMs = (update.Body.PollingFrequencyMs == 0) ? Contract.DefaultPollingFrequencyMs : update.Body.PollingFrequencyMs;
_state.Connected = (response.Connection.Port != LegoNxtPort.NotConnected);
if (_state.Connected)
_state.MotorPort = (NxtMotorPort)response.Connection.Port;
// Set the motor name
if (!string.IsNullOrEmpty(update.Body.Name))
_state.Name = update.Body.Name;
else if (string.IsNullOrEmpty(currentState.Name)
|| (currentState.Name.StartsWith("Motor") && currentState.Name.Length == 6))
_state.Name = response.Connection.ToString();
// Send a notification of the connected port
update.Body.Name = _state.Name;
update.Body.PollingFrequencyMs = _state.PollingFrequencyMs;
update.Body.MotorPort = _state.MotorPort;
update.Body.ReversePolarity = _state.ReversePolarity;
SendNotification<ConnectToBrick>(_subMgrPort, update);
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
LogVerbose(string.Format("Motor is attached on port {0}", update.Body.MotorPort));
if (!_initialized)
{
_initialized = true;
base.ActivateDsspOperationHandlers();
}
yield break;
}
public virtual IEnumerator<ITask> MainConnectToBrickHandler(ConnectToBrick connectToBrick)
{
DriveState currentState = _state.Clone();
// Validate the motor port.
if ((connectToBrick.Body.LeftWheel.MotorPort & NxtMotorPort.AnyMotorPort)
!= connectToBrick.Body.LeftWheel.MotorPort)
if ((connectToBrick.Body.RightWheel.MotorPort & NxtMotorPort.AnyMotorPort)
!= connectToBrick.Body.RightWheel.MotorPort)
{
connectToBrick.ResponsePort.Post(
Fault.FromException(
new ArgumentException(
string.Format("Invalid Drive Ports: {0},{1}",
((LegoNxtPort)connectToBrick.Body.LeftWheel.MotorPort), ((LegoNxtPort)connectToBrick.Body.RightWheel.MotorPort)))));
yield break;
}
_state.PollingFrequencyMs = connectToBrick.Body.PollingFrequencyMs;
_state.DistanceBetweenWheels = connectToBrick.Body.DistanceBetweenWheels;
_state.LeftWheel.WheelDiameter = connectToBrick.Body.LeftWheel.WheelDiameter;
_state.LeftWheel.MotorPort = connectToBrick.Body.LeftWheel.MotorPort;
_state.LeftWheel.ReversePolarity = connectToBrick.Body.LeftWheel.ReversePolarity;
_state.RightWheel.WheelDiameter = connectToBrick.Body.RightWheel.WheelDiameter;
_state.RightWheel.MotorPort = connectToBrick.Body.RightWheel.MotorPort;
_state.RightWheel.ReversePolarity = connectToBrick.Body.RightWheel.ReversePolarity;
_state.Connected = false;
currentState = _state.Clone();
Fault fault = null;
// Registration
brick.Registration registration = new brick.Registration(
new LegoNxtConnection((LegoNxtPort)currentState.LeftWheel.MotorPort),
LegoDeviceType.Actuator,
Contract.DeviceModel,
Contract.Identifier,
ServiceInfo.Service,
Contract.DeviceModel);
#region Configure Left Motor
// Reserve the Left Motor Port
yield return Arbiter.Choice(_legoBrickPort.ReserveDevicePort(registration),
delegate(brick.AttachResponse reserveResponse)
{
if (reserveResponse.DeviceModel == registration.DeviceModel)
registration.Connection = reserveResponse.Connection;
else
registration.Connection.Port = LegoNxtPort.NotConnected;
},
delegate(Fault f)
{
fault = f;
LogError(fault);
registration.Connection.Port = LegoNxtPort.NotConnected;
});
if (registration.Connection.Port == LegoNxtPort.NotConnected)
{
_state.LeftWheel.MotorPort = NxtMotorPort.NotConnected;
if (fault == null)
fault = Fault.FromException(new Exception("Failure Configuring Drive on Left Motor Port " + connectToBrick.Body.ToString()));
connectToBrick.ResponsePort.Post(fault);
yield break;
}
brick.AttachRequest attachRequest = new brick.AttachRequest(registration);
attachRequest.PollingCommands = new NxtCommandSequence(currentState.PollingFrequencyMs,
new LegoGetOutputState((NxtMotorPort)registration.Connection.Port));
brick.AttachResponse response = null;
yield return Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp) { response = rsp; },
delegate(Fault f) { fault = f; });
if (response == null)
{
if (fault == null)
fault = Fault.FromException(new Exception("Failure Configuring Left Motor on NXT Drive"));
// Update state
_state.LeftWheel.MotorPort = NxtMotorPort.NotConnected;
connectToBrick.ResponsePort.Post(fault);
yield break;
}
if ((LegoNxtPort)currentState.LeftWheel.MotorPort != response.Connection.Port)
{
_state.LeftWheel.MotorPort = currentState.LeftWheel.MotorPort = (NxtMotorPort)response.Connection.Port;
}
#endregion
#region Configure Right Motor
registration.Connection.Port = (LegoNxtPort)connectToBrick.Body.RightWheel.MotorPort;
// Reserve the Right Motor Port
yield return Arbiter.Choice(_legoBrickPort.ReserveDevicePort(registration),
delegate(brick.AttachResponse reserveResponse)
{
if (reserveResponse.DeviceModel == registration.DeviceModel)
registration.Connection = reserveResponse.Connection;
else
registration.Connection.Port = LegoNxtPort.NotConnected;
},
delegate(Fault f)
{
fault = f;
LogError(fault);
registration.Connection.Port = LegoNxtPort.NotConnected;
});
if (registration.Connection.Port == LegoNxtPort.NotConnected)
{
if (fault == null)
fault = Fault.FromException(new Exception("Failure Configuring Drive on Right Motor Port " + connectToBrick.Body.ToString()));
_state.RightWheel.MotorPort = NxtMotorPort.NotConnected;
connectToBrick.ResponsePort.Post(fault);
yield break;
}
attachRequest = new brick.AttachRequest(registration);
attachRequest.PollingCommands = new NxtCommandSequence(currentState.PollingFrequencyMs,
new LegoGetOutputState((NxtMotorPort)registration.Connection.Port));
response = null;
yield return Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp) { response = rsp; },
delegate(Fault f) { fault = f; });
if (response == null)
{
if (fault == null)
fault = Fault.FromException(new Exception("Failure Configuring Right Motor on NXT Drive"));
// Update state
_state.RightWheel.MotorPort = NxtMotorPort.NotConnected;
connectToBrick.ResponsePort.Post(fault);
yield break;
}
if ((LegoNxtPort)currentState.RightWheel.MotorPort != response.Connection.Port)
{
_state.RightWheel.MotorPort = currentState.RightWheel.MotorPort = (NxtMotorPort)response.Connection.Port;
}
#endregion
// If we made it to this point, both motors are connected.
_state.Connected = true;
// save and refresh the state.
currentState = _state.Clone();
// Send a notification of the connected port
connectToBrick.Body.LeftWheel.MotorPort = currentState.LeftWheel.MotorPort;
connectToBrick.Body.RightWheel.MotorPort = currentState.RightWheel.MotorPort;
SendNotification(_subMgrPort, connectToBrick);
connectToBrick.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}