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