From 0853b6cc3d2905056cc57c3ce7308bab69ddb76b Mon Sep 17 00:00:00 2001
From: Thierry Fleury <thierry@codinlab.fr>
Date: Sat, 6 Sep 2025 14:19:44 +0200
Subject: [PATCH] wip

---
 lib/trip/tripController.cpp | 12 ++++++++++--
 work.gcode                  |  8 ++++++++
 2 files changed, 18 insertions(+), 2 deletions(-)
 create mode 100644 work.gcode

diff --git a/lib/trip/tripController.cpp b/lib/trip/tripController.cpp
index 5a2a889..79ef7ca 100644
--- a/lib/trip/tripController.cpp
+++ b/lib/trip/tripController.cpp
@@ -53,9 +53,13 @@ void TripControllerClass::Stop() {
   MotorsController->SetMoveSpeed({0 , 0});
 }
 
-void TripControllerClass::RotateToHeading(double heading, uint timeout) {
+void TripControllerClass::RotateToHeading(double heading, uint timeout = 0) {
   int16_t delta = angle_difference(Compass->GetHeading(), heading);
   int16_t lastDelta = delta;
+
+  if (timeout <= 0) {
+    timeout = 10000 + abs(delta) * 100;
+  }
   
   u_int32_t startTime = millis();
   while ((delta < -10 || delta > 10 || abs(delta) < abs(lastDelta)) && millis() <= startTime + timeout)
@@ -71,12 +75,16 @@ void TripControllerClass::RotateToHeading(double heading, uint timeout) {
   }
 }
 
-void TripControllerClass::GoTo(geo_pos &pos, u_int8_t speed, uint timeout) {
+void TripControllerClass::GoTo(geo_pos &pos, u_int8_t speed, uint timeout = 0) {
   geo_pos origin = GnssService->GetPosition();
   double bearing = bearing_to(origin, pos);
   double distance = distance_to(origin, pos);
   log_d("Bearing : %.1f distance : %.2f");
 
+  if (timeout <= 0 && speed > 0) {
+    timeout = 30000 + (distance / speed) * 2000000;
+  }
+  
   uint32_t startTime = millis();
 
   RotateToHeading(bearing, min(timeout, 20000u));
diff --git a/work.gcode b/work.gcode
new file mode 100644
index 0000000..75ecfa8
--- /dev/null
+++ b/work.gcode
@@ -0,0 +1,8 @@
+G0 X-146 Y-882
+G1 X-4798 Y2497
+G1 X-11560 Y-6810
+G1 X-4495 Y-11850
+G1 X1089 Y-6304
+G1 X-2298 Y-3843
+G1 X-146 Y-882
+M30
\ No newline at end of file