public IEnumerator <ITask> SubscribeHandler(encoder.Subscribe subscribe)
{
yield return(Arbiter.Choice(
SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
SendNotification <encoder.Replace>(_subMgrPort, subscribe.Body.Subscriber, new encoder.Replace(_state));
},
delegate(Exception fault)
{
LogError(fault);
}
));
}
public IEnumerator <ITask> SubscribeHandler(pxencoder.Subscribe subscribe)
{
LogInfo("TrackRoamerEncoder " + _state.HardwareIdentifier + " received Subscription request from Subscriber=" + subscribe.Body.Subscriber);
yield return(Arbiter.Choice(
SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
LogInfo("TrackRoamerEncoder : Subscription granted (succeeded) subscriber: " + subscribe.Body.Subscriber);
//_subMgrPort.Post(new submgr.Submit(subscribe.Body.Subscriber, DsspActions.ReplaceRequest, _state, null));
},
delegate(Exception fault)
{
LogError(fault);
}
));
}
private IEnumerator <ITask> ConfigureDrive()
{
LogInfo("TrackRoamerDriveService:: ConfigureDrive()");
bool noError = true;
// Configure motor connections
motor.Replace configureLeftMotor = new motor.Replace();
configureLeftMotor.Body = _state.LeftWheel.MotorState;
_leftMotorPort.Post(configureLeftMotor);
motor.Replace configureRightMotor = new motor.Replace();
configureRightMotor.Body = _state.RightWheel.MotorState;
_rightMotorPort.Post(configureRightMotor);
yield return(Arbiter.Choice(configureLeftMotor.ResponsePort,
delegate(DefaultReplaceResponseType success) { LogInfo(" Left Motor Port set"); },
delegate(Fault fault) { LogError(fault); noError = false; }));
yield return(Arbiter.Choice(configureRightMotor.ResponsePort,
delegate(DefaultReplaceResponseType success) { LogInfo(" Right Motor Port set"); },
delegate(Fault fault) { LogError(fault); noError = false; }));
// Configure encoder connections
if (_leftEncoderCmdPort != null)
{
encoder.Replace configureLeftEncoder = new encoder.Replace();
configureLeftEncoder.Body = _state.LeftWheel.EncoderState;
_leftEncoderCmdPort.Post(configureLeftEncoder);
yield return(Arbiter.Choice(configureLeftEncoder.ResponsePort,
delegate(DefaultReplaceResponseType success) { LogInfo(" Left Encoder Port set"); },
delegate(Fault fault) { LogError(fault); noError = false; }));
encoder.Subscribe op = new encoder.Subscribe();
op.Body = new SubscribeRequestType();
op.NotificationPort = _leftEncoderTickPort;
_leftEncoderCmdPort.Post(op);
yield return(Arbiter.Choice(op.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive <encoder.UpdateTickCount>(true, _leftEncoderTickPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(_leftEncoderTickPort, "left", update, _leftMotorPort);
}));
},
delegate(Fault fault) { LogError(fault); }
));
}
if (_rightEncoderCmdPort != null)
{
encoder.Replace configureRightEncoder = new encoder.Replace();
configureRightEncoder.Body = _state.RightWheel.EncoderState;
_rightEncoderCmdPort.Post(configureRightEncoder);
yield return(Arbiter.Choice(configureRightEncoder.ResponsePort,
delegate(DefaultReplaceResponseType success) { LogInfo(" Right Encoder Port set"); },
delegate(Fault fault) { LogError(fault); noError = false; }));
encoder.Subscribe op2 = new encoder.Subscribe();
op2.Body = new SubscribeRequestType();
op2.NotificationPort = _rightEncoderTickPort;
_leftEncoderCmdPort.Post(op2);
yield return(Arbiter.Choice(op2.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive <encoder.UpdateTickCount>(true, _rightEncoderTickPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(_rightEncoderTickPort, "right", update, _rightMotorPort);
}
));
},
delegate(Fault fault) { LogError(fault); }
));
}
if (noError)
{
LogInfo("TrackRoamerDriveService:: ConfigureDrive() - success");
_state.IsEnabled = true;
}
yield break;
}
private IEnumerator<ITask> ConfigureDrive()
{
LogInfo("TrackRoamerDriveService:: ConfigureDrive()");
bool noError = true;
// Configure motor connections
motor.Replace configureLeftMotor = new motor.Replace();
configureLeftMotor.Body = _state.LeftWheel.MotorState;
_leftMotorPort.Post(configureLeftMotor);
motor.Replace configureRightMotor = new motor.Replace();
configureRightMotor.Body = _state.RightWheel.MotorState;
_rightMotorPort.Post(configureRightMotor);
yield return Arbiter.Choice(configureLeftMotor.ResponsePort,
delegate(DefaultReplaceResponseType success) { LogInfo(" Left Motor Configured"); },
delegate(Fault fault) { LogError(fault); noError = false; });
yield return Arbiter.Choice(configureRightMotor.ResponsePort,
delegate(DefaultReplaceResponseType success) { LogInfo(" Right Motor Configured"); },
delegate(Fault fault) { LogError(fault); noError = false; });
// Configure encoder connections, and permanently subscribe to the encoders on internal ports.
if (_leftEncoderPort != null)
{
encoder.Replace configureLeftEncoder = new encoder.Replace();
configureLeftEncoder.Body = _state.LeftWheel.EncoderState;
_leftEncoderPort.Post(configureLeftEncoder);
yield return Arbiter.Choice(configureLeftEncoder.ResponsePort,
delegate(DefaultReplaceResponseType success) { LogInfo(" Left Encoder Configured"); },
delegate(Fault fault) { LogError(fault); noError = false; });
encoder.Subscribe op = new encoder.Subscribe();
op.Body = new SubscribeRequestType();
op.NotificationPort = _leftEncoderTickPort;
_leftEncoderPort.Post(op);
yield return (Arbiter.Choice(op.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive<encoder.UpdateTickCount>(true, _leftEncoderTickPort, // "true" here makes listener to subscription permanent
delegate(encoder.UpdateTickCount update)
{
#if TRACEDEBUGTICKS
LogInfo("Drive: left encoder tick: " + update.Body.Count);
#endif // TRACEDEBUGTICKS
StopMotorWithEncoderHandler(_leftEncoderTickPort, "left", update, _leftMotorPort);
}));
},
delegate(Fault fault) { LogError(fault); noError = false; }
));
}
if (_rightEncoderPort != null)
{
encoder.Replace configureRightEncoder = new encoder.Replace();
configureRightEncoder.Body = _state.RightWheel.EncoderState;
_rightEncoderPort.Post(configureRightEncoder);
yield return Arbiter.Choice(configureRightEncoder.ResponsePort,
delegate(DefaultReplaceResponseType success) { LogInfo(" Right Encoder Configured"); },
delegate(Fault fault) { LogError(fault); noError = false; });
encoder.Subscribe op2 = new encoder.Subscribe();
op2.Body = new SubscribeRequestType();
op2.NotificationPort = _rightEncoderTickPort;
_rightEncoderPort.Post(op2);
yield return (Arbiter.Choice(op2.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive<encoder.UpdateTickCount>(true, _rightEncoderTickPort, // "true" here makes listener to subscription permanent
delegate(encoder.UpdateTickCount update)
{
#if TRACEDEBUGTICKS
LogInfo("Drive: right encoder tick: " + update.Body.Count);
#endif // TRACEDEBUGTICKS
StopMotorWithEncoderHandler(_rightEncoderTickPort, "right", update, _rightMotorPort);
}
));
},
delegate(Fault fault) { LogError(fault); noError = false; }
));
}
if (noError)
{
LogInfo("TrackRoamerDriveService:: ConfigureDrive() - success");
_state.IsEnabled = true;
// Start the encoder polling interval
this.encodersPollingPort.Post(DateTime.Now);
}
else
{
LogError("TrackRoamerDriveService:: ConfigureDrive() - failure");
_state.IsEnabled = false;
}
yield break;
}
private IEnumerator<ITask> ConfigureDrive()
{
LogInfo("TrackRoamerDriveService:: ConfigureDrive()");
bool noError = true;
// Configure motor connections
motor.Replace configureLeftMotor = new motor.Replace();
configureLeftMotor.Body = _state.LeftWheel.MotorState;
_leftMotorPort.Post(configureLeftMotor);
motor.Replace configureRightMotor = new motor.Replace();
configureRightMotor.Body = _state.RightWheel.MotorState;
_rightMotorPort.Post(configureRightMotor);
yield return Arbiter.Choice(configureLeftMotor.ResponsePort,
delegate(DefaultReplaceResponseType success) { LogInfo(" Left Motor Port set"); },
delegate(Fault fault) { LogError(fault); noError = false; });
yield return Arbiter.Choice(configureRightMotor.ResponsePort,
delegate(DefaultReplaceResponseType success) { LogInfo(" Right Motor Port set"); },
delegate(Fault fault) { LogError(fault); noError = false; });
// Configure encoder connections
if (_leftEncoderCmdPort != null)
{
encoder.Replace configureLeftEncoder = new encoder.Replace();
configureLeftEncoder.Body = _state.LeftWheel.EncoderState;
_leftEncoderCmdPort.Post(configureLeftEncoder);
yield return Arbiter.Choice(configureLeftEncoder.ResponsePort,
delegate(DefaultReplaceResponseType success) { LogInfo(" Left Encoder Port set"); },
delegate(Fault fault) { LogError(fault); noError = false; });
encoder.Subscribe op = new encoder.Subscribe();
op.Body = new SubscribeRequestType();
op.NotificationPort = _leftEncoderTickPort;
_leftEncoderCmdPort.Post(op);
yield return (Arbiter.Choice(op.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive<encoder.UpdateTickCount>(true, _leftEncoderTickPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(_leftEncoderTickPort, "left", update, _leftMotorPort);
}));
},
delegate(Fault fault) { LogError(fault); }
));
}
if (_rightEncoderCmdPort != null)
{
encoder.Replace configureRightEncoder = new encoder.Replace();
configureRightEncoder.Body = _state.RightWheel.EncoderState;
_rightEncoderCmdPort.Post(configureRightEncoder);
yield return Arbiter.Choice(configureRightEncoder.ResponsePort,
delegate(DefaultReplaceResponseType success) { LogInfo(" Right Encoder Port set"); },
delegate(Fault fault) { LogError(fault); noError = false; });
encoder.Subscribe op2 = new encoder.Subscribe();
op2.Body = new SubscribeRequestType();
op2.NotificationPort = _rightEncoderTickPort;
_leftEncoderCmdPort.Post(op2);
yield return (Arbiter.Choice(op2.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive<encoder.UpdateTickCount>(true, _rightEncoderTickPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(_rightEncoderTickPort, "right", update, _rightMotorPort);
}
));
},
delegate(Fault fault) { LogError(fault); }
));
}
if (noError)
{
LogInfo("TrackRoamerDriveService:: ConfigureDrive() - success");
_state.IsEnabled = true;
}
yield break;
}