private const double CHR_QUATERNION_YAW_TRUE_NORTH_OFFSET = 14.0; // manual correction to the Quaternion yaw, to align it with true North reading. /// <summary> /// Handle CH Robotics UM6 Orientation Sensor Notification - Euler /// </summary> /// <param name="notification">Euler notification</param> private void ChrEulerHandler(chrum6orientationsensor.EulerNotification notification) { //Tracer.Trace(string.Format("the UM6 Sensor reported Euler: {0} PHI={1} THETA={2} PSI={3}", notification.Body.LastUpdate, notification.Body.phi, notification.Body.theta, notification.Body.psi)); if (USE_DIRECTION_UM6_EULER) { try { // mag heading is reported as a short within +-16200 range double magHeading = Direction.to180(notification.Body.psi * CHR_EULER_YAW_FACTOR + CHR_EULER_YAW_TRUE_NORTHOFFSET); // convert to degrees and ensure that it is within +- 180 degrees and related to True North. // we still use proxibrick.DirectionDataDssSerializable because it is the direction data in the State: proxibrick.DirectionDataDssSerializable newDir = new proxibrick.DirectionDataDssSerializable() { TimeStamp = DateTime.Now, heading = magHeading }; setCurrentDirection(newDir); } catch (Exception exc) { Tracer.Trace("ChrEulerHandler() - " + exc); } } }
/// <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 }
protected void incrementOdometry(double?dL, double?dR) { if (dL.HasValue) { ticksL = ticksL.HasValue ? ticksL + dL : dL; } else { dL = 0.0d; // must have value for calculations below } if (dR.HasValue) { ticksR = ticksR.HasValue ? ticksR + dR : dR; } else { dR = 0.0d; } double wheelRadius = RobotState.wheelRadius; // = 0.1805d; -- meters double TicksPerRevolution = RobotState.ticksPerRevolution; // = 6150; double bodyWidth = RobotState.distanceBetweenWheels; // = 0.570d; -- meters double wheelFactor = 2.0d * Math.PI * wheelRadius / TicksPerRevolution; double distanceLeft = dL.Value * wheelFactor; double distanceRight = dR.Value * wheelFactor; // Now, calculate the final angle, and use that to estimate // the final position. See Gary Lucas' paper for derivations // of the equations. double theta = _mapperVicinity.currentOdometryTheta + (distanceLeft - distanceRight) / bodyWidth; // radians _mapperVicinity.currentOdometryX += ((distanceRight + distanceLeft) / 2.0d) * Math.Cos(theta); // meters _mapperVicinity.currentOdometryY += ((distanceRight + distanceLeft) / 2.0d) * Math.Sin(theta); // meters _mapperVicinity.currentOdometryTheta = theta; #if TRACEDEBUGTICKS Tracer.Trace("****************************** DriveBehaviorServiceBase:: incrementOdometry() -- distance meters Left: " + distanceLeft + " Right: " + distanceRight + " heading degrees: " + Direction.to360(toDegrees(_mapperVicinity.currentOdometryTheta))); #endif // TRACEDEBUGTICKS if (_mapperVicinity.robotState.ignoreAhrs) { // do what compass data handlers usually do: proxibrick.DirectionDataDssSerializable newDir = new proxibrick.DirectionDataDssSerializable() { TimeStamp = DateTime.Now, heading = Direction.to360(toDegrees(_mapperVicinity.currentOdometryTheta)) }; setCurrentDirection(newDir); } }
protected void incrementOdometry(double? dL, double? dR) { if (dL.HasValue) { ticksL = ticksL.HasValue ? ticksL + dL : dL; } else { dL = 0.0d; // must have value for calculations below } if (dR.HasValue) { ticksR = ticksR.HasValue ? ticksR + dR : dR; } else { dR = 0.0d; } double wheelRadius = RobotState.wheelRadius; // = 0.1805d; -- meters double TicksPerRevolution = RobotState.ticksPerRevolution; // = 6150; double bodyWidth = RobotState.distanceBetweenWheels; // = 0.570d; -- meters double wheelFactor = 2.0d * Math.PI * wheelRadius / TicksPerRevolution; double distanceLeft = dL.Value * wheelFactor; double distanceRight = dR.Value * wheelFactor; // Now, calculate the final angle, and use that to estimate // the final position. See Gary Lucas' paper for derivations // of the equations. double theta = _mapperVicinity.currentOdometryTheta + (distanceLeft - distanceRight) / bodyWidth; // radians _mapperVicinity.currentOdometryX += ((distanceRight + distanceLeft) / 2.0d) * Math.Cos(theta); // meters _mapperVicinity.currentOdometryY += ((distanceRight + distanceLeft) / 2.0d) * Math.Sin(theta); // meters _mapperVicinity.currentOdometryTheta = theta; #if TRACEDEBUGTICKS Tracer.Trace("****************************** DriveBehaviorServiceBase:: incrementOdometry() -- distance meters Left: " + distanceLeft + " Right: " + distanceRight + " heading degrees: " + Direction.to360(toDegrees(_mapperVicinity.currentOdometryTheta))); #endif // TRACEDEBUGTICKS if (_mapperVicinity.robotState.ignoreAhrs) { // do what compass data handlers usually do: proxibrick.DirectionDataDssSerializable newDir = new proxibrick.DirectionDataDssSerializable() { TimeStamp = DateTime.Now, heading = Direction.to360(toDegrees(_mapperVicinity.currentOdometryTheta)) }; setCurrentDirection(newDir); } }
/// <summary> /// Handle CH Robotics UM6 Orientation Sensor Notification - Quaternion /// </summary> /// <param name="notification">Quaternion notification</param> private void ChrQuaternionHandler(chrum6orientationsensor.QuaternionNotification notification) { //Tracer.Trace(string.Format("the UM6 Sensor reported Quaternion: {0} {1} {2} {3} {4}", notification.Body.LastUpdate, notification.Body.a, notification.Body.b, notification.Body.c, notification.Body.d)); if (!_mapperVicinity.robotState.ignoreAhrs && (USE_ORIENTATION_UM6 || USE_DIRECTION_UM6_QUATERNION)) { try { // we switch a and b here to match WPF quaternion's orientation. Maybe there is a proper transformation to do it, but this seems to work as well. Quaternion aq = new Quaternion(notification.Body.b, notification.Body.a, notification.Body.c, notification.Body.d); // X, Y, Z, W components in WPF correspond to a, b, c, d in CH UM6 and Wikipedia // have to turn it still around the Y axis (facing East): Vector3D axis = new Vector3D(0, 1, 0); aq = aq * new Quaternion(axis, 180); libguiwpf.OrientationData attitudeData = new libguiwpf.OrientationData() { timestamp = notification.Body.LastUpdate, attitudeQuaternion = aq }; if (USE_ORIENTATION_UM6) { setGuiCurrentAttitude(attitudeData); if (!_doUnitTest && !USE_DIRECTION_UM6_QUATERNION) { // do it now if direction is not taken from UM6 quaternion, otherwise let setCurrentDirection() call the Decide(): Decide(SensorEventSource.Orientation); } } if (USE_DIRECTION_UM6_QUATERNION) { // do what compass data handlers do: proxibrick.DirectionDataDssSerializable newDir = new proxibrick.DirectionDataDssSerializable() { TimeStamp = notification.Body.LastUpdate, heading = toDegrees(attitudeData.heading) + CHR_QUATERNION_YAW_TRUE_NORTH_OFFSET }; setCurrentDirection(newDir); } } catch (Exception exc) { Tracer.Trace("ChrQuaternionHandler() - " + exc); } } }
void trpbUpdateDirectionNotification(proxibrick.UpdateDirectionData update) { //LogInfo("DriveBehaviorServiceBase: trpbUpdateDirectionNotification()"); //Tracer.Trace("DriveBehaviorServiceBase: trpbUpdateDirectionNotification()"); if (USE_DIRECTION_PROXIBRICK) { try { proxibrick.DirectionDataDssSerializable newDir = update.Body; setCurrentDirection(newDir); } catch (Exception exc) { Tracer.Trace("trpbUpdateDirectionNotification() - " + exc); } } }
/// <summary> /// update GUI with current direction /// </summary> /// <param name="dir"></param> protected void setGuiCurrentDirection(proxibrick.DirectionDataDssSerializable dir) { if (_mainWindow != null) { ccrwpf.Invoke invoke = new ccrwpf.Invoke(delegate() { _mainWindow.CurrentDirection = new DirectionData() { TimeStamp = dir.TimeStamp.Ticks, heading = dir.heading, bearing = _mapperVicinity.robotDirection.bearing }; } ); wpfServicePort.Post(invoke); Arbiter.Activate(TaskQueue, invoke.ResponsePort.Choice( s => { }, // delegate for success ex => { } //Tracer.Error(ex) // delegate for failure )); } }
/// <summary> /// called when the AHRS/Compass reports new orientation. Preserves bearing to the goal. /// </summary> /// <param name="newDir"></param> protected void setCurrentDirection(proxibrick.DirectionDataDssSerializable newDir) { proxibrick.DirectionDataDssSerializable curDir = _state.MostRecentDirection; if (curDir == null || Math.Abs(newDir.heading - curDir.heading) > 0.9d || DateTime.Now > curDir.TimeStamp.AddSeconds(3.0d)) // only react on significant changes in direction { _state.MostRecentDirection = newDir; //.Clone() //Tracer.Trace("heading: " + newDir.heading); if (_mapperVicinity.turnState != null && !_mapperVicinity.turnState.hasFinished) { _mapperVicinity.turnState.directionCurrent = new Direction() { heading = newDir.heading, TimeStamp = newDir.TimeStamp.Ticks }; } // update mapper with Direction data: _mapperVicinity.robotDirection = new Direction() { TimeStamp = newDir.TimeStamp.Ticks, heading = newDir.heading, bearing = _mapperVicinity.robotDirection.bearing }; // set same bearing to the new Direction // update mapper with Odometry data: updateMapperWithOdometryData(); // update GUI (compass control): setGuiCurrentDirection(newDir); if (!_doUnitTest) { Decide(SensorEventSource.Compass); } } }
/// <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; } }