public virtual IEnumerator <ITask> SetMotorPowerHandler(pxmotor.SetMotorPower update) { if (_state.HardwareIdentifier == 0) { throw new InvalidOperationException("TrackRoamer Motor configuration missing"); } powerbrick.MotorSpeed motorSpeed = new powerbrick.MotorSpeed(); // Default -1 which is ignored by the controller. motorSpeed.LeftSpeed = null; motorSpeed.RightSpeed = null; double power = update.Body.TargetPower * _state.PowerScalingFactor / 100.0d; // PowerScalingFactor is percentage, 0 to 100 if (power != 0.0d && Math.Abs(power) < MinimumMotorPower) { //string msg = string.Format("Warning: Motor {0} - requested power {1} less than minimum {2}", _state.HardwareIdentifier, power, MinimumMotorPower); //Tracer.Trace(msg); //LogWarning(msg); power = (power < 0) ? -MinimumMotorPower : MinimumMotorPower; } //Tracer.Trace(string.Format("Motor {0} - setting power to {1}", _state.HardwareIdentifier, power)); if (_state.HardwareIdentifier == 1) { motorSpeed.LeftSpeed = _state.ReversePolarity ? -power : power; } else if (_state.HardwareIdentifier == 2) { motorSpeed.RightSpeed = _state.ReversePolarity ? -power : power; } else { throw new ArgumentException("TrackRoamerMotorService : SetMotorPowerHandler() invalid HardwareIdentifier=" + _state.HardwareIdentifier); } _state.CurrentPower = power; coord.ActuatorCoordination coordination = update.GetHeader <coord.ActuatorCoordination>(); if (coordination == null) { _powerbrickPort.UpdateMotorSpeed(motorSpeed); } else { // TODO: Remove this extra code when we allow headers // to flow with a causality across services. // Pass on the coordination to the trackroamerbot powerbrick.UpdateMotorSpeed updateMotorSpeed = new powerbrick.UpdateMotorSpeed(motorSpeed); updateMotorSpeed.AddHeader(coordination); _powerbrickPort.Post(updateMotorSpeed); } update.ResponsePort.Post(DefaultUpdateResponseType.Instance); yield break; }
public IEnumerator <ITask> SetDrivePowerHandler(drive.SetDrivePower setDrivePower) { ValidateDriveConfiguration(false); _state.TimeStamp = DateTime.Now; PortSet <DefaultUpdateResponseType, Fault> responsePort = new PortSet <DefaultUpdateResponseType, Fault>(); // Add a coordination header to our motor requests // so that advanced motor implementations can // coordinate the individual motor reqests. coord.ActuatorCoordination coordination = new coord.ActuatorCoordination(); motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = setDrivePower.Body.LeftWheelPower }); leftPower.ResponsePort = responsePort; leftPower.AddHeader(coordination); _leftMotorPort.Post(leftPower); motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = setDrivePower.Body.RightWheelPower }); rightPower.ResponsePort = responsePort; rightPower.AddHeader(coordination); _rightMotorPort.Post(rightPower); // send notification to subscription manager Update update = new Update(_state); SendNotification <Update>(_subMgrPort, update); Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2, delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList) { if (successList.Count == 2) { setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType()); } foreach (Fault fault in failureList) { setDrivePower.ResponsePort.Post(fault); break; } })); yield break; }
public IEnumerator<ITask> SetDrivePowerHandler(drive.SetDrivePower setDrivePower) { ValidateDriveConfiguration(false); _state.TimeStamp = DateTime.Now; PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>(); // Add a coordination header to our motor requests // so that advanced motor implementations can // coordinate the individual motor reqests. coord.ActuatorCoordination coordination = new coord.ActuatorCoordination(); motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = setDrivePower.Body.LeftWheelPower } ); leftPower.ResponsePort = responsePort; leftPower.AddHeader(coordination); _leftMotorPort.Post(leftPower); motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = setDrivePower.Body.RightWheelPower } ); rightPower.ResponsePort = responsePort; rightPower.AddHeader(coordination); _rightMotorPort.Post(rightPower); // send notification to subscription manager Update update = new Update(_state); SendNotification<Update>(_subMgrPort, update); Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2, delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList) { if (successList.Count == 2) setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType()); foreach (Fault fault in failureList) { setDrivePower.ResponsePort.Post(fault); break; } })); yield break; }