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 virtual IEnumerator<ITask> MainAllStopHandler(motor.AllStop allStop) { DriveDistance drive = new DriveDistance(new SetDriveRequest()); drive.Body.LeftPower = 0.0; drive.Body.RightPower = 0.0; drive.Body.LeftStopAtRotationDegrees = 0; drive.Body.RightStopAtRotationDegrees = 0; drive.Body.StopState = allStop.Body.StopState; drive.ResponsePort = allStop.ResponsePort; drive.Body.DriveRequestOperation = pxdrive.DriveRequestOperation.AllStop; _internalDrivePowerPort.Post(drive); allStop.ResponsePort.Post(DefaultUpdateResponseType.Instance); yield break; }
public virtual IEnumerator<ITask> MainDriveDistanceHandler(DriveDistance driveDistance) { _internalDrivePowerPort.Post(driveDistance); driveDistance.ResponsePort.Post(DefaultUpdateResponseType.Instance); yield break; }
/// <summary> /// Notified subscribers a success or fault to completed Drive request. /// </summary> /// <param name="driveDistance"></param> /// <param name="success"></param> /// <param name="fault"></param> public void HandleDriveResponseForGenericOperationsNotifications(DriveDistance driveDistance, bool success, Fault fault) { if (fault == null) { if (driveDistance.Body.isGenericOperation == true) { //notify subscribers of generic drive distance -- complete switch (driveDistance.Body.DriveRequestOperation) { case pxdrive.DriveRequestOperation.DriveDistance: pxdrive.DriveDistanceRequest driveDistanceRequest = new pxdrive.DriveDistanceRequest(); driveDistanceRequest.DriveDistanceStage = pxdrive.DriveStage.Completed; pxdrive.DriveDistance driveDistanceUpdate = new pxdrive.DriveDistance(driveDistanceRequest); SendNotification<pxdrive.DriveDistance>(_genericSubMgrPort, driveDistanceUpdate); break; case pxdrive.DriveRequestOperation.RotateDegrees: pxdrive.RotateDegreesRequest rotateDegreesRequest = new pxdrive.RotateDegreesRequest(); rotateDegreesRequest.RotateDegreesStage = pxdrive.DriveStage.Completed; pxdrive.RotateDegrees rotateDegreesUpdate = new pxdrive.RotateDegrees(rotateDegreesRequest); SendNotification<pxdrive.RotateDegrees>(_genericSubMgrPort, rotateDegreesUpdate); break; } } else { // Operation canceled. driveDistance.Body.DriveDistanceStage = pxdrive.DriveStage.Canceled; SendNotification<pxdrive.SetDrivePower>(_genericSubMgrPort, driveDistance.Body); } _internalPendingDriveOperation = pxdrive.DriveRequestOperation.NotSpecified; } }
/// <summary> /// Process the most recent Drive Power command /// When complete, self activate for the next internal command /// </summary> /// <param name="driveDistance"></param> /// <returns></returns> public virtual IEnumerator<ITask> InternalDrivePowerHandler(DriveDistance driveDistance) { try { #region Check for a backlog of drive commands // Take a snapshot of the number of pending commands at the time // we entered this routine. // This will prevent a livelock which can occur if we try to // process the queue until it is empty, but the inbound queue // is growing at the same rate as we are pulling off the queue. int pendingCommands = _internalDrivePowerPort.ItemCount; // If newer commands have been issued, // respond success to the older command and // move to the newer one. DriveDistance newerUpdate; while (pendingCommands > 0) { if (_internalDrivePowerPort.Test(out newerUpdate)) { driveDistance = newerUpdate; } pendingCommands--; } #endregion // Get a snapshot of our state. DriveState currentState = _state.Clone(); #region Cancel any prior encoder target // If a prior encoder target was active, // signal that it is cancelled by a new Motor command. if (_targetEncoderPending[LEFT] || _targetEncoderPending[RIGHT]) { lock (_targetEncoderPending) { LogVerbose(LogGroups.Console, "Cancel prior target!"); if (_targetEncoderPending[LEFT]) { _targetEncoderReachedPort[LEFT].Post(false); _targetEncoderPending[LEFT] = false; } if (_targetEncoderPending[RIGHT]) { _targetEncoderReachedPort[RIGHT].Post(false); _targetEncoderPending[RIGHT] = false; } _state.RuntimeStatistics.LeftEncoderTarget = 0; _state.RuntimeStatistics.RightEncoderTarget = 0; } } #endregion // Calculate the motor power and target degrees for both motors. int leftMotorPower, rightMotorPower; long leftTargetEncoderDegrees, rightTargetEncoderDegrees; CalculatePowerAndTargetDegrees(driveDistance.Body, currentState, out leftMotorPower, out rightMotorPower, out leftTargetEncoderDegrees, out rightTargetEncoderDegrees); _state.RuntimeStatistics.LeftPowerCurrent = ((double)leftMotorPower) / 100.0; ; _state.RuntimeStatistics.LeftPowerTarget = driveDistance.Body.LeftPower; _state.RuntimeStatistics.RightPowerCurrent = ((double)rightMotorPower) / 100.0; _state.RuntimeStatistics.RightPowerTarget = driveDistance.Body.RightPower; LegoOutputMode leftOutputMode = (leftMotorPower == 0 || leftTargetEncoderDegrees != 0) ? LegoOutputMode.PowerBrake : LegoOutputMode.PowerRegulated; LegoOutputMode rightOutputMode = (rightMotorPower == 0 || rightTargetEncoderDegrees != 0) ? LegoOutputMode.PowerBrake : LegoOutputMode.PowerRegulated; if (leftTargetEncoderDegrees != 0) { // Calcuate a new target encoder which is based on the current encoder position. leftTargetEncoderDegrees += currentState.RuntimeStatistics.LeftEncoderCurrent; // A target of zero diables the PD control. Make sure our result isn't 0. if (leftTargetEncoderDegrees == 0) leftTargetEncoderDegrees++; _state.RuntimeStatistics.TargetStopState = driveDistance.Body.StopState; _targetEncoderPending[LEFT] = true; } _state.RuntimeStatistics.LeftEncoderTarget = leftTargetEncoderDegrees; if (rightTargetEncoderDegrees != 0) { // Calcuate a new target encoder which is based on the current encoder position. rightTargetEncoderDegrees += currentState.RuntimeStatistics.RightEncoderCurrent; // A target of zero diables the PD control. Make sure our result isn't 0. if (rightTargetEncoderDegrees == 0) rightTargetEncoderDegrees++; _state.RuntimeStatistics.TargetStopState = driveDistance.Body.StopState; _targetEncoderPending[RIGHT] = true; } _state.RuntimeStatistics.RightEncoderTarget = rightTargetEncoderDegrees; _state.TimeStamp = DateTime.Now; LegoRegulationMode syncMode = LegoRegulationMode.Individual; // Send the left motor command to the brick LegoSetOutputState motorCmd = new LegoSetOutputState( currentState.LeftWheel.MotorPort, leftMotorPower, leftOutputMode, syncMode, 0, // turnratio RunState.Constant, driveDistance.Body.LeftStopAtRotationDegrees); motorCmd.RequireResponse = false; LogVerbose(LogGroups.Console, motorCmd.ToString()); Fault fault = null; PortSet<LegoResponse, Fault> leftResponse = _legoBrickPort.SendNxtCommand(motorCmd); // Send the right motor comand to the brick motorCmd.MotorPort = currentState.RightWheel.MotorPort; motorCmd.PowerSetPoint = rightMotorPower; motorCmd.Mode = rightOutputMode; motorCmd.TurnRatio = 0; motorCmd.EncoderLimit = driveDistance.Body.RightStopAtRotationDegrees; LogVerbose(LogGroups.Console, motorCmd.ToString()); PortSet<LegoResponse, Fault> rightResponse = _legoBrickPort.SendNxtCommand(motorCmd); yield return Arbiter.Choice(leftResponse, delegate(LegoResponse response) { if (!response.Success) fault = Fault.FromException( new InvalidOperationException(response.ErrorCode.ToString())); }, delegate(Fault f) { fault = f; }); yield return Arbiter.Choice(rightResponse, delegate(LegoResponse response) { if (!response.Success) fault = Fault.FromException( new InvalidOperationException(response.ErrorCode.ToString())); }, delegate(Fault f) { fault = f; }); if (leftTargetEncoderDegrees != 0 && rightTargetEncoderDegrees != 0) { // Wait for the target encoders to either be cancelled or reach its target. Activate(Arbiter.MultiplePortReceive<bool>(false, _targetEncoderReachedPort, delegate(bool[] finished) { HandleDriveResponse(driveDistance, finished[LEFT] && finished[RIGHT], null); })); } else if (leftTargetEncoderDegrees != 0 || rightTargetEncoderDegrees != 0) { int ix = (leftTargetEncoderDegrees != 0) ? LEFT : RIGHT; // Wait for the target encoder to either be cancelled or reach its target. Activate(Arbiter.Receive<bool>(false, _targetEncoderReachedPort[ix], delegate(bool finished) { HandleDriveResponse(driveDistance, finished, null); })); } else { bool success = (fault == null); HandleDriveResponse(driveDistance, success, fault); } } finally { // Wait one time for the next InternalDrivePower command Activate(Arbiter.ReceiveWithIterator(false, _internalDrivePowerPort, InternalDrivePowerHandler)); } yield break; }
public virtual IEnumerator<ITask> GenericSetDriveSpeedHandler(pxdrive.SetDriveSpeed driveSpeed) { // set back response immediately or fault if drive is not enabled. ValidateDriveEnabledAndRespondHelper(driveSpeed.ResponsePort); DriveDistance drive = new DriveDistance(new SetDriveRequest()); drive.Body.LeftPower = driveSpeed.Body.LeftWheelSpeed; drive.Body.RightPower = driveSpeed.Body.RightWheelSpeed; drive.Body.LeftStopAtRotationDegrees = 0; drive.Body.RightStopAtRotationDegrees = 0; drive.Body.StopState = MotorStopState.Default; drive.Body.isGenericOperation = true; drive.ResponsePort = driveSpeed.ResponsePort; _internalDrivePowerPort.Post(drive); yield break; }
public virtual IEnumerator<ITask> GenericRotateDegreesHandler(pxdrive.RotateDegrees rotateDegrees) { if (_state.DistanceBetweenWheels <= 0) { rotateDegrees.ResponsePort.Post(Fault.FromException(new ArgumentOutOfRangeException("DistanceBetweenWheels must be specified in the Drive Configuration."))); yield break; } // set back response immediately or fault if drive is not enabled. ValidateDriveEnabledAndRespondHelper(rotateDegrees.ResponsePort); // distance = circumference / 360 * degreesToTurn double distance = Math.PI * _state.DistanceBetweenWheels * rotateDegrees.Body.Degrees / 360.0; // axleRotationDegrees = distance (meters) / wheelCircumference (pi * diameter) * 360 long axleRotationDegrees = (long)Math.Round(Math.Abs(distance) / (Math.PI * _state.LeftWheel.WheelDiameter) * 360.0); LogVerbose(LogGroups.Console, "RotateDegrees: Wheel Distance: " + distance.ToString() + " Axle Rotation Degrees: " + axleRotationDegrees.ToString()); double leftDirection = 1.0, rightDirection = 1.0; if (rotateDegrees.Body.Degrees > 0) leftDirection = -1.0; else rightDirection = -1.0; DriveDistance drive = new DriveDistance(new SetDriveRequest()); drive.Body.LeftStopAtRotationDegrees = axleRotationDegrees; drive.Body.RightStopAtRotationDegrees = axleRotationDegrees; drive.Body.StopState = MotorStopState.Brake; drive.Body.isGenericOperation = true; drive.Body.DriveRequestOperation = pxdrive.DriveRequestOperation.RotateDegrees; _internalPendingDriveOperation = pxdrive.DriveRequestOperation.RotateDegrees; bool synchronized = false; if (synchronized) { drive.Body.LeftPower = Math.Abs(rotateDegrees.Body.Power); drive.Body.RightPower = Math.Abs(rotateDegrees.Body.Power); } else { drive.Body.LeftPower = rotateDegrees.Body.Power * leftDirection; drive.Body.RightPower = rotateDegrees.Body.Power * rightDirection; } drive.ResponsePort = rotateDegrees.ResponsePort; // notify subscribers of rotate degrees start rotateDegrees.Body.RotateDegreesStage = pxdrive.DriveStage.Started; pxdrive.RotateDegrees rotateDegreesUpdate = new pxdrive.RotateDegrees(rotateDegrees.Body); SendNotification<pxdrive.RotateDegrees>(_genericSubMgrPort, rotateDegreesUpdate); _internalDrivePowerPort.Post(drive); yield break; }
public virtual IEnumerator<ITask> GenericDriveDistanceHandler(pxdrive.DriveDistance driveDistance) { double distance = driveDistance.Body.Distance; // set back response immediately or fault if drive is not enabled. ValidateDriveEnabledAndRespondHelper(driveDistance.ResponsePort); // rotations = distance (meters) / circumference (pi * diameter) // degrees = rotations * 360 double stopLeftWheelAtDegrees = Math.Round(Math.Abs(distance) / (Math.PI * _state.LeftWheel.WheelDiameter) * 360.0); double stopRightWheelAtDegrees = Math.Round(Math.Abs(distance) / (Math.PI * _state.RightWheel.WheelDiameter) * 360.0); DriveDistance drive = new DriveDistance(new SetDriveRequest()); drive.Body.LeftPower = driveDistance.Body.Power; drive.Body.RightPower = driveDistance.Body.Power; drive.Body.LeftStopAtRotationDegrees = (long)stopLeftWheelAtDegrees; drive.Body.RightStopAtRotationDegrees = (long)stopRightWheelAtDegrees; drive.Body.StopState = MotorStopState.Brake; drive.Body.isGenericOperation = true; drive.Body.DriveRequestOperation = pxdrive.DriveRequestOperation.DriveDistance; _internalPendingDriveOperation = pxdrive.DriveRequestOperation.DriveDistance; drive.ResponsePort = driveDistance.ResponsePort; // notify subscribers of drive distance start driveDistance.Body.DriveDistanceStage = pxdrive.DriveStage.Started; pxdrive.DriveDistance driveDistanceUpdate = new pxdrive.DriveDistance(driveDistance.Body); SendNotification<pxdrive.DriveDistance>(_genericSubMgrPort, driveDistanceUpdate); _internalDrivePowerPort.Post(drive); yield break; }
public virtual IEnumerator<ITask> GenericAllStopHandler(pxdrive.AllStop allStop) { DriveDistance drive = new DriveDistance(new SetDriveRequest()); drive.Body.LeftPower = 0.0; drive.Body.RightPower = 0.0; drive.Body.LeftStopAtRotationDegrees = 0; drive.Body.RightStopAtRotationDegrees = 0; drive.Body.StopState = MotorStopState.Brake; drive.ResponsePort = allStop.ResponsePort; drive.Body.DriveRequestOperation = pxdrive.DriveRequestOperation.AllStop; _internalDrivePowerPort.Post(drive); // disable drive _genericState.IsEnabled = false; pxdrive.EnableDrive disableDrive = new pxdrive.EnableDrive(); disableDrive.Body.Enable = _genericState.IsEnabled; _genericState.TimeStamp = DateTime.Now; _state.TimeStamp = _genericState.TimeStamp; SendNotification<pxdrive.EnableDrive>(_genericSubMgrPort, disableDrive); _drivePort.Post(disableDrive); yield break; }
/// <summary> /// Post a success or fault to complete a DriveDistance request. /// </summary> /// <param name="driveDistance"></param> /// <param name="success"></param> /// <param name="fault"></param> private void HandleDriveResponse(DriveDistance driveDistance, bool success, Fault fault) { if (success) { LogVerbose(LogGroups.Console, "InternalDrive Completed Successfully"); if (driveDistance.Body.isGenericOperation == true) { //notify subscribers of generic drive distance -- completed HandleDriveResponseForGenericOperationsNotifications(driveDistance, success, fault); } else { //notify subscribers of specific drive distance -- completed } } else { string msg; if (fault == null) { msg = "The Drive operation was canceled due to a newer drive command."; fault = Fault.FromException(new OperationCanceledException(msg)); } else { msg = fault.ToString(); } driveDistance.ResponsePort.Post(fault); LogVerbose(LogGroups.Console, msg); } }