method AvoidObstacle() { variable int Degrees = 0 variable int Distance = 5 variable int NewHeading = 0 variable bool Left = FALSE variable float X variable float Y variable float Z variable float MeZ MeZ:Set[${Me.Z} + 0.3] do { Degrees:Inc[5] if ${Left} { if ${Math.Calc[${Me.Heading} - ${Degrees}]} < 0 { NewHeading:Set[${Math.Calc[${Degrees} - ${Me.Heading}]}] } else { NewHeading:Set[${Me.Heading} - ${Degrees}] } Left:Set[FALSE] } else { if ${Math.Calc[${Me.Heading} + ${Degrees}]} > 360 { NewHeading:Set[${Math.Calc[360 - (${Degrees} + ${Me.Heading})]}] } else { NewHeading:Set[${Me.Heading} + ${Degrees}] } Left:Set[TRUE] } X:Set[${Math.Calc[${Me.X}+${Distance}*${Math.Cos[${NewHeading}]}]}] Y:Set[${Math.Calc[${Me.Y}-${Distance}*${Math.Sin[${NewHeading}]}]}] Z:Set[${Math.Calc[${Me.Z} + 0.3]}] } while ${Me.IsPathObstructed[${X},${Y},${Z},999,${Me.X},${Me.Y}.${MeZ}]} && ${Degrees} < 190 This:Debug[">>> ${Me.Heading} ${NewHeading}"] This.CurrentDestination:Set[${X}] This.CurrentDestination:Set[${Y}] This.CurrentDestination:Set[${Z}] This:FaceXYZ[${X},${Y},${Z}] }