1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
| """Check path segment headings"""
| from math import atan2, pi
|
| approach_points = [
| (-1.5, -1.0),
| (-1.262, -0.749),
| (-1.045, -0.514),
| (-0.829, -0.309),
| (-0.596, -0.146),
| (-0.326, -0.039),
| (0, 0)
| ]
|
| print("Approach path segment headings:")
| for i in range(len(approach_points)-1):
| p1, p2 = approach_points[i], approach_points[i+1]
| heading = atan2(p2[1]-p1[1], p2[0]-p1[0])
| print(f"Segment {i}->{i+1}: heading={heading:.3f} rad = {heading*180/pi:.1f} deg")
|
| print("\nWork path segment headings (first few):")
| work_points = [(0, 0), (10, 0), (10, 1), (0, 1)]
| for i in range(len(work_points)-1):
| p1, p2 = work_points[i], work_points[i+1]
| heading = atan2(p2[1]-p1[1], p2[0]-p1[0])
| print(f"Segment {i}->{i+1}: heading={heading:.3f} rad = {heading*180/pi:.1f} deg")
|
|
