public override void SetMotors(float leftPower, float rightPower)
{
if (!motorsOn)
{
EnableMotors();
}
drive.SetDrivePowerRequest drivePowerReq = new drive.SetDrivePowerRequest();
drivePowerReq.LeftWheelPower = leftPower;
drivePowerReq.RightWheelPower = rightPower;
drive.SetDrivePower setDrivePower = new drive.SetDrivePower(drivePowerReq);
drivePort.Post(setDrivePower);
bool done = false;
Arbiter.Activate(DssEnvironment.TaskQueue,
Arbiter.Receive <DefaultUpdateResponseType>(false,
setDrivePower.ResponsePort,
delegate(DefaultUpdateResponseType state)
{
done = true;
}
));
while (!done)
{
;
}
}
public IEnumerator <ITask> DriveDistanceHandler(diffdrive.DriveDistance driveDistance)
{
if (_entity == null)
{
throw new InvalidOperationException("Simulation entity not registered with service");
}
if (!_state.IsEnabled)
{
driveDistance.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("DriveDistance request to disabled drive.");
yield break;
}
if ((driveDistance.Body.Power > 1.0f) || (driveDistance.Body.Power < -1.0f))
{
// invalid drive power
driveDistance.ResponsePort.Post(Fault.FromException(new Exception("Invalid Power parameter.")));
LogError("Invalid Power parameter in DriveDistanceHandler.");
yield break;
}
_state.DriveDistanceStage = driveDistance.Body.DriveDistanceStage;
if (driveDistance.Body.DriveDistanceStage == diffdrive.DriveStage.InitialRequest)
{
Port <simengine.OperationResult> entityResponse = new Port <simengine.OperationResult>();
Activate(Arbiter.Receive <simengine.OperationResult>(false, entityResponse, delegate(simengine.OperationResult result)
{
// post a message to ourselves indicating that the drive distance has completed
diffdrive.DriveDistanceRequest req = new diffdrive.DriveDistanceRequest(0, 0);
switch (result)
{
case simengine.OperationResult.Error:
req.DriveDistanceStage = diffdrive.DriveStage.Canceled;
break;
case simengine.OperationResult.Canceled:
req.DriveDistanceStage = diffdrive.DriveStage.Canceled;
break;
case simengine.OperationResult.Completed:
req.DriveDistanceStage = diffdrive.DriveStage.Completed;
break;
}
_mainPort.Post(new diffdrive.DriveDistance(req));
}));
_entity.DriveDistance((float)driveDistance.Body.Distance, (float)driveDistance.Body.Power, entityResponse);
diffdrive.DriveDistanceRequest req2 = new diffdrive.DriveDistanceRequest(0, 0);
req2.DriveDistanceStage = diffdrive.DriveStage.Started;
_mainPort.Post(new diffdrive.DriveDistance(req2));
}
else
{
base.SendNotification(_subMgrPort, driveDistance);
}
driveDistance.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
public IEnumerator <ITask> AllStopHandler(drive.AllStop allStop)
{
drive.SetDrivePower zeroPower = new drive.SetDrivePower();
zeroPower.Body = new drive.SetDrivePowerRequest(0.0, 0.0);
zeroPower.ResponsePort = allStop.ResponsePort;
_mainPort.Post(zeroPower);
yield break;
}
IEnumerator <ITask> OnConnectMotorHandler(OnConnectMotor onConnectMotor)
{
if (onConnectMotor.DriveControl == _driveControl)
{
drive.EnableDriveRequest request = new drive.EnableDriveRequest(false);
if (_drivePort != null)
{
yield return(Arbiter.Choice(
_drivePort.EnableDrive(request),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault f)
{
LogError(f);
}
));
if (_motorShutdown != null)
{
PerformShutdown(ref _motorShutdown);
}
}
_drivePort = ServiceForwarder <drive.DriveOperations>(onConnectMotor.Service);
_motorShutdown = new Port <Shutdown>();
drive.ReliableSubscribe subscribe = new drive.ReliableSubscribe(
new ReliableSubscribeRequestType(10)
);
subscribe.NotificationPort = _driveNotify;
subscribe.NotificationShutdownPort = _motorShutdown;
_drivePort.Post(subscribe);
yield return(Arbiter.Choice(
subscribe.ResponsePort,
delegate(SubscribeResponseType response)
{
LogInfo("Subscribed to " + onConnectMotor.Service);
},
delegate(Fault fault)
{
_motorShutdown = null;
LogError(fault);
}
));
request = new drive.EnableDriveRequest(true);
yield return(Arbiter.Choice(
_drivePort.EnableDrive(request),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault f)
{
LogError(f);
}
));
}
}
IEnumerator<ITask> OnConnectMotorHandler(OnConnectMotor onConnectMotor)
{
if (onConnectMotor.DriveControl == _driveControl)
{
drive.EnableDriveRequest request = new drive.EnableDriveRequest();
if (_drivePort != null)
{
yield return Arbiter.Choice(
_drivePort.EnableDrive(request),
delegate(DefaultUpdateResponseType response)
{
},
delegate(Fault f)
{
LogError(f);
}
);
if (_motorShutdown != null)
{
yield return PerformShutdown(ref _motorShutdown);
}
}
_drivePort = ServiceForwarder<drive.DriveOperations>(onConnectMotor.Service);
_motorShutdown = new Port<Shutdown>();
request.Enable = true;
yield return Arbiter.Choice(
_drivePort.EnableDrive(request),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault f)
{
LogError(f);
}
);
drive.ReliableSubscribe subscribe = new drive.ReliableSubscribe(
new ReliableSubscribeRequestType(10)
);
subscribe.NotificationPort = _driveNotify;
subscribe.NotificationShutdownPort = _motorShutdown;
_drivePort.Post(subscribe);
yield return Arbiter.Choice(
subscribe.ResponsePort,
delegate(SubscribeResponseType response)
{
LogInfo("Subscribed to " + onConnectMotor.Service);
},
delegate(Fault fault)
{
_motorShutdown = null;
LogError(fault);
}
);
}
}
