Autopilot Arduino 2Motors DC straight line from A to B

Potrzebuję wykonać projekt na zaliczenie termin październik 2022
Założenia:
Pojazd dwusilnikowy 2motor DC
Mostrk H
GPS
Kompas

Zadanie:
Pojazd ma przejechać w linii prostej z punktu A do punktu B 100m

Koordynaty wpisane na sztywno FromLat FromLon oraz GotoLat oraz GotoLon

Potrzebuję sam kod.

Platforma dowolna Arduino, Stm32, Esp32