Mowbot/Mowbot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Thierry Fleury
2025-09-06 14:19:44 +02:00
parent db0d5e6230
commit 0853b6cc3d
2 changed files with 18 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
lib/trip/tripController.cpp
View File

@@ -53,9 +53,13 @@ void TripControllerClass::Stop() {
MotorsController->SetMoveSpeed({0 , 0}); 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 delta = angle_difference(Compass->GetHeading(), heading);
int16_t lastDelta = delta; int16_t lastDelta = delta;
if (timeout <= 0) {
timeout = 10000 + abs(delta) * 100;
}
u_int32_t startTime = millis(); u_int32_t startTime = millis();
while ((delta < -10 || delta > 10 || abs(delta) < abs(lastDelta)) && millis() <= startTime + timeout) 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(); geo_pos origin = GnssService->GetPosition();
double bearing = bearing_to(origin, pos); double bearing = bearing_to(origin, pos);
double distance = distance_to(origin, pos); double distance = distance_to(origin, pos);
log_d("Bearing : %.1f distance : %.2f"); log_d("Bearing : %.1f distance : %.2f");
if (timeout <= 0 && speed > 0) {
timeout = 30000 + (distance / speed) * 2000000;
}
uint32_t startTime = millis(); uint32_t startTime = millis();
RotateToHeading(bearing, min(timeout, 20000u)); RotateToHeading(bearing, min(timeout, 20000u));

8
work.gcode Normal file
View File

@@ -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