/// <summary>
/// Handles DriveUpdate request
/// </summary>
/// <param name="update">request</param>
void DriveUpdateHandler(DriveUpdate update)
{
//LogInfo("****************************** DriveBehaviorServiceBase:: DriveUpdateHandler: state.IsEnabled=" + update.Body.IsEnabled);
_state.DriveState = update.Body;
//_state.Velocity = (VelocityFromWheel(update.Body.LeftWheel) + VelocityFromWheel(update.Body.RightWheel)) / 2;
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
/// <summary>
/// Handles DriveUpdate request
/// </summary>
/// <param name="update">request</param>
void DriveUpdateHandler(DriveUpdate update)
{
_state.DriveState = update.Body;
_state.Velocity = (VelocityFromWheel(update.Body.LeftWheel) + VelocityFromWheel(update.Body.RightWheel)) / 2;
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
/// <summary>
/// Handles DriveUpdate request
/// </summary>
/// <param name="update">request</param>
void DriveUpdateHandler(DriveUpdate update)
{
DriveStateUpdate(update.Body);
/*
float w = update.Body.LeftWheel.MotorState.Pose.Orientation.W;
_state.DriveState = update.Body;
_state.Velocity = (VelocityFromWheel(update.Body.LeftWheel) + VelocityFromWheel(update.Body.RightWheel)) / 2;
// Make sure that the pose is up to date
// NOTE: This is a FUDGE! Also, it only gets updated when something
// changes, not continuously, so it is usually OUT OF DATE!
_state.X = (update.Body.LeftWheel.MotorState.Pose.Position.X +
update.Body.RightWheel.MotorState.Pose.Position.X) / 2;
_state.Y = (update.Body.LeftWheel.MotorState.Pose.Position.Z +
update.Body.RightWheel.MotorState.Pose.Position.Z) / 2;
_state.Theta = (float) (Math.Acos(w)*2*180/Math.PI);
if (update.Body.LeftWheel.MotorState.Pose.Orientation.Y < 0)
_state.Theta = -_state.Theta;
*/
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
