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