/// <summary> /// Internal drive distance operation handler /// </summary> /// <param name="driveDistance"></param> /// <returns></returns> public virtual IEnumerator <ITask> InternalDriveDistanceHandler(drive.DriveDistance driveDistance) { switch (driveDistance.Body.DriveDistanceStage) { case drive.DriveStage.InitialRequest: _state.InternalPendingDriveOperation = drive.DriveRequestOperation.DriveDistance; SpawnIterator <double, double>(driveDistance.Body.Distance, driveDistance.Body.Power, DriveUntilDistance); break; case drive.DriveStage.Started: SendNotification <drive.DriveDistance>(_subMgrPort, driveDistance.Body); break; case drive.DriveStage.Completed: _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; SendNotification <drive.DriveDistance>(_subMgrPort, driveDistance.Body); break; case drive.DriveStage.Canceled: _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; SendNotification <drive.DriveDistance>(_subMgrPort, driveDistance.Body); break; } yield break; }
public void DriveDriveDistanceHandler(drive.DriveDistance drivedistance) { this.controllerDrivePort.Post( new drive.DriveDistance() { Body = drivedistance.Body, ResponsePort = drivedistance.ResponsePort }); }
public IEnumerator <ITask> DriveDriveDistanceHandler(drive.DriveDistance drivedistance) { if (!this.state.DriveState.IsEnabled) { drivedistance.ResponsePort.Post(soap.Fault.FromCodeSubcodeReason( soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Drive not enabled!")); yield break; } this.state.DriveState.TimeStamp = DateTime.Now; short pwr = (short)(board.HBridgeReverseMax + ((drivedistance.Body.Power - GDDriveMin) * DrivePowerScale)); short dist = (short)(this.encoderTicksPerMeter[(int)Sides.Left] * drivedistance.Body.Distance); byte[] cmdpacket = board.CreatePacket <short, short>(board.SetTravelDistanceString, dist, pwr); serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest(); sg.Timeout = this.state.DefaultResponsePause; sg.Data = new serialcomservice.Packet(cmdpacket); var resultPort = this.serialCOMServicePort.SendAndGet(sg); yield return(resultPort.Choice()); soap.Fault f = (soap.Fault)resultPort; if (f != null) { LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message))); drivedistance.ResponsePort.Post(f); yield break; } if (this.HasFWError((serialcomservice.Packet)resultPort)) { f = soap.Fault.FromCodeSubcodeReason( soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!"); drivedistance.ResponsePort.Post(f); yield break; } this.state.DriveState.LeftWheel.MotorState.CurrentPower = drivedistance.Body.Power; this.state.DriveState.RightWheel.MotorState.CurrentPower = drivedistance.Body.Power; this.state.DriveState.DriveDistanceStage = drive.DriveStage.Completed; drivedistance.ResponsePort.Post(DefaultUpdateResponseType.Instance); }
public IEnumerator <ITask> DriveDistanceHandler(DriveDistance driveDistance) { // If configuration is invalid, an InvalidException is thrown. ValidateDriveConfiguration(true); _state.TimeStamp = DateTime.Now; // send immediate response driveDistance.ResponsePort.Post(DefaultUpdateResponseType.Instance); // post request to internal port. _internalDriveOperationsPort.Post(driveDistance); yield break; }
/// <summary> /// drives a specified number of meters /// </summary> IEnumerator <ITask> DriveUntilDistance(double distance, double power) { //reset encoders encoder.Reset Lreset = new encoder.Reset(); _leftEncoderPort.Post(Lreset); yield return(Arbiter.Choice(Lreset.ResponsePort, delegate(DefaultUpdateResponseType response) { }, delegate(Fault fault) { LogError(fault); } )); encoder.Reset Rreset = new encoder.Reset(); _rightEncoderPort.Post(Rreset); yield return(Arbiter.Choice(Rreset.ResponsePort, delegate(DefaultUpdateResponseType response) { }, delegate(Fault fault) { LogError(fault); } )); //compute tick to stop at int stopLeftWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution)); int stopRightWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution)); // Subscribe to the encoders on internal ports encoder.EncoderOperations leftNotificationPort = new encoder.EncoderOperations(); encoder.EncoderOperations rightNotificationPort = new encoder.EncoderOperations(); encoder.Subscribe op = new encoder.Subscribe(); op.Body = new SubscribeRequestType(); op.NotificationPort = leftNotificationPort; _leftEncoderPort.Post(op); yield return(Arbiter.Choice(op.ResponsePort, delegate(SubscribeResponseType response) { //subscription was successful, start listening for encoder replace messages Activate(Arbiter.Receive <encoder.UpdateTickCount>(false, leftNotificationPort, delegate(encoder.UpdateTickCount update) { StopMotorWithEncoderHandler(leftNotificationPort, update, _leftMotorPort, stopLeftWheelAt); })); }, delegate(Fault fault) { LogError(fault); } )); encoder.Subscribe op2 = new encoder.Subscribe(); op2.Body = new SubscribeRequestType(); op2.NotificationPort = rightNotificationPort; _leftEncoderPort.Post(op2); yield return(Arbiter.Choice(op2.ResponsePort, delegate(SubscribeResponseType response) { //subscription was successful, start listening for encoder replace messages Activate(Arbiter.Receive <encoder.UpdateTickCount>(false, rightNotificationPort, delegate(encoder.UpdateTickCount update) { StopMotorWithEncoderHandler(rightNotificationPort, update, _rightMotorPort, stopRightWheelAt); } )); }, delegate(Fault fault) { LogError(fault); } )); //start moving double Pow; if (distance > 0) { Pow = power; } else { Pow = -power; } PortSet <DefaultUpdateResponseType, Fault> responsePort = new PortSet <DefaultUpdateResponseType, Fault>(); // send notification of driveDistance start to subscription manager _state.DriveDistanceStage = DriveStage.Started; DriveDistance driveUpdate = new DriveDistance(); driveUpdate.Body.DriveDistanceStage = DriveStage.Started; _internalDriveOperationsPort.Post(driveUpdate); Motor.SetMotorPower leftPower = new Motor.SetMotorPower(Pow); leftPower.ResponsePort = responsePort; _leftMotorPort.Post(leftPower); Motor.SetMotorPower rightPower = new Motor.SetMotorPower(Pow); rightPower.ResponsePort = responsePort; _rightMotorPort.Post(rightPower); Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2, delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList) { foreach (Fault fault in failureList) { LogError(fault); } } )); // send notification of driveDistance complete to subscription manager driveUpdate.Body.DriveDistanceStage = DriveStage.Completed; _internalDriveOperationsPort.Post(driveUpdate); _state.DriveDistanceStage = DriveStage.Completed; }
public IEnumerator <ITask> DriveDistanceHandler(drive.DriveDistance driveDistance) { LogError("Drive distance is not implemented"); yield break; }
public void DriveDistanceHandler(diffdrive.DriveDistance driveDistance) { if (this.driveEntity == null) { throw new InvalidOperationException("Simulation entity not registered with service"); } if (!this.state.DriveState.IsEnabled) { driveDistance.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled."))); LogError("DriveDistance request to disabled drive."); return; } 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."); return; } this.state.DriveState.DriveDistanceStage = driveDistance.Body.DriveDistanceStage; if (driveDistance.Body.DriveDistanceStage == diffdrive.DriveStage.InitialRequest) { var entityResponse = new Port <engine.OperationResult>(); Activate( Arbiter.Receive( false, entityResponse, result => { // post a message to ourselves indicating that the drive distance has completed var req = new diffdrive.DriveDistanceRequest(0, 0); switch (result) { case engine.OperationResult.Error: req.DriveDistanceStage = diffdrive.DriveStage.Canceled; break; case engine.OperationResult.Canceled: req.DriveDistanceStage = diffdrive.DriveStage.Canceled; break; case engine.OperationResult.Completed: req.DriveDistanceStage = diffdrive.DriveStage.Completed; break; } this.drivePort.Post(new diffdrive.DriveDistance(req)); })); this.driveEntity.DriveDistance( (float)driveDistance.Body.Distance, (float)driveDistance.Body.Power, entityResponse); var req2 = new diffdrive.DriveDistanceRequest(0, 0) { DriveDistanceStage = diffdrive.DriveStage.Started }; this.drivePort.Post(new diffdrive.DriveDistance(req2)); } else { SendNotification(this.subMgrPort, driveDistance); } driveDistance.ResponsePort.Post(DefaultUpdateResponseType.Instance); }