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