private void Statusparser_OnParsedSOP() { if (EnableTraceMode) { if (GPSSpotManager.GPSPositions.Count == 0) { MoveFront(0); return; } IGConsole.Instance.Main.println(CurrentMoveSequence.ToString()); GPSsPosition MyPos = /*new GPSsPosition(0, 0);//*/ GPSCore.Instance.GetGPSsPosition(); GPSsPosition TargetPos = GPSSpotManager.GPSPositions[GPSSpotManager.CurrentMovePosIndex].GetGPSS(); float x = Mathf.Abs(MyPos.longitude - TargetPos.longitude); float y = Mathf.Abs(MyPos.latitude - TargetPos.latitude); float Distance = Mathf.Sqrt(x * x + y * y); float ReqAngleFromN = CalcReqAngleFromN(MyPos, TargetPos); if (Distance < 0.00006) { GPSSpotManager.MoveNext(); return; } switch (CurrentMoveSequence) { case MoveSequence.ForcedForward: #region 1-1 if (RotateToReqAngle(ReqAngleFromN) == false) { if (carDivice.rh_sonardist > 10 && carDivice.lh_sonardist > 10) { MoveBack(255); } else if (carDivice.rh_sonardist > 10) { RotationL(255); } else if (carDivice.lh_sonardist > 10) { RotationR(255); } else if (carDivice.f_sonardist < 30) { RBlocked = false; LBlocked = false; rotationSaved = false; CurrentMoveSequence = MoveSequence.Rotat90To2n2; goto case MoveSequence.Rotat90To2n2; } else if (carDivice.rs_sonardist < 20) { TraceDraction = Direction.Right; CurrentMoveSequence = MoveSequence.SemiAutomaticForward; goto case MoveSequence.SemiAutomaticForward; } else if (carDivice.ls_sonardist < 20) { TraceDraction = Direction.Left; CurrentMoveSequence = MoveSequence.SemiAutomaticForward; goto case MoveSequence.SemiAutomaticForward; } else { MoveFront(255); } } break; #endregion case MoveSequence.SemiAutomaticForward: #region 1-2 BetweenAngle = GetBetweenTargetAngle(ReqAngleFromN); if (BetweenAngle > 60) { Direction direction = GetRotateTargetDerection(ReqAngleFromN); if (direction == Direction.Right && TraceDraction == Direction.Left) { CurrentMoveSequence = MoveSequence.ForcedForward; goto case MoveSequence.ForcedForward; } else if (direction == Direction.Left && TraceDraction == Direction.Right) { CurrentMoveSequence = MoveSequence.ForcedForward; goto case MoveSequence.ForcedForward; } if (BetweenAngle > 80) { if (direction == Direction.Right && TraceDraction == Direction.Right) { RBlocked = true; LBlocked = false; MoveFrontTickCount = 100; CurrentMoveFrontCount = 0; //TraceDraction = TraceDraction; CurrentMoveSequence = MoveSequence.ProgressiveLengthMeasure; ProgressiveLengthMeasureSubSequence = ProgressiveLengthMeasureSubSequence.Front; goto case MoveSequence.ProgressiveLengthMeasure; } else if (direction == Direction.Left && TraceDraction == Direction.Left) { RBlocked = false; LBlocked = true; MoveFrontTickCount = 100; CurrentMoveFrontCount = 0; //TraceDraction = TraceDraction; CurrentMoveSequence = MoveSequence.ProgressiveLengthMeasure; ProgressiveLengthMeasureSubSequence = ProgressiveLengthMeasureSubSequence.Front; goto case MoveSequence.ProgressiveLengthMeasure; } } } else if (carDivice.f_sonardist < 30) ////////////////////////// { if (TraceDraction == Direction.Right) { RBlocked = true; LBlocked = false; } else { RBlocked = false; LBlocked = true; } rotationSaved = false; CurrentMoveSequence = MoveSequence.Rotat90To2n2; goto case MoveSequence.Rotat90To2n2; } else { DriveWall(TraceDraction); } break; #endregion case MoveSequence.Rotat90To2n2: #region 2-1 if (rotationSaved == false) //reqrotation 설정 { rotationSaved = true; if (RBlocked == true) { reqrotation = (CompassCore.Instance.AngleFromN - 90) % 360; TraceDraction = Direction.Right; } else if (LBlocked == true) { reqrotation = (CompassCore.Instance.AngleFromN + 90) % 360; TraceDraction = Direction.Left; } else { Direction direction = GetRotateTargetDerection(ReqAngleFromN); if (direction == Direction.Right) { reqrotation = (CompassCore.Instance.AngleFromN + 90) % 360; TraceDraction = Direction.Left; } else //왼쪽에 ReqAngleFromN존재 { reqrotation = (CompassCore.Instance.AngleFromN - 90) % 360; TraceDraction = Direction.Right; } } } if (RotateToReqAngle(reqrotation) == false) { //RBlocked = RBlocked; //LBlocked = LBlocked; MoveFrontTickCount = 100; CurrentMoveFrontCount = 0; //TraceDraction = TraceDraction; CurrentMoveSequence = MoveSequence.ProgressiveLengthMeasure; ProgressiveLengthMeasureSubSequence = ProgressiveLengthMeasureSubSequence.Front; goto case MoveSequence.ProgressiveLengthMeasure; } break; #endregion case MoveSequence.ProgressiveLengthMeasure: #region 2-2 BetweenAngle = GetBetweenTargetAngle(ReqAngleFromN); if (BetweenAngle > 130 && ProgressiveLengthMeasureSubSequence != ProgressiveLengthMeasureSubSequence.Rotate180) { //TraceDraction = TraceDraction; TurnBack = false; CurrentMoveSequence = MoveSequence.RetractionMeasure; goto case MoveSequence.RetractionMeasure; } else if (BetweenAngle < 70 && ProgressiveLengthMeasureSubSequence != ProgressiveLengthMeasureSubSequence.Rotate180) { //TraceDraction = TraceDraction; CurrentMoveSequence = MoveSequence.SemiAutomaticForward; goto case MoveSequence.SemiAutomaticForward; } else { switch (ProgressiveLengthMeasureSubSequence) { case ProgressiveLengthMeasureSubSequence.Front: if (carDivice.f_sonardist < 30) { if (TraceDraction == Direction.Right) { LBlocked = true; if (!RBlocked) { rotationSaved = false; ProgressiveLengthMeasureSubSequence = ProgressiveLengthMeasureSubSequence.Rotate180; goto case ProgressiveLengthMeasureSubSequence.Rotate180; } } else { RBlocked = true; if (!LBlocked) { rotationSaved = false; ProgressiveLengthMeasureSubSequence = ProgressiveLengthMeasureSubSequence.Rotate180; goto case ProgressiveLengthMeasureSubSequence.Rotate180; } } if (LBlocked && RBlocked) { rotationSaved = false; ProgressiveLengthMeasureSubSequence = ProgressiveLengthMeasureSubSequence.Rotate90; goto case ProgressiveLengthMeasureSubSequence.Rotate90; } } else if (MoveFrontTickCount == CurrentMoveFrontCount) { rotationSaved = false; ProgressiveLengthMeasureSubSequence = ProgressiveLengthMeasureSubSequence.Rotate180; goto case ProgressiveLengthMeasureSubSequence.Rotate180; } else { CurrentMoveFrontCount++; DriveWall(TraceDraction); } break; case ProgressiveLengthMeasureSubSequence.Rotate180: if (rotationSaved == false) { rotationSaved = true; reqrotation = (CompassCore.Instance.AngleFromN + 180) % 360; } if (RotateToReqAngle(reqrotation) == false) { MoveFrontTickCount *= MoveFrontTickCount; CurrentMoveFrontCount = 0; TraceDraction = (TraceDraction == Direction.Right) ? Direction.Left : Direction.Right; ProgressiveLengthMeasureSubSequence = ProgressiveLengthMeasureSubSequence.Front; goto case ProgressiveLengthMeasureSubSequence.Front; } break; case ProgressiveLengthMeasureSubSequence.Rotate90: if (rotationSaved == false) { rotationSaved = true; if (TraceDraction == Direction.Left) { reqrotation = (CompassCore.Instance.AngleFromN + 90) % 360; } else { reqrotation = (CompassCore.Instance.AngleFromN - 90) % 360; } } if (RotateToReqAngle(reqrotation) == false) { //XXXXXXXXgoto2-3 } break; default: break; } } break; #endregion case MoveSequence.RetractionMeasure: #region 2-3 if (TurnBack == false) { TurnBack = !RotateToReqAngle((ReqAngleFromN + 180) % 360); } else { if (carDivice.f_sonardist < 30) { CurrentMoveSequence = MoveSequence.ForcedForward; goto case MoveSequence.ForcedForward; //goto 2-4 } else { BetweenAngle = GetBetweenTargetAngle(ReqAngleFromN); Direction direction = GetRotateTargetDerection(ReqAngleFromN); if (BetweenAngle < 120) { if (TraceDraction == direction) //바깥쪽으로 나갈때 { if (TraceDraction == Direction.Right) { RBlocked = true; LBlocked = false; } else { RBlocked = false; LBlocked = true; } MoveFrontTickCount = 100; CurrentMoveFrontCount = 0; //TraceDraction = TraceDraction; CurrentMoveSequence = MoveSequence.ProgressiveLengthMeasure; ProgressiveLengthMeasureSubSequence = ProgressiveLengthMeasureSubSequence.Front; goto case MoveSequence.ProgressiveLengthMeasure; } else //안쪽으로 들어갈때 { CurrentMoveSequence = MoveSequence.ForcedForward; goto case MoveSequence.ForcedForward; //goto 2-4 } } DriveWall(TraceDraction); } } break; #endregion case MoveSequence.Root: #region 2-4 break; #endregion default: break; } } }
private float CalcReqAngleFromN(GPSsPosition MyPos, GPSsPosition TargetPos) { float x = Mathf.Abs(MyPos.longitude - TargetPos.longitude); float y = Mathf.Abs(MyPos.latitude - TargetPos.latitude); float ReqAngleFromN; if (MyPos.longitude == TargetPos.longitude && MyPos.latitude == TargetPos.latitude)//위치가 동일함 { ReqAngleFromN = 0; } else if (MyPos.longitude == TargetPos.longitude) //x축 상에 위치 { if (MyPos.latitude < TargetPos.latitude) //+ { ReqAngleFromN = 90; } else//- { ReqAngleFromN = 270; } } else if (MyPos.latitude == TargetPos.latitude) //y축 상에 위치 { if (MyPos.longitude < TargetPos.longitude) //+ { ReqAngleFromN = 0; } else//- { ReqAngleFromN = 180; } } else { ReqAngleFromN = Mathf.Atan2(y, x) / Mathf.PI * 180; if (MyPos.longitude < TargetPos.longitude) //1, 4 분면에 위치 { if (MyPos.latitude < TargetPos.latitude) //1 { ReqAngleFromN = 90 - ReqAngleFromN; } else//4 { ReqAngleFromN = 90 + ReqAngleFromN; } } else//2, 3 분면에 위치 { if (MyPos.latitude < TargetPos.latitude)//2 { ReqAngleFromN = 270 + ReqAngleFromN; } else//3 { ReqAngleFromN = 270 - ReqAngleFromN; } } } return(ReqAngleFromN); }