/// <summary>
/// we are getting encoder ticks for both sides from Power Brick here
/// </summary>
/// <param name="update"></param>
private void MotorEncoderNotificationHandler(powerbrick.UpdateMotorEncoder update)
{
if (_state.HardwareIdentifier != update.Body.HardwareIdentifier)
{
#if TRACEDEBUGTICKS
//LogInfo("TrackRoamerEncoder:MotorEncoderNotificationHandler() " + Name + " ignored other side -- update: left=" + update.Body.LeftDistance + " right=" + update.Body.RightDistance);
#endif // TRACEDEBUGTICKS
// addressed to the other side encoder, ignore it
return;
}
#if TRACEDEBUGTICKS
LogInfo("TrackRoamerEncoder:MotorEncoderNotificationHandler() " + Name + " got update: left=" + update.Body.LeftDistance + " right=" + update.Body.RightDistance);
#endif // TRACEDEBUGTICKS
bool changed = false;
int val = 0;
double dval = 0.0d;
double dabs = 0.0d;
if (_state.HardwareIdentifier == 1 && update.Body.LeftDistance != null)
{
lock (m_lastResetTicksLock)
{
// Generic Drive operates on positive tick counter values
dabs = (double)update.Body.LeftDistance;
if (m_lastResetTicks == null)
{
m_lastResetTicks = dabs;
}
else
{
dval = Math.Abs(dabs - (double)m_lastResetTicks);
val = (int)dval;
}
changed = _state.TicksSinceReset != val;
#if TRACEDEBUGTICKS
//LogInfo("TrackRoamerEncoder:MotorEncoderNotificationHandler() -- left: " + _state.TicksSinceReset + " --> " + val + " (" + update.Body.LeftDistance + ")");
#endif // TRACEDEBUGTICKS
}
}
if (_state.HardwareIdentifier == 2 && update.Body.RightDistance != null)
{
lock (m_lastResetTicksLock)
{
// Generic Drive operates on positive tick counter values
dabs = (double)update.Body.RightDistance;
if (m_lastResetTicks == null)
{
m_lastResetTicks = dabs;
}
else
{
dval = Math.Abs(dabs - (double)m_lastResetTicks);
val = (int)dval;
}
changed = _state.TicksSinceReset != val;
#if TRACEDEBUGTICKS
//LogInfo("TrackRoamerEncoder:MotorEncoderNotificationHandler() -- right: " + _state.TicksSinceReset + " --> " + val + " (" + update.Body.RightDistance + ")");
#endif // TRACEDEBUGTICKS
}
}
if (changed)
{
//LogInfo("TrackRoamerEncoder:MotorEncoderNotificationHandler() -- "
// + (_state.HardwareIdentifier == 1 ? " left: " : "right: ")
// + _state.TicksSinceReset + " --> " + val + " (" + dabs + ")");
_state.TicksSinceReset = val;
_state.CurrentReading = val;
_state.CurrentAngle = val * 2.0d * Math.PI / _state.TicksPerRevolution;
//update time
_state.TimeStamp = DateTime.Now;
this.SendNotification <pxencoder.UpdateTickCount>(_subMgrPort, new pxencoder.UpdateTickCountRequest(_state.TimeStamp, _state.TicksSinceReset));
}
//update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
/// <summary>
/// Handles UpdateTickCount notification from the Encoder partners
/// </summary>
/// <remarks>Posts a <typeparamref name="EncoderReplace"/> request to itself.</remarks>
/// <param name="update">notification</param>
//void EncoderUpdateTickCountNotificationLeft(encoder.UpdateTickCount update)
//{
// //update.Body.Count;
// //update.Body.TimeStamp;
// //_mainPort.Post(new EncoderUpdate(update.Body, 1));
//}
//void EncoderUpdateTickCountNotificationRight(encoder.UpdateTickCount update)
//{
// //_mainPort.Post(new EncoderUpdate(update.Body, 2));
//}
/// <summary>
/// Handles EncoderUpdate request
/// </summary>
/// <param name="update">request</param>
//void EncoderUpdateHandler(EncoderUpdate update)
//{
// LogInfo("****************************** DriveBehaviorServiceBase:: EncoderUpdateHandler: id=" + update.HardwareIdentifier + " count=" + update.Body.Count);
//
// //_state.EncoderStateLeft = update.Body;
// //_state.Velocity = (VelocityFromWheel(update.Body.LeftWheel) + VelocityFromWheel(update.Body.RightWheel)) / 2;
// update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
//}
/// <summary>
/// gets encoder ticks (distance) notifications directly from Power Brick
/// </summary>
/// <param name="update"></param>
private void EncoderNotificationHandler(powerbrick.UpdateMotorEncoder update)
{
// Note: update.Body.LeftDistance / update.Body.RightDistance are absolute encoder ticks, that grow or decrease as the wheel turns. Forward rotation produces positive increments on both wheels.
switch (update.Body.HardwareIdentifier)
{
case 1: // Left
{
double leftDistance = update.Body.LeftDistance ?? 0.0d;
if (RobotState.IsEncoderSpeedReversed)
{
leftDistance = -leftDistance;
}
#if TRACEDEBUGTICKS
Tracer.Trace("****************************** DriveBehaviorServiceBase:: EncoderNotificationHandler() -- notification - Left: d=" + leftDistance + " t=" + update.Body.Timestamp.Ticks + " dt=" + (DateTime.Now.Ticks - update.Body.Timestamp.Ticks));
#endif // TRACEDEBUGTICKS
_state.WheelsEncoderState.LeftMostRecent = update.Body.Timestamp;
incrementOdometry(leftDistance - (_state.WheelsEncoderState.LeftDistance ?? 0.0d), null); // we are interested in increments
_state.WheelsEncoderState.LeftDistance = update.Body.LeftDistance.HasValue ? (double?)leftDistance : null;
if (_mapperVicinity.turnState != null && !_mapperVicinity.turnState.hasFinished)
{
proxibrick.DirectionDataDssSerializable mostRecentDirection = _state.MostRecentDirection;
if (mostRecentDirection != null)
{
_mapperVicinity.turnState.directionCurrent = new Direction()
{
heading = mostRecentDirection.heading, TimeStamp = update.Body.Timestamp.Ticks
};
}
}
}
break;
case 2: // Right
{
double rightDistance = update.Body.RightDistance ?? 0.0d;
if (RobotState.IsEncoderSpeedReversed)
{
rightDistance = -rightDistance;
}
#if TRACEDEBUGTICKS
Tracer.Trace("****************************** DriveBehaviorServiceBase:: EncoderNotificationHandler() -- notification - Right: d=" + rightDistance + " t=" + update.Body.Timestamp.Ticks + " dt=" + (DateTime.Now.Ticks - update.Body.Timestamp.Ticks));
#endif // TRACEDEBUGTICKS
_state.WheelsEncoderState.RightMostRecent = update.Body.Timestamp;
incrementOdometry(null, rightDistance - (_state.WheelsEncoderState.RightDistance ?? 0.0d)); // we are interested in increments
_state.WheelsEncoderState.RightDistance = update.Body.RightDistance.HasValue ? (double?)rightDistance : null;
if (_mapperVicinity.turnState != null && !_mapperVicinity.turnState.hasFinished)
{
proxibrick.DirectionDataDssSerializable mostRecentDirection = _state.MostRecentDirection;
if (mostRecentDirection != null)
{
_mapperVicinity.turnState.directionCurrent = new Direction()
{
heading = mostRecentDirection.heading, TimeStamp = update.Body.Timestamp.Ticks
};
}
}
}
break;
default:
LogError("Error: ****************************** DriveBehaviorServiceBase:: EncoderNotificationHandler() -- notification - bad HardwareIdentifier=" + update.Body.HardwareIdentifier);
break;
}
}
