/// <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; } }