/// <summary> /// Post Set Motor Power with body and return the response port. /// </summary> public virtual PortSet <DefaultUpdateResponseType, Fault> SetMotorRotation(pxmotor.SetMotorPowerRequest body) { pxmotor.SetMotorPower op = new pxmotor.SetMotorPower(); op.Body = body ?? new pxmotor.SetMotorPowerRequest(); this.PostUnknownType(op); return(op.ResponsePort); }
public virtual IEnumerator <ITask> SetMotorPowerHandler(pxmotor.SetMotorPower update) { if (_state.HardwareIdentifier == 0) { throw new InvalidOperationException("TrackRoamer Motor configuration missing"); } powerbrick.MotorSpeed motorSpeed = new powerbrick.MotorSpeed(); // Default -1 which is ignored by the controller. motorSpeed.LeftSpeed = null; motorSpeed.RightSpeed = null; double power = update.Body.TargetPower * _state.PowerScalingFactor / 100.0d; // PowerScalingFactor is percentage, 0 to 100 if (power != 0.0d && Math.Abs(power) < MinimumMotorPower) { //string msg = string.Format("Warning: Motor {0} - requested power {1} less than minimum {2}", _state.HardwareIdentifier, power, MinimumMotorPower); //Tracer.Trace(msg); //LogWarning(msg); power = (power < 0) ? -MinimumMotorPower : MinimumMotorPower; } //Tracer.Trace(string.Format("Motor {0} - setting power to {1}", _state.HardwareIdentifier, power)); if (_state.HardwareIdentifier == 1) { motorSpeed.LeftSpeed = _state.ReversePolarity ? -power : power; } else if (_state.HardwareIdentifier == 2) { motorSpeed.RightSpeed = _state.ReversePolarity ? -power : power; } else { throw new ArgumentException("TrackRoamerMotorService : SetMotorPowerHandler() invalid HardwareIdentifier=" + _state.HardwareIdentifier); } _state.CurrentPower = power; coord.ActuatorCoordination coordination = update.GetHeader <coord.ActuatorCoordination>(); if (coordination == null) { _powerbrickPort.UpdateMotorSpeed(motorSpeed); } else { // TODO: Remove this extra code when we allow headers // to flow with a causality across services. // Pass on the coordination to the trackroamerbot powerbrick.UpdateMotorSpeed updateMotorSpeed = new powerbrick.UpdateMotorSpeed(motorSpeed); updateMotorSpeed.AddHeader(coordination); _powerbrickPort.Post(updateMotorSpeed); } update.ResponsePort.Post(DefaultUpdateResponseType.Instance); yield break; }
void StopMotorWithEncoderHandler(encoder.EncoderOperations encoderNotificationPort, string side, encoder.UpdateTickCount update, motor.MotorOperations motorPort) { //LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " encoder at=" + update.Body.Count + " will stop wheel at=" + stopWheelAt); int stopWheelAt; bool ignore; switch (side) { case "left": stopWheelAt = stopLeftWheelAt; ignore = !_leftEncoderTickEnabled; break; default: case "right": stopWheelAt = stopRightWheelAt; ignore = !_rightEncoderTickEnabled; break; } if (!ignore && update.Body.Count >= stopWheelAt) { switch (side) { case "left": _leftEncoderTickEnabled = false; break; default: case "right": _rightEncoderTickEnabled = false; break; } // whatever else got stuck there, we are not interested. Keep the port clear. //Port<encoder.UpdateTickCount> port = (Port<encoder.UpdateTickCount>)encoderNotificationPort[typeof(encoder.UpdateTickCount)]; //port.Clear(); motor.SetMotorPower stop = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = 0 }); motorPort.Post(stop); Arbiter.Choice(stop.ResponsePort, delegate(DefaultUpdateResponseType resp) { LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - motor stopped !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); }, delegate(Fault fault) { LogError(fault); } ); LogInfo("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - Sending internal DriveStage.Completed !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); completionPort.Post(drive.DriveStage.Completed); } }
public IEnumerator <ITask> SetMotorPowerHandler(motor.SetMotorPower setMotorPower) { // Requests come too fast, so dump ones that come in too fast. if (RequestPending > 0) { setMotorPower.ResponsePort.Post(new DefaultUpdateResponseType()); yield break; } //flip direction if necessary double revPow = setMotorPower.Body.TargetPower; if (_state.ReversePolarity) { revPow *= -1.0; } //update state _state.CurrentPower = revPow; //convert to native units revPow *= 100; revPow += 100; int power = (int)Math.Round(revPow); if (power == 0) { power = 1; } //send hardware specific motor data brick.SetMotorData motordata = new brick.SetMotorData(); motordata.Motor = _state.Name; motordata.Speed = power; RequestPending++; Activate(Arbiter.Choice(_scribblerPort.SetMotor(motordata), delegate(DefaultUpdateResponseType status) { setMotorPower.ResponsePort.Post(DefaultUpdateResponseType.Instance); RequestPending--; }, delegate(soap.Fault failure) { setMotorPower.ResponsePort.Post(failure); RequestPending--; } )); yield break; }
public IEnumerator <ITask> SetDrivePowerHandler(drive.SetDrivePower setDrivePower) { ValidateDriveConfiguration(false); _state.TimeStamp = DateTime.Now; PortSet <DefaultUpdateResponseType, Fault> responsePort = new PortSet <DefaultUpdateResponseType, Fault>(); // Add a coordination header to our motor requests // so that advanced motor implementations can // coordinate the individual motor reqests. coord.ActuatorCoordination coordination = new coord.ActuatorCoordination(); motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = setDrivePower.Body.LeftWheelPower }); leftPower.ResponsePort = responsePort; leftPower.AddHeader(coordination); _leftMotorPort.Post(leftPower); motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = setDrivePower.Body.RightWheelPower }); rightPower.ResponsePort = responsePort; rightPower.AddHeader(coordination); _rightMotorPort.Post(rightPower); // send notification to subscription manager Update update = new Update(_state); SendNotification <Update>(_subMgrPort, update); Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2, delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList) { if (successList.Count == 2) { setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType()); } foreach (Fault fault in failureList) { setDrivePower.ResponsePort.Post(fault); break; } })); yield break; }
public IEnumerator <ITask> SetMotorPowerHandler(motor.SetMotorPower setMotorPower) { // For now, just dump requests that come in too fast. if (_requestPending > 0) { setMotorPower.ResponsePort.Post(new DefaultUpdateResponseType()); yield break; } //flip direction if nessisary double revPow = setMotorPower.Body.TargetPower; if (_state.ReversePolarity) { revPow = (-1) * revPow; } //update state _state.CurrentPower = revPow; //convert power int power = (int)Math.Round(revPow * _state.PowerScalingFactor); //send hardware specific motor data legoNXT.LegoSetOutputState motordata = new legoNXT.LegoSetOutputState(); motordata.OutputPort = _state.HardwareIdentifier - 1; motordata.PowerSetPoint = power; motordata.Mode = legoNXT.LegoOutputMode.PowerControl; motordata.RegulationMode = legoNXT.LegoRegulationMode.MotorSpeed; motordata.TurnRatio = 0; motordata.RunState = legoNXT.LegoRunState.Running; motordata.RotationLimit = 0; //run forever motordata.RequireResponse = false; _requestPending++; Activate(Arbiter.Choice(_legoPort.SendLegoCommand(motordata), delegate(legoNXT.LegoResponse status) { _requestPending--; }, delegate(Fault failure) { _requestPending--; } )); //reply to sender setMotorPower.ResponsePort.Post(DefaultUpdateResponseType.Instance); yield break; }
public virtual IEnumerator <ITask> SetMotorPowerHandler(motor.SetMotorPower update) { //Console.WriteLine("In set motor power handler .Got " + update.Body.TargetPower); if (_state.HardwareIdentifier == 0) { throw new InvalidOperationException("Surveyor SRV-1 Motor is not configured"); } _state.CurrentPower = update.Body.TargetPower; float srv1Power = EnsureMinimumPower(_state.CurrentPower); actuator.ActuatorValueRequest req = new actuator.ActuatorValueRequest(); // Leave the other motors unchanged req.activateA = false; req.activateB = false; switch (_state.HardwareIdentifier) { case 1: req.actuatorA = srv1Power; req.activateA = true; break; case 2: req.actuatorB = srv1Power; req.activateB = true; break; } // Post a message to the Surveyor SRV-1 Actuator //Console.WriteLine("==== Posting to actuator: " + req.actuatorA.Value + "," + req.actuatorB.Value); actuator.SetActuatorValues msg = new actuator.SetActuatorValues(req); _actuator.Post(msg); //_actuator.SetActuatorValues(req); yield return(Arbiter.Receive <DefaultUpdateResponseType>(false, msg.ResponsePort, delegate(DefaultUpdateResponseType resp){})); 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> SetDrivePowerHandler(drive.SetDrivePower setDrivePower) { ValidateDriveConfiguration(false); #if TRACELOG Tracer.Trace("TR Drive: SetDrivePowerHandler() =============================== TR handler ==================================== left=" + setDrivePower.Body.LeftWheelPower + " right=" + setDrivePower.Body.RightWheelPower); #endif cancelCurrentOperation(); _state.TimeStamp = DateTime.Now; PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>(); // Add a coordination header to our motor requests // so that advanced motor implementations can // coordinate the individual motor reqests. coord.ActuatorCoordination coordination = new coord.ActuatorCoordination(2); motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = setDrivePower.Body.LeftWheelPower }); leftPower.ResponsePort = responsePort; leftPower.AddHeader(coordination); _leftMotorPort.Post(leftPower); motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = setDrivePower.Body.RightWheelPower }); rightPower.ResponsePort = responsePort; rightPower.AddHeader(coordination); _rightMotorPort.Post(rightPower); // send notification to subscription manager drive.Update update = new drive.Update(_state); SendNotification<drive.Update>(_subMgrPort, update); Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2, delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList) { if (successList.Count == 2) setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType()); foreach (Fault fault in failureList) { setDrivePower.ResponsePort.Post(fault); break; } })); pollEncoderState(); yield break; }
void StopMotorWithEncoderHandler(Port<encoder.UpdateTickCount> encoderNotificationPort, string side, encoder.UpdateTickCount update, motor.MotorOperations motorPort) { int stopWheelAt; bool ignore; switch (side) { case "left": stopWheelAt = stopLeftWheelAt; ignore = !_leftEncoderTickEnabled; break; default: case "right": stopWheelAt = stopRightWheelAt; ignore = !_rightEncoderTickEnabled; break; } if (!ignore) { #if TRACEDEBUGTICKS Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " encoder at=" + update.Body.Count + " will stop wheel at=" + stopWheelAt); #endif // TRACEDEBUGTICKS if (update.Body.Count >= stopWheelAt) { switch (side) { case "left": _leftEncoderTickEnabled = false; break; default: case "right": _rightEncoderTickEnabled = false; break; } // whatever else got stuck there, we are not interested. Keep the port clear. //Port<encoder.UpdateTickCount> port = (Port<encoder.UpdateTickCount>)encoderNotificationPort[typeof(encoder.UpdateTickCount)]; //port.Clear(); motor.SetMotorPower stop = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = 0 }); motorPort.Post(stop); Activate(Arbiter.Choice(stop.ResponsePort, delegate(DefaultUpdateResponseType resp) { #if TRACEDEBUGTICKS Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - motor stopped by encoder !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); #endif // TRACEDEBUGTICKS }, delegate(Fault fault) { LogError(fault); } )); #if TRACEDEBUGTICKS Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: StopMotorWithEncoderHandler() " + side + " - Sending to completionPort: DriveStage.Completed !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); #endif // TRACEDEBUGTICKS completionPort.Post(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) { #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; }