public IEnumerator <ITask> ResetHandler(pxencoder.Reset update) { LogInfo("TrackRoamerEncoder : ResetHandler() m_lastResetTicks=" + m_lastResetTicks); // reset physical counter in the bot AX2850 controller: //trackroamerbot.ResetMotorEncoder resetMotorEncoder = new trackroamerbot.ResetMotorEncoder(); //resetMotorEncoder.Body.HardwareIdentifier = _state.HardwareIdentifier; //_trackroamerbotPort.Post(resetMotorEncoder); // initialize our local logical counter: lock (m_lastResetTicksLock) { _state.TicksSinceReset = 0; _state.CurrentReading = 0; _state.CurrentAngle = 0.0d; m_lastResetTicks = null; } update.ResponsePort.Post(DefaultUpdateResponseType.Instance); yield break; }
/// <summary> /// drives a specified number of meters /// </summary> IEnumerator <ITask> DriveUntilDistance(double distance, double power) { LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance(distance=" + distance + " meters, power=" + power + ")"); _leftEncoderTickEnabled = false; _rightEncoderTickEnabled = false; //reset encoders encoder.Reset Lreset = new encoder.Reset(); _leftEncoderCmdPort.Post(Lreset); yield return(Arbiter.Choice(Lreset.ResponsePort, delegate(DefaultUpdateResponseType response) { }, delegate(Fault fault) { LogError(fault); } )); encoder.Reset Rreset = new encoder.Reset(); _rightEncoderCmdPort.Post(Rreset); yield return(Arbiter.Choice(Rreset.ResponsePort, delegate(DefaultUpdateResponseType response) { }, delegate(Fault fault) { LogError(fault); } )); //compute tick to stop at stopLeftWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution)); stopRightWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution)); _leftEncoderTickEnabled = true; _rightEncoderTickEnabled = true; //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 = drive.DriveStage.Started; drive.DriveDistance driveUpdate = new drive.DriveDistance(); driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Started; _internalDriveOperationsPort.Post(driveUpdate); motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = Pow }); leftPower.ResponsePort = responsePort; _leftMotorPort.Post(leftPower); motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = Pow }); rightPower.ResponsePort = responsePort; _rightMotorPort.Post(rightPower); LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() start moving: distance=" + distance + " meters"); LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() will stop wheel at: Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt); Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2, delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList) { foreach (Fault fault in failureList) { LogError(fault); } } )); LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() calling WaitForCompletion() - waiting for the first side to complete..."); yield return(WaitForCompletion()); LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() calling WaitForCompletion() - other side should complete too..."); yield return(WaitForCompletion()); LogInfo("=============== TrackRoamerDriveService:: DriveUntilDistance() - both sides completed, send notification of driveDistance complete to subscription manager"); // send notification of driveDistance complete to subscription manager driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Completed; _internalDriveOperationsPort.Post(driveUpdate); _state.DriveDistanceStage = drive.DriveStage.Completed; }
/// <summary> /// Rotate the the drive (positive degrees turn counterclockwise) /// </summary> /// <param name="degrees">(positive degrees turn counterclockwise)</param> /// <param name="power">(-1.0 to 1.0)</param> IEnumerator <ITask> RotateUntilDegrees(double degrees, double power) { LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: RotateUntilDegrees(degrees=" + degrees + ", power=" + power + ")"); _leftEncoderTickEnabled = false; _rightEncoderTickEnabled = false; //reset encoders encoder.Reset Lreset = new encoder.Reset(); _leftEncoderCmdPort.Post(Lreset); yield return(Arbiter.Choice(Lreset.ResponsePort, delegate(DefaultUpdateResponseType response) { }, delegate(Fault fault) { LogError(fault); } )); encoder.Reset Rreset = new encoder.Reset(); _rightEncoderCmdPort.Post(Rreset); yield return(Arbiter.Choice(Rreset.ResponsePort, delegate(DefaultUpdateResponseType response) { }, delegate(Fault fault) { LogError(fault); } )); double arcDistance = Math.Abs(degrees) * _state.DistanceBetweenWheels * Math.PI / 360.0d; //compute tick to stop at stopLeftWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution)); stopRightWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution)); _leftEncoderTickEnabled = true; _rightEncoderTickEnabled = true; //start moving // start rotate operation _state.RotateDegreesStage = drive.DriveStage.Started; drive.RotateDegrees rotateUpdate = new drive.RotateDegrees(); rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Started; _internalDriveOperationsPort.Post(rotateUpdate); PortSet <DefaultUpdateResponseType, Fault> responsePort = new PortSet <DefaultUpdateResponseType, Fault>(); double rightPow; double leftPow; if (degrees > 0) { rightPow = power; leftPow = -power; } else { rightPow = -power; leftPow = power; } motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = leftPow }); leftPower.ResponsePort = responsePort; _leftMotorPort.Post(leftPower); motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = rightPow }); rightPower.ResponsePort = responsePort; _rightMotorPort.Post(rightPower); LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() start moving: degrees=" + degrees); LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() will stop wheels at: Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt); Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2, delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList) { foreach (Fault fault in failureList) { LogError(fault); } } )); LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() calling WaitForCompletion() - waiting for the first side to complete..."); yield return(WaitForCompletion()); LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() calling WaitForCompletion() - other side should complete too..."); yield return(WaitForCompletion()); LogInfo("=============== TrackRoamerDriveService:: RotateUntilDegrees() - both sides completed, send notification of RotateDegrees complete to subscription manager"); // send notification of RotateDegrees complete to subscription manager rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Completed; _internalDriveOperationsPort.Post(rotateUpdate); _state.RotateDegreesStage = drive.DriveStage.Completed; }
/// <summary> /// drives a specified number of meters /// </summary> IEnumerator<ITask> DriveUntilDistance(double distance, double power) { #if TRACELOG Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance(distance=" + distance + " meters, power=" + power + ")"); #endif EncoderTicksEnabled = false; //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 stopLeftWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution)); stopRightWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution)); EncoderTicksEnabled = true; pollEncoderState(); // get starting encoder state //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 = drive.DriveStage.Started; drive.DriveDistance driveUpdate = new drive.DriveDistance(); driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Started; #if TRACELOG Tracer.Trace("++++++++++++++++++ DRIVE: DriveUntilDistance() DriveStage.Started"); #endif _internalDriveOperationsPort.Post(driveUpdate); motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = Pow }); leftPower.ResponsePort = responsePort; _leftMotorPort.Post(leftPower); motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = Pow }); rightPower.ResponsePort = responsePort; _rightMotorPort.Post(rightPower); #if TRACELOG Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() start moving: distance=" + distance + " meters"); Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() will stop wheels at: Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt); #endif Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2, delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList) { foreach (Fault fault in failureList) { LogError(fault); } } )); #if TRACELOG Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() calling DriveWaitForCompletionDual() - waiting for both sides to complete..."); #endif yield return DriveWaitForCompletionDual(); // send notification of driveDistance complete to subscription manager driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Completed; #if TRACELOG Tracer.Trace("++++++++++++++++++ DRIVE: DriveUntilDistance() DriveStage.Completed"); #endif _internalDriveOperationsPort.Post(driveUpdate); _state.DriveDistanceStage = drive.DriveStage.Completed; }
public IEnumerator<ITask> ResetEncodersHandler(drive.ResetEncoders resetEncoders) { encoder.Reset rightResetRequest = null; encoder.Reset leftResetRequest = null; if (this._rightEncoderPort != null) { rightResetRequest = new encoder.Reset(); this._rightEncoderPort.Post(rightResetRequest); } if (this._leftEncoderPort != null) { leftResetRequest = new encoder.Reset(); this._leftEncoderPort.Post(leftResetRequest); } if (rightResetRequest != null) { yield return rightResetRequest.ResponsePort.Choice(EmptyHandler, EmptyHandler); } if (leftResetRequest != null) { yield return leftResetRequest.ResponsePort.Choice(EmptyHandler, EmptyHandler); } this.SendNotification<drive.ResetEncoders>(this._subMgrPort, resetEncoders.Body); resetEncoders.ResponsePort.Post(new DefaultUpdateResponseType()); yield break; }
/// <summary> /// Rotate the the drive (positive degrees turn counterclockwise) /// </summary> /// <param name="degrees">(positive degrees turn counterclockwise)</param> /// <param name="power">(-1.0 to 1.0)</param> IEnumerator<ITask> RotateUntilDegrees(double degrees, double power) { #if TRACELOG Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: RotateUntilDegrees(degrees=" + degrees + ", power=" + power + ")"); #endif EncoderTicksEnabled = false; //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); } )); double arcDistance = Math.Abs(degrees) * _state.DistanceBetweenWheels * Math.PI / 360.0d; //compute tick to stop at stopLeftWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution)); stopRightWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution)); EncoderTicksEnabled = true; pollEncoderState(); // get starting encoder state //start moving // start rotate operation _state.RotateDegreesStage = drive.DriveStage.Started; drive.RotateDegrees rotateUpdate = new drive.RotateDegrees(); rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Started; #if TRACELOG Tracer.Trace("++++++++++++++++++ DRIVE: RotateUntilDegrees() DriveStage.Started"); #endif _internalDriveOperationsPort.Post(rotateUpdate); PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>(); double rightPow; double leftPow; if (degrees > 0) { rightPow = power; leftPow = -power; } else { rightPow = -power; leftPow = power; } motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = leftPow }); leftPower.ResponsePort = responsePort; _leftMotorPort.Post(leftPower); motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = rightPow }); rightPower.ResponsePort = responsePort; _rightMotorPort.Post(rightPower); #if TRACELOG Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() start moving: degrees=" + degrees); Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() will stop wheels at: Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt); #endif Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2, delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList) { foreach (Fault fault in failureList) { LogError(fault); } } )); #if TRACELOG Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() calling DriveWaitForCompletionDual() - waiting for both sides to complete..."); #endif yield return DriveWaitForCompletionDual(); #if TRACELOG Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() - both sides completed, send notification of RotateDegrees complete to subscription manager"); #endif // send notification of RotateDegrees complete to subscription manager rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Completed; #if TRACELOG Tracer.Trace("++++++++++++++++++ DRIVE: RotateUntilDegrees() DriveStage.Completed"); #endif _internalDriveOperationsPort.Post(rotateUpdate); _state.RotateDegreesStage = drive.DriveStage.Completed; }
public IEnumerator <ITask> ResetHandler(encoder.Reset reset) { _state.TicksSinceReset = 0; reset.ResponsePort.Post(DefaultUpdateResponseType.Instance); yield break; }