/// <summary>
/// Clone ActuatorCoordination
/// </summary>
/// <returns></returns>
public object Clone()
{
ActuatorCoordination dmc = new ActuatorCoordination();
dmc.RequestId = this.RequestId;
dmc.Count = this.Count;
return(dmc);
}
/// <summary>
/// CopyTo
/// </summary>
/// <param name="target"></param>
public void CopyTo(IDssSerializable target)
{
ActuatorCoordination typedTarget = target as ActuatorCoordination;
if (typedTarget == null)
{
throw new ArgumentException(string.Format("CopyTo({0}) requires type {0}", this.GetType().FullName));
}
typedTarget.Count = this.Count;
typedTarget.RequestId = this.RequestId;
}
public IEnumerator <ITask> SetDrivePowerHandler(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(2);
Motor.SetMotorPower leftPower = new Motor.SetMotorPower(setDrivePower.Body.LeftWheelPower);
leftPower.ResponsePort = responsePort;
leftPower.AddHeader(coordination);
_leftMotorPort.Post(leftPower);
Motor.SetMotorPower rightPower = new Motor.SetMotorPower(setDrivePower.Body.RightWheelPower);
rightPower.ResponsePort = responsePort;
rightPower.AddHeader(coordination);
_rightMotorPort.Post(rightPower);
// send notification to subscription manager
drive.Update update = new drive.Update(_state);
SendNotification <drive.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(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(2);
Motor.SetMotorPower leftPower = new Motor.SetMotorPower(setDrivePower.Body.LeftWheelPower);
leftPower.ResponsePort = responsePort;
leftPower.AddHeader(coordination);
_leftMotorPort.Post(leftPower);
Motor.SetMotorPower rightPower = new Motor.SetMotorPower(setDrivePower.Body.RightWheelPower);
rightPower.ResponsePort = responsePort;
rightPower.AddHeader(coordination);
_rightMotorPort.Post(rightPower);
// send notification to subscription manager
drive.Update update = new drive.Update(_state);
SendNotification<drive.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 virtual IEnumerator <ITask> UpdateMotorSpeedHandler(UpdateMotorSpeed update)
{
//LogInfo("TrackRoamerBrickPowerService:UpdateMotorSpeedHandler()");
coord.ActuatorCoordination coordination = update.GetHeader <coord.ActuatorCoordination>();
if (coordination != null && coordination.Count > 1)
{
if (!_coordinationList.ContainsKey(coordination.RequestId))
{
_coordinationList.Add(coordination.RequestId, new Port <UpdateMotorSpeed>());
Activate(
Arbiter.MultipleItemReceive <UpdateMotorSpeed>(
false,
_coordinationList[coordination.RequestId],
coordination.Count,
SetCoordinatedMotors));
}
_coordinationList[coordination.RequestId].Post(update);
yield break;
}
bool changed = ((update.Body.LeftSpeed >= 0 && _state.MotorSpeed.LeftSpeed != update.Body.LeftSpeed) ||
(update.Body.RightSpeed >= 0 && _state.MotorSpeed.RightSpeed != update.Body.RightSpeed));
if (update.Body.LeftSpeed != null)
{
if (update.Body.LeftSpeed >= -1.0 && update.Body.LeftSpeed <= 1.0)
{
//LogInfo("TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - LeftSpeed=" + update.Body.LeftSpeed + " requested");
_state.MotorSpeed.LeftSpeed = update.Body.LeftSpeed;
}
else
{
LogInfo("Error: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - invalid LeftSpeed=" + update.Body.LeftSpeed + " requested, must be between -1.0 and +1.0");
}
}
if (update.Body.RightSpeed != null)
{
if (update.Body.RightSpeed >= -1.0 && update.Body.RightSpeed <= 1.0)
{
//LogInfo("TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - RightSpeed=" + update.Body.RightSpeed + " requested");
_state.MotorSpeed.RightSpeed = update.Body.RightSpeed;
}
else
{
LogInfo("Error: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - invalid RightSpeed=" + update.Body.RightSpeed + " requested, must be between -1.0 and +1.0");
}
}
// If we are connected, send the speed to the robot wheels controller:
if (ensureHardwareController())
{
double?leftSpeed = _state.MotorSpeed.LeftSpeed;
double?rightSpeed = _state.MotorSpeed.RightSpeed;
Tracer.Trace("IP: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - speed L: " + leftSpeed + " R: " + rightSpeed);
// it will take time for upper layers to react on whiskers. We want to have some protection here, to avoid damage.
// Note: while moving forward the speeds are negative.
// cannot move forward if whiskers are pressed; replace it with backwards movement at half speed though:
if (leftSpeed < 0 && _state.Whiskers.FrontWhiskerLeft.GetValueOrDefault())
{
Tracer.Trace("Warning: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - left whisker pressed, speed " + leftSpeed + " reversed");
leftSpeed = -leftSpeed / 2;
}
if (rightSpeed < 0 && _state.Whiskers.FrontWhiskerRight.GetValueOrDefault())
{
Tracer.Trace("Warning: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - right whisker pressed, speed " + rightSpeed + " reversed");
rightSpeed = -rightSpeed / 2;
}
_brickConnection.SetSpeed(leftSpeed, rightSpeed);
}
// Send Notifications to subscribers
if (changed)
{
_subMgrPort.Post(new submgr.Submit(_state.MotorSpeed, DsspActions.UpdateRequest));
}
_state.TimeStamp = _state.MotorSpeed.Timestamp = DateTime.Now;
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
