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