private void ConfigureInitialState() { //configure initial state if (_state == null) { _state = new DriveState(); _state.WheelRadius = 0.052; _state.DistanceBetweenWheels = 0.165; _state.LeftMotorPortNumber = 1; _state.RightMotorPortNumber = 3; _state.LeftEncoderPortNumber = 4; _state.RightEncoderPortNumber = 5; _state.ForwardSpeed = 1.0; _state.ReverseSpeed = 0.8; _state.RotateSpeed = 0.5; } }
public virtual IEnumerator <ITask> ReplaceHandler(Replace replace) { _state = replace.Body; replace.ResponsePort.Post(DefaultReplaceResponseType.Instance); yield break; }
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; }