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