public IEnumerator <ITask> ConfigureDrive(DriveState state) { PartnerType leftMotorPartner = this.ServiceInfo.PartnerList.Find(delegate(PartnerType partner) { return(partner.Name.Name == "LeftMotor"); }); PartnerType rightMotorPartner = this.ServiceInfo.PartnerList.Find(delegate(PartnerType partner) { return(partner.Name.Name == "RightMotor"); }); // Configure the left motor connection motorbase.MotorState Lconfigure = new motorbase.MotorState(); Lconfigure.HardwareIdentifier = _state.LeftMotorPortNumber; Lconfigure.Name = "Left Motor"; Lconfigure.PowerScalingFactor = 1; yield return(Arbiter.Choice(_leftMotorPort.Replace(Lconfigure), delegate(DefaultReplaceResponseType success) { LogInfo("Left Motor Port set"); }, delegate(Fault fault) { LogError(fault); _initFault = true; })); if (_initFault) { yield break; } // Configure the right motor connection motorbase.MotorState Rconfigure = new motorbase.MotorState(); Rconfigure.HardwareIdentifier = _state.RightMotorPortNumber; Rconfigure.Name = "Right Motor"; Rconfigure.PowerScalingFactor = 1; yield return(Arbiter.Choice(_rightMotorPort.Replace(Rconfigure), delegate(DefaultReplaceResponseType success) { LogInfo("Right Motor Port set"); }, delegate(Fault fault) { LogError(fault); _initFault = true; })); if (_initFault) { yield break; } // Configure the drive service drivesrv.DriveDifferentialTwoWheelState driveState = new drivesrv.DriveDifferentialTwoWheelState(); driveState.DistanceBetweenWheels = state.DistanceBetweenWheels; driveState.TimeStamp = DateTime.Now; driveState.LeftWheel = new motorbase.WheeledMotorState(); driveState.LeftWheel.Radius = _state.WheelRadius; driveState.LeftWheel.Name = "Left Wheel"; driveState.LeftWheel.MotorState = new motorbase.MotorState(); driveState.LeftWheel.GearRatio = 1.0; driveState.LeftWheel.EncoderState = new encoderbase.EncoderState(); driveState.RightWheel = new motorbase.WheeledMotorState(); driveState.RightWheel.Radius = _state.WheelRadius; driveState.RightWheel.Name = "Right Wheel"; driveState.RightWheel.MotorState = new motorbase.MotorState(); driveState.RightWheel.GearRatio = 1.0; driveState.RightWheel.EncoderState = new encoderbase.EncoderState(); driveState.IsEnabled = true; yield return(Arbiter.Choice(_drivePort.Update(driveState), delegate(DefaultUpdateResponseType success) { LogInfo("Drive configuration updated"); }, delegate(Fault f) { LogError(f); _initFault = true; })); if (_initFault) { yield break; } drivesrv.EnableDriveRequest en = new drivesrv.EnableDriveRequest(); en.Enable = true; yield return(Arbiter.Receive <DefaultUpdateResponseType>(false, _drivePort.EnableDrive(en), delegate(DefaultUpdateResponseType response){})); yield break; }