/// <summary> /// just turn where the plan wants you to; we are calling with good plan (heading) and no legMeters. /// </summary> /// <param name="plan"></param> private void TurnToGoal(RoutePlan plan) { // at this point constraints and allowable speeds and distances are known and stored in _state.collisionState // we must use them while executing the plan. _state.NewHeading = (int)Math.Round((double)plan.bestHeadingRelative(_mapperVicinity)); setMovingStateDetail("TurnToGoal to NewHeading=" + _state.NewHeading); AdjustHeading(); }
/// <summary> /// see if current plan chain has a plan good enough to satisfy collisionState constraints; /// </summary> /// <param name="plan"></param> private RoutePlan critiquePlanChain(RoutePlan plan) { RoutePlan ret = plan; if (plan != null) { double?bestHeadingRelative = plan.bestHeadingRelative(_mapperVicinity); if (bestHeadingRelative.HasValue) { if (!_state.collisionState.canTurnLeft && bestHeadingRelative < 0 || !_state.collisionState.canTurnRight && bestHeadingRelative > 0) { Tracer.Trace("Unable to turn " + bestHeadingRelative + " degrees due to collisionState"); // TODO: see if any of plan.fallBackPlans can be used ret = null; } } } return(ret); // selected from initial plan chain }
/// <summary> /// evaluates proximity and parking sensor data and translates all of it into /// actionable restrictions in CollisionState object /// </summary> public void computeCollisionState(RoutePlan plan, double?intendedVelocity) { CollisionState cs = _state.collisionState; //cs.initRestrictive(); cs.initPermissive(FreeDistanceMm, MaximumForwardVelocityMmSec, MaximumBackwardVelocityMmSec); // initialize to the maximums: canMoveForwardDistanceM = FreeDistanceMm * 2.0d / 1000.0d; canMoveBackwardsDistanceM = FreeDistanceMm * 2.0d / 1000.0d; //Tracer.Trace("IR ffl=" + _state.MostRecentProximity.mfl + "m PS MetersLF=" + _state.MostRecentParkingSensor.parkingSensorMetersLF + "m"); if (intendedVelocity == null) { intendedVelocity = _state.Velocity; } bool isMovingForward = intendedVelocity > 0.01d; // m/s bool isMovingBackwards = intendedVelocity < -0.01d; int?distance = null; // mm if (plan != null && plan.isGoodPlan) { double?bestHeadingRelative = plan.bestHeadingRelative(_mapperVicinity); if (bestHeadingRelative != null && bestHeadingRelative.Value > -45.0d && bestHeadingRelative.Value < 45.0d) { distance = (int)(plan.legMeters.Value * 1000.0d); if (distance < ObstacleDistanceMm) { cs.canMoveByPlan = false; } else { cs.canMoveByPlan = true; } } } DateTime sensorInvalidateTime = DateTime.Now.AddSeconds(-ignoreOldSensorReadingsIntervalSec); // ignore sensor readings that are older than this time StringBuilder msb = new StringBuilder(); // build the message here // we compute full state every time, but format the message so it is relevant to the direction we move to. // forward sensors: ------------------------------------------------------------------------------------------- proxibrick.ProximityDataDssSerializable MostRecentProximity = _state.MostRecentProximity; proxibrick.ParkingSensorDataDssSerializable MostRecentParkingSensor = _state.MostRecentParkingSensor; if (MostRecentProximity != null && MostRecentProximity.TimeStamp > sensorInvalidateTime) { double mfl = MostRecentProximity.mfl; if (mfl < proxiTreshold) { if (isMovingForward) { msb.AppendFormat("Front Left Side proximity detected obstacle at {0}m; ", mfl); } cs.canTurnLeft &= mfl > proxiTresholdTurn1; if (mfl < canMoveForwardDistanceM) { canMoveForwardDistanceM = mfl; } } double mffl = MostRecentProximity.mffl; if (mffl < proxiTreshold) { if (isMovingForward) { msb.AppendFormat("FF_Left proximity detected obstacle at {0}m; ", mffl); } //cs.canTurnLeft &= mffl > proxiTresholdTurn2; if (mffl < canMoveForwardDistanceM) { canMoveForwardDistanceM = mffl; } } double mffr = MostRecentProximity.mffr; if (mffr < proxiTreshold) { if (isMovingForward) { msb.AppendFormat("FF_Right proximity detected obstacle at {0}m; ", mffr); } //cs.canTurnRight &= mffr > proxiTresholdTurn2; if (mffr < canMoveForwardDistanceM) { canMoveForwardDistanceM = mffr; } } double mfr = MostRecentProximity.mfr; if (mfr < proxiTreshold) { if (isMovingForward) { msb.AppendFormat("Front Right Side proximity detected obstacle at {0}m; ", mfr); } cs.canTurnRight &= mfr > proxiTresholdTurn1; if (mfr < canMoveForwardDistanceM) { canMoveForwardDistanceM = mfr; } } } if (MostRecentParkingSensor != null && MostRecentParkingSensor.TimeStamp > sensorInvalidateTime) { double parkingSensorMetersLF = MostRecentParkingSensor.parkingSensorMetersLF; if (parkingSensorMetersLF < psiTreshold) { if (isMovingForward) { msb.AppendFormat("Front Left parking sensor detected obstacle at {0}m; ", parkingSensorMetersLF); } if (parkingSensorMetersLF < canMoveForwardDistanceM) { canMoveForwardDistanceM = parkingSensorMetersLF; } } double parkingSensorMetersRF = MostRecentParkingSensor.parkingSensorMetersRF; if (parkingSensorMetersRF < psiTreshold) { if (isMovingForward) { msb.AppendFormat("Front Right parking sensor detected obstacle at {0}m; ", parkingSensorMetersRF); } if (parkingSensorMetersRF < canMoveForwardDistanceM) { canMoveForwardDistanceM = parkingSensorMetersRF; } } } // backward sensors: ------------------------------------------------------------------------------------------- if (MostRecentProximity != null && MostRecentProximity.TimeStamp > sensorInvalidateTime) { double mbl = MostRecentProximity.mbl; if (mbl < proxiTreshold) { if (isMovingBackwards) { msb.AppendFormat("Back Left Side proximity detected obstacle at {0}m; ", mbl); } cs.canTurnRight &= mbl > proxiTresholdTurn1; if (mbl < canMoveBackwardsDistanceM) { canMoveBackwardsDistanceM = mbl; } } double mbbl = MostRecentProximity.mbbl; if (mbbl < proxiTreshold) { if (isMovingBackwards) { msb.AppendFormat("BB_Left proximity detected obstacle at {0}m; ", mbbl); } //cs.canTurnRight &= mbbl > proxiTresholdTurn2; if (mbbl < canMoveBackwardsDistanceM) { canMoveBackwardsDistanceM = mbbl; } } double mbbr = MostRecentProximity.mbbr; if (mbbr < proxiTreshold) { if (isMovingBackwards) { msb.AppendFormat("BB_Right proximity detected obstacle at {0}m; ", mbbr); } //cs.canTurnLeft &= mbbr > proxiTresholdTurn2; if (mbbr < canMoveBackwardsDistanceM) { canMoveBackwardsDistanceM = mbbr; } } double mbr = MostRecentProximity.mbr; if (mbr < proxiTreshold) { if (isMovingBackwards) { msb.AppendFormat("Back Right Side proximity detected obstacle at {0}m; ", mbr); } cs.canTurnLeft &= mbr > proxiTresholdTurn1; if (mbr < canMoveBackwardsDistanceM) { canMoveBackwardsDistanceM = mbr; } } } if (MostRecentParkingSensor != null && MostRecentParkingSensor.TimeStamp > sensorInvalidateTime) { double parkingSensorMetersLB = MostRecentParkingSensor.parkingSensorMetersLB; if (parkingSensorMetersLB < psiTreshold) { if (isMovingBackwards) { msb.AppendFormat("Back Left parking sensor detected obstacle at {0}m; ", parkingSensorMetersLB); } if (parkingSensorMetersLB < canMoveBackwardsDistanceM) { canMoveBackwardsDistanceM = parkingSensorMetersLB; } } double parkingSensorMetersRB = MostRecentParkingSensor.parkingSensorMetersRB; if (parkingSensorMetersRB < psiTreshold) { if (isMovingBackwards) { msb.AppendFormat("Back Right parking sensor detected obstacle at {0}m; ", parkingSensorMetersRB); } if (parkingSensorMetersRB < canMoveBackwardsDistanceM) { canMoveBackwardsDistanceM = parkingSensorMetersRB; } } } if (_state.MostRecentWhiskerLeft) { msb.Append("Left Whisker; "); canMoveForwardDistanceM = 0.0d; cs.canMoveForward = false; cs.canTurnRight = false; cs.canTurnLeft = false; } if (_state.MostRecentWhiskerRight) { msb.Append("Right Whisker; "); canMoveForwardDistanceM = 0.0d; cs.canMoveForward = false; cs.canTurnRight = false; cs.canTurnLeft = false; } adjustNearCollisionSpeed(canMoveForwardDistanceM, true); adjustNearCollisionSpeed(canMoveBackwardsDistanceM, false); // compute the "mustStop" and "message": if (isMovingForward) { cs.mustStop = !cs.canMoveForward; } else if (isMovingBackwards) { cs.mustStop = !cs.canMoveBackwards; } if (msb.Length > 0 || cs.mustStop) { if (isMovingForward) { msb.Insert(0, "While moving forward: "); } else if (isMovingBackwards) { msb.Insert(0, "While moving backwards: "); } if (cs.mustStop) { msb.Insert(0, "!! MUST STOP !! "); } } else { if (isMovingForward) { msb.Append("Can continue forward: "); } else if (isMovingBackwards) { msb.Append("Can continue backwards: "); } else { msb.Append("Can move: "); } } msb.AppendFormat(" FWD: {0} to {1} mm at {2:F0} mm/s BKWD: {3} to {4} mm at {5:F0} mm/s LEFT:{6} RIGHT:{7}", cs.canMoveForward, cs.canMoveForwardDistanceMm, cs.canMoveForwardSpeedMms, cs.canMoveBackwards, cs.canMoveBackwardsDistanceMm, cs.canMoveBackwardsSpeedMms, cs.canTurnLeft, cs.canTurnRight); if (cs.canMoveByPlan.HasValue && !cs.canMoveByPlan.Value) { msb.AppendFormat("; best plan obstacle too close at {0} mm; ", distance); } string message = msb.ToString(); //if (!string.Equals(message, _lastMessage)) //{ // Tracer.Trace(message); // _lastMessage = message; //} cs.message = message; }
protected void GenerateTop(sicklrf.State laserData, int fieldOfView, RoutePlan plan) { //lock (lockStatusGraphics) //{ if (currentStatusGraphics != null) { bool haveLaser = laserData != null && laserData.DistanceMeasurements != null && (DateTime.Now - _laserData.TimeStamp).TotalSeconds < 2.0d; //Bitmap bmp = (_state.MovingState == MovingState.MapSouth) ? currentStatusGraphics.statusBmp : currentStatusGraphics.northBmp; //Bitmap bmp = currentStatusGraphics.statusBmp; Bitmap bmp = currentStatusGraphics.northBmp; lock (bmp) { using (Graphics g = Graphics.FromImage(bmp)) { g.Clear(Color.LightGray); List <Lbl> labels = new List <Lbl>(); if (haveLaser) { double angularOffset = -90 + laserData.AngularRange / 2.0; double piBy180 = Math.PI / 180.0; double halfAngle = laserData.AngularResolution / 2.0; double drangeMax = 0.0d; GraphicsPath path = new GraphicsPath(); // make two passes, drawing laser data and label every 20th range: for (int pass = 1; pass <= 2; pass++) { for (int i = 0; i < laserData.DistanceMeasurements.Length; i++) { int range = laserData.DistanceMeasurements[i]; if (range > 0 && range < 8192) { double angle = i * laserData.AngularResolution - angularOffset; double lowAngle = (angle - halfAngle) * piBy180; double highAngle = (angle + halfAngle) * piBy180; double drange = range * StatusGraphics.scale; float lx = (float)(StatusGraphics.xCenter + drange * Math.Cos(lowAngle)); float ly = (float)(StatusGraphics.xCenter - drange * Math.Sin(lowAngle)); float hx = (float)(StatusGraphics.xCenter + drange * Math.Cos(highAngle)); float hy = (float)(StatusGraphics.xCenter - drange * Math.Sin(highAngle)); if (pass == 1) { // on the first pass just add lines to the Path and calculate the max range: if (i == 0) { path.AddLine(StatusGraphics.xCenter, StatusGraphics.imageHeight, lx, ly); } path.AddLine(lx, ly, hx, hy); drangeMax = Math.Max(drangeMax, drange); } else { // on the second pass draw the perimeter and label every 20th range: g.DrawLine(Pens.DarkBlue, lx, ly, hx, hy); if (i > 0 && i % 20 == 0 && i < laserData.DistanceMeasurements.Length - 10) { float llx = (float)(StatusGraphics.xCenter + drangeMax * 1.3f * Math.Cos(lowAngle)); float lly = (float)(StatusGraphics.xCenter - drangeMax * 1.3f * Math.Sin(lowAngle)); double roundRange = Math.Round(range / 1000.0d, 1); // meters string str = "" + roundRange; labels.Add(new Lbl() { label = str, lx = llx, ly = lly, brush = Brushes.Black }); } } } } if (pass == 1) { // draw the laser sweep on the first pass: g.FillPath(Brushes.White, path); } } } // draw important decision-influencing boundaries: float startAngle = -150.0f; float sweepAngle = 120.0f; // the "stop moving" distance: float radius = (float)(ObstacleDistanceMm * StatusGraphics.scale); //g.DrawRectangle(Pens.Red, xCenter - radius, imageHeight - radius, radius * 2, radius * 2); g.DrawArc(Pens.Red, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, startAngle, sweepAngle); rayLabel("stop", labels, ObstacleDistanceMm, -60.0d, Brushes.Red); // the "slow down" distance: radius = (float)(AwareOfObstacleDistanceMm * StatusGraphics.scale); g.DrawArc(Pens.Orange, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, startAngle, sweepAngle); rayLabel("slow", labels, AwareOfObstacleDistanceMm, -60.0d, Brushes.Orange); // the "stop mapping, enter the open space" distance radius = (float)(SafeDistanceMm * StatusGraphics.scale); g.DrawArc(Pens.LightBlue, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, startAngle, sweepAngle); rayLabel("enter", labels, SafeDistanceMm, -60.0d, Brushes.LightBlue); // the "max velocity" distance: radius = (float)(FreeDistanceMm * StatusGraphics.scale); g.DrawArc(Pens.Green, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, startAngle, sweepAngle); rayLabel("free", labels, FreeDistanceMm, -60.0d, Brushes.Green); // the fieldOfView arc: radius = (float)(StatusGraphics.imageHeight - 10); g.DrawArc(Pens.LimeGreen, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, (float)(-90 - fieldOfView), (float)(fieldOfView * 2)); rayLabel("field of view", labels, (StatusGraphics.imageHeight + 5) / StatusGraphics.scale, 10.0d, Brushes.LimeGreen); // now draw the robot. Trackroamer is 680 mm wide. float botHalfWidth = (float)(680 / 2.0d * StatusGraphics.scale); DrawHelper.drawRobotBoundaries(g, botHalfWidth, StatusGraphics.imageWidth / 2, StatusGraphics.imageHeight); // debug- draw a ray pointing to predefined direction; left is positive, right is negative: //double rayLength = 2000.0d; //rayLine("20", g, rayLength, 20.0d, Pens.Cyan, Brushes.DarkCyan); //rayLine("-20", g, rayLength, -20.0d, Pens.Blue, Brushes.Blue); //rayLine("70", g, rayLength, 70.0d, Pens.Red, Brushes.Red); //rayLine("-70", g, rayLength, -70.0d, Pens.Green, Brushes.Green); // draw laser's time stamp: if (haveLaser) { TimeSpan howOld = DateTime.Now - laserData.TimeStamp; g.DrawString(laserData.TimeStamp.ToString() + " (" + howOld + ")", StatusGraphics.fontBmp, Brushes.Black, 0, 0); } HistoryItem latestDecisionsHistory = StatusGraphics.HistoryDecisions.Peek(); HistoryItem latestSaidHistory = Talker.HistorySaid.Peek(); if (latestSaidHistory != null) { g.DrawString(latestSaidHistory.message, StatusGraphics.fontBmpL, Brushes.Black, StatusGraphics.imageWidth / 2 + 80, StatusGraphics.imageHeight + StatusGraphics.extraHeight - 20); } if (latestDecisionsHistory != null) { g.DrawString(latestDecisionsHistory.message, StatusGraphics.fontBmpL, Brushes.Black, StatusGraphics.imageWidth / 2 + 80, StatusGraphics.imageHeight + StatusGraphics.extraHeight - 40); } g.DrawString(_state.MovingState.ToString(), StatusGraphics.fontBmpL, Brushes.Black, StatusGraphics.imageWidth / 2 - 40, StatusGraphics.imageHeight + StatusGraphics.extraHeight - 20); // draw distance labels over all other graphics: foreach (Lbl lbl in labels) { g.DrawString(lbl.label, StatusGraphics.fontBmp, lbl.brush, lbl.lx, lbl.ly + 20); } // a 200W x 400H rectangle: Rectangle drawRect = new Rectangle(StatusGraphics.imageWidth / 2 - StatusGraphics.extraHeight, StatusGraphics.imageHeight - StatusGraphics.extraHeight * 2, StatusGraphics.extraHeight * 2, StatusGraphics.extraHeight * 4); if (_state.MostRecentProximity != null) { // the MostRecentProximity class here comes from the proxy, and does not have arrangedForDrawing member. Restore it: double[] arrangedForDrawing = new double[8]; arrangedForDrawing[0] = _state.MostRecentProximity.mbr; arrangedForDrawing[1] = _state.MostRecentProximity.mbbr; arrangedForDrawing[2] = _state.MostRecentProximity.mbbl; arrangedForDrawing[3] = _state.MostRecentProximity.mbl; arrangedForDrawing[4] = _state.MostRecentProximity.mfr; arrangedForDrawing[5] = _state.MostRecentProximity.mffr; arrangedForDrawing[6] = _state.MostRecentProximity.mffl; arrangedForDrawing[7] = _state.MostRecentProximity.mfl; // draw a 200x400 image of IR proximity sensors: DrawHelper.drawProximityVectors(g, drawRect, arrangedForDrawing, 1); } if (_state.MostRecentParkingSensor != null) { // the MostRecentParkingSensor class here comes from the proxy, and does not have arrangedForDrawing member. Restore it: double[] arrangedForDrawing = new double[4]; arrangedForDrawing[0] = _state.MostRecentParkingSensor.parkingSensorMetersRB; arrangedForDrawing[1] = _state.MostRecentParkingSensor.parkingSensorMetersLB; arrangedForDrawing[2] = _state.MostRecentParkingSensor.parkingSensorMetersLF; arrangedForDrawing[3] = _state.MostRecentParkingSensor.parkingSensorMetersRF; // draw a 200x400 image of parking sensors: DrawHelper.drawProximityVectors(g, drawRect, arrangedForDrawing, 2); } if (plan != null && plan.isGoodPlan) { // right turn - positive, left turn - negative, expressed in degrees: int bestHeadingInt = (int)Math.Round((double)plan.bestHeadingRelative(_mapperVicinity)); double bestHeadingDistance = plan.legMeters.Value * 1000.0d; using (Pen dirPen = new Pen(Color.Green, 5.0f)) { dirPen.EndCap = LineCap.ArrowAnchor; rayLine("best:" + bestHeadingInt, g, bestHeadingDistance * 1.1d, (double)bestHeadingInt, dirPen, Brushes.Green); } //int xLbl = 10; //int yLbl = bmp.Height - 40; //g.DrawString(comment, StatusGraphics.fontBmpL, Brushes.Green, xLbl, yLbl); } if (_mapperVicinity.robotDirection.bearingRelative.HasValue) { int goalBearingInt = (int)Math.Round((double)_mapperVicinity.robotDirection.bearingRelative); double goalDistance = 3000.0d; using (Pen dirPen = new Pen(Color.LimeGreen, 2.0f)) { dirPen.EndCap = LineCap.ArrowAnchor; rayLine("goal:" + goalBearingInt, g, goalDistance, (double)goalBearingInt, dirPen, Brushes.LimeGreen); } } } } } }
/// <summary> /// Transitions to "Mapping" meta state or "AdjustHeading" state depending on /// environment. /// </summary> /// <param name="distance">closest obstacle in corridor ahead</param> private void MetaStateStartMapping(RoutePlan plan) { int distance = 0; if (plan.legMeters.HasValue) { distance = (int)((double)plan.legMeters * 1000.0d); } LogInfo("TrackRoamerBehaviorsService: MetaStateStartMapping(distance=" + distance + ") ObstacleDistance=" + ObstacleDistanceMm); Talker.Say(5, "Mapping"); markNorthBitmap("Start Mapping"); StopMoving(); // at this point constraints and allowable speeds and distances are known and stored in _state.collisionState // we must use them while executing the plan. if (distance < ObstacleDistanceMm) { LogInfo(" distance < ObstacleDistance (not finding exit) _state.Mapped=" + _state.Mapped); _state.MovingState = MovingState.Recovering; LogInfo(" MovingState Recovering (not finding exit)"); //Talker.Say(5, "Recovering"); setMovingStateDetail("Recovering (not finding exit)"); /* * if (_state.Mapped) * { * // We have been mapping before but do not seem to * // have found anything. * _state.MovingState = MovingState.RandomTurn; * LogInfo(" requesting RandomTurn"); * Talker.Say(5, "random turn"); * setMovingStateDetail("Start Mapping (not finding exit) - requesting RandomTurn"); * } * else * { * _state.MovingState = MovingState.MapSurroundings; * LogInfo(" requesting MapSurroundings"); * Talker.Say(5, "map surroundings"); * setMovingStateDetail("Start Mapping (not finding exit) - requesting MapSurroundings"); * } * */ } else { LogInfo(" distance > ObstacleDistance (exit route found) CorridorWidthMapping=" + CorridorWidthMappingMm); //Talker.Say(5, "exit route found"); int legMm = Math.Min(ObstacleDistanceMm, distance); _state.NewHeading = (int)plan.bestHeadingRelative(_mapperVicinity); // right - positive, left - negative LogInfo("Leg: " + legMm + " mm NewHeading=" + _state.NewHeading); setMovingStateDetail("Start Mapping (exit route found) NewHeading=" + _state.NewHeading + "Leg: " + legMm + " mm"); _state.MovingState = MovingState.AdjustHeading; _state.Countdown = legMm / 50 + Math.Abs(_state.NewHeading / 10); LogInfo(" new Countdown=" + _state.Countdown); } }
/// <summary> /// Adjusts the velocity based on environment. /// </summary> /// <param name="distance">closest obstacle in corridor ahead</param> private void AdjustDirectionAndVelocity(RoutePlan plan) { LogInfo("TrackRoamerBehaviorsService: AdjustDirectionAndVelocity()"); // at this point constraints and allowable speeds and distances are known and stored in _state.collisionState // we must use them while executing the plan. int legDistanceMm = _state.collisionState.canMoveForwardDistanceMm; if (plan.legMeters.HasValue) { legDistanceMm = Math.Min(legDistanceMm, (int)((double)plan.legMeters * 1000.0d)); } // we come here when _state.MovingState == MovingState.FreeForwards _state.Mapped = false; //int bestHeading = FindBestHeading(_laserData, 0, (int)Math.Round(_state.Velocity * 1000.0d / 10.0d), CorridorWidthMoving, "FreeForwards - Adjust"); _state.NewHeading = (int)plan.bestHeadingRelative(_mapperVicinity); // right - positive, left - negative LogInfo(" bestHeading=" + _state.NewHeading + currentCompass); if (legDistanceMm > FreeDistanceMm) { SpawnIterator <TurnAndMoveParameters, Handler>( new TurnAndMoveParameters() { rotateAngle = (int)(((double)_state.NewHeading) * turnHeadingFactor), rotatePower = MaximumTurnPower, speed = (int)Math.Round(Math.Min(_state.collisionState.canMoveForwardSpeedMms, MaximumForwardVelocityMmSec)), desiredMovingState = MovingState.FreeForwards }, delegate() { }, TurnAndMoveForward); } else if (legDistanceMm > AwareOfObstacleDistanceMm) { SpawnIterator <TurnAndMoveParameters, Handler>( new TurnAndMoveParameters() { rotateAngle = (int)(((double)_state.NewHeading) * turnHeadingFactor), rotatePower = ModerateTurnPower, speed = (int)Math.Round(Math.Min(_state.collisionState.canMoveForwardSpeedMms, ModerateForwardVelocityMmSec)), desiredMovingState = MovingState.FreeForwards }, delegate() { }, TurnAndMoveForward); } else { SpawnIterator <TurnAndMoveParameters, Handler>( new TurnAndMoveParameters() { rotateAngle = (int)(((double)_state.NewHeading) * turnHeadingFactor), rotatePower = MinimumTurnPower, speed = (int)Math.Round(Math.Min(_state.collisionState.canMoveForwardSpeedMms, MinimumForwardVelocityMmSec)), desiredMovingState = MovingState.FreeForwards }, delegate() { _state.Countdown = Math.Abs(_state.NewHeading / 10); }, TurnAndMoveForward); } LogInfo(" new Countdown=" + _state.Countdown); }