public IEnumerator<ITask> ReliableSubscribeHandler(pxencoder.ReliableSubscribe subscribe)
{
LogInfo("TrackRoamerEncoder " + _state.HardwareIdentifier + " received Reliable Subscription request from Subscriber=" + subscribe.Body.Subscriber);
yield return Arbiter.Choice(
SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
LogInfo("TrackRoamerEncoder : Reliable 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 static bool ValidState(pxencoder.EncoderState state)
{
if (state != null)
{
if (state.HardwareIdentifier >= 1 && state.HardwareIdentifier <= 2)
{
return true;
}
}
return false;
}
public IEnumerator<ITask> ResetHandler(pxencoder.Reset update)
{
LogInfo("TrackRoamerEncoder : ResetHandler() m_lastResetTicks=" + m_lastResetTicks);
// reset physical counter in the bot AX2850 controller:
//trackroamerbot.ResetMotorEncoder resetMotorEncoder = new trackroamerbot.ResetMotorEncoder();
//resetMotorEncoder.Body.HardwareIdentifier = _state.HardwareIdentifier;
//_trackroamerbotPort.Post(resetMotorEncoder);
// initialize our local logical counter:
lock (m_lastResetTicksLock)
{
_state.TicksSinceReset = 0;
_state.CurrentReading = 0;
_state.CurrentAngle = 0.0d;
m_lastResetTicks = null;
}
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
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;
}
void StopMotorWithEncoderHandler(Port<encoder.UpdateTickCount> encoderNotificationPort, string side, encoder.UpdateTickCount update, motor.MotorOperations motorPort)
{
int stopWheelAt;
bool ignore;
switch (side)
{
case "left":
stopWheelAt = stopLeftWheelAt;
ignore = !_leftEncoderTickEnabled;
break;
default:
case "right":
stopWheelAt = stopRightWheelAt;
ignore = !_rightEncoderTickEnabled;
break;
}
if (!ignore)
{
#if TRACEDEBUGTICKS
Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " encoder at=" + update.Body.Count + " will stop wheel at=" + stopWheelAt);
#endif // TRACEDEBUGTICKS
if (update.Body.Count >= stopWheelAt)
{
switch (side)
{
case "left":
_leftEncoderTickEnabled = false;
break;
default:
case "right":
_rightEncoderTickEnabled = false;
break;
}
// whatever else got stuck there, we are not interested. Keep the port clear.
//Port<encoder.UpdateTickCount> port = (Port<encoder.UpdateTickCount>)encoderNotificationPort[typeof(encoder.UpdateTickCount)];
//port.Clear();
motor.SetMotorPower stop = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = 0 });
motorPort.Post(stop);
Activate(Arbiter.Choice(stop.ResponsePort,
delegate(DefaultUpdateResponseType resp)
{
#if TRACEDEBUGTICKS
Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - motor stopped by encoder !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
#endif // TRACEDEBUGTICKS
},
delegate(Fault fault) { LogError(fault); }
));
#if TRACEDEBUGTICKS
Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - Sending to completionPort: DriveStage.Completed !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
#endif // TRACEDEBUGTICKS
completionPort.Post(drive.DriveStage.Completed);
}
}
}
public IEnumerator<ITask> GetHandler(pxencoder.Get get)
{
get.ResponsePort.Post(_state);
yield break;
}
public virtual IEnumerator<ITask> EncoderUpdateTickCountHandler(pxencoder.UpdateTickCount update)
{
throw new InvalidOperationException("Outbound sensor notifications are not valid for sending requests.");
}
public virtual IEnumerator<ITask> EncoderSubscribeHandler(pxencoder.Subscribe subscribe)
{
base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
yield break;
}
public virtual IEnumerator<ITask> EncoderResetHandler(pxencoder.Reset resetEncoder)
{
if (!ValidateConnection(_state, resetEncoder.ResponsePort))
yield break;
// Reset the encoder relative to the last power command.
yield return Arbiter.Choice(_legoBrickPort.SendNxtCommand(new LegoResetMotorPosition(_state.MotorPort, true)),
delegate(LegoResponse response)
{
// The Current encoder will be updated the next time we poll.
SendNotification<pxencoder.Reset>(_subMgrPort, resetEncoder);
resetEncoder.ResponsePort.Post(DefaultUpdateResponseType.Instance);
},
delegate(Fault fault) { resetEncoder.ResponsePort.Post(fault); });
yield break;
}
public virtual IEnumerator<ITask> EncoderReplaceHandler(pxencoder.Replace replace)
{
_state.ResetableEncoderDegrees = replace.Body.CurrentReading;
_state.CurrentEncoderTimeStamp = replace.Body.TimeStamp;
SendNotification<pxencoder.Replace>(_subMgrPort, replace);
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
yield break;
}
public virtual IEnumerator<ITask> EncoderGetHandler(pxencoder.Get get)
{
int hardwareIdentifier = _state.ToGenericState(_genericState).HardwareIdentifier;
pxencoder.EncoderState encoderState = new pxencoder.EncoderState(hardwareIdentifier, (int)_state.CurrentEncoderDegrees);
get.ResponsePort.Post(encoderState);
yield break;
}
