/// <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>
/// gets encoder speed notifications directly from Power Brick
/// </summary>
/// <param name="update"></param>
private void EncoderSpeedNotificationHandler(powerbrick.UpdateMotorEncoderSpeed update)
{
#if TRACEDEBUGTICKS
Tracer.Trace("****************************** DriveBehaviorServiceBase:: EncoderSpeedNotificationHandler() -- update - Speed: Left=" + update.Body.LeftSpeed + " Right=" + update.Body.RightSpeed + " t=" + update.Body.Timestamp.Ticks + " dt=" + (DateTime.Now.Ticks - update.Body.Timestamp.Ticks));
#endif // TRACEDEBUGTICKS
_state.WheelsEncoderState.LeftSpeed = VelocityFromWheelSpeedMetersPerSec(update.Body.LeftSpeed);
_state.WheelsEncoderState.RightSpeed = VelocityFromWheelSpeedMetersPerSec(update.Body.RightSpeed);
double lv = (double)_state.WheelsEncoderState.LeftSpeed; // m/s
double rv = (double)_state.WheelsEncoderState.RightSpeed; // m/s
double velocity = (lv + rv) / 2.0d;
_state.Velocity = velocity;
// fill robotState to be used for drawing on the next redraw cycle.
_mapperVicinity.robotState.medianVelocity = velocity;
_mapperVicinity.robotState.leftSpeed = lv;
_mapperVicinity.robotState.rightSpeed = rv;
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 };
}
}
#if TRACEDEBUGTICKS
Tracer.Trace("****************************** DriveBehaviorServiceBase:: EncoderSpeedNotificationHandler() -- Median Velocity: " + Math.Round(velocity, 3) + " m/s (l=" + lv + " r=" + rv + ")");
#endif // TRACEDEBUGTICKS
}
/// <summary>
/// receiving Whiskers notifications here
/// </summary>
/// <param name="notification"></param>
public void WhiskersNotificationHandler(powerbrick.UpdateWhiskers notification)
{
/*
HardwareIdentifier coding:
1st digit 1=Left 2=Right
2d digit 0=Front 1=Rear
3d digit 1=Whisker 2=IRBumper 3=StepSensor
*/
foreach (pxbumper.ContactSensor bumper in _state.Sensors)
{
bool changed = false;
switch (bumper.HardwareIdentifier)
{
case 101:
if (notification.Body.FrontWhiskerLeft != null && bumper.Pressed != notification.Body.FrontWhiskerLeft)
{
bumper.Pressed = (bool)notification.Body.FrontWhiskerLeft;
changed = true;
}
break;
case 201:
if (notification.Body.FrontWhiskerRight != null && bumper.Pressed != notification.Body.FrontWhiskerRight)
{
bumper.Pressed = (bool)notification.Body.FrontWhiskerRight;
changed = true;
}
break;
}
if (changed)
{
this.SendNotification<pxbumper.Update>(_subMgrPort, new pxbumper.Update(bumper));
}
}
}
/// <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;
}
}
