まず、あらかじめ、走りたい経路上の座標をある程度プロットしておきます。
①Aくん家(出発地) ②Bくん家 ③Cくん家... ⑩Jくん家(ゴール)みたいな感じです。
次に、GPSから現在地の座標を取得して、Aくん家との距離を出します。もし、距離が3m以内になったらBくん家を目的地にかえます。
※GPSは誤差があるので、3m以内になったら目的地を次に変えるようにしています。
次に、今のロボットの方位と、現在地とAくん家の方位を出します。二つの方位から何度旋回すればいいのかをだします。もし、+10以上だったら右に旋回し、-10以上だったら左に旋回します。
※誤差もでるので-10から10の間はまっすぐ進みます。