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