public IEnumerator <ITask> SetDrivePowerHandler(drive.SetDrivePower setDrivePower) { if (setDrivePower.Body == null) { LogError("setDrivePower.Body == null"); setDrivePower.ResponsePort.Post(new Fault()); yield break; } double leftPower = setDrivePower.Body.LeftWheelPower; double rightPower = setDrivePower.Body.RightWheelPower; // Dump requests that come too fast. // but let stop messages through if (RequestPending > 1 && leftPower != 0 && rightPower != 0) { setDrivePower.ResponsePort.Post(DefaultUpdateResponseType.Instance); yield break; } RequestPending++; BoundsCheck(leftPower); BoundsCheck(rightPower); //update state _state.TimeStamp = DateTime.Now; _state.LeftWheel.MotorState.CurrentPower = leftPower; _state.RightWheel.MotorState.CurrentPower = rightPower; //convert to native units int leftPowerScaled = (int)Math.Round(leftPower * 100 + 100); int rightPowerScaled = (int)Math.Round(rightPower * 100 + 100); //send hardware specific motor data brick.SetMotorsBody motordata = new brick.SetMotorsBody(leftPowerScaled, rightPowerScaled); Activate(Arbiter.Choice(_scribblerPort.SetMotors(motordata), delegate(DefaultUpdateResponseType status) { setDrivePower.ResponsePort.Post(DefaultUpdateResponseType.Instance); RequestPending--; }, delegate(soap.Fault failure) { setDrivePower.ResponsePort.Post(failure); RequestPending--; } )); yield break; }
public IEnumerator<ITask> SetDrivePowerHandler(drive.SetDrivePower setDrivePower) { if (setDrivePower.Body == null) { LogError("setDrivePower.Body == null"); setDrivePower.ResponsePort.Post(new Fault()); yield break; } double leftPower = setDrivePower.Body.LeftWheelPower; double rightPower = setDrivePower.Body.RightWheelPower; // Dump requests that come too fast. // but let stop messages through if (RequestPending > 1 && leftPower != 0 && rightPower != 0) { setDrivePower.ResponsePort.Post(DefaultUpdateResponseType.Instance); yield break; } RequestPending++; BoundsCheck(leftPower); BoundsCheck(rightPower); //update state _state.TimeStamp = DateTime.Now; _state.LeftWheel.MotorState.CurrentPower = leftPower; _state.RightWheel.MotorState.CurrentPower = rightPower; //convert to native units int leftPowerScaled = (int)Math.Round(leftPower * 100 + 100); int rightPowerScaled = (int)Math.Round(rightPower * 100 + 100); //send hardware specific motor data brick.SetMotorsBody motordata = new brick.SetMotorsBody(leftPowerScaled, rightPowerScaled); Activate(Arbiter.Choice(_scribblerPort.SetMotors(motordata), delegate(DefaultUpdateResponseType status) { setDrivePower.ResponsePort.Post(DefaultUpdateResponseType.Instance); RequestPending--; }, delegate(Fault failure) { setDrivePower.ResponsePort.Post(failure); RequestPending--; } )); yield break; }