public IEnumerator <ITask> ReplaceHandler(encoder.Replace replace)
{
if (_subscribed)
{
throw new InvalidOperationException("Already subscribed");
}
else if (ValidState(replace.Body))
{
_state = replace.Body;
SaveState(_state);
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
SubscribeToNXT();
}
else
{
throw new InvalidOperationException("Invalid State for replacement");
}
yield break;
}
public IEnumerator <ITask> ReplaceHandler(pxencoder.Replace replace)
{
//LogInfo("TrackRoamerEncoder : ReplaceHandler()");
if (_subscribed)
{
LogError("TrackRoamerEncoder : ReplaceHandler(): Already subscribed");
}
else if (ValidState(replace.Body))
{
//_state = (TREncoderState)replace.Body;
_state = replace.Body;
SaveState(_state);
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
SubscribeToPowerBrick();
}
else
{
LogError("TrackRoamerEncoder : ReplaceHandler(): Invalid State for replacement");
}
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;
}
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;
}