-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathfound path!!.txt
74 lines (73 loc) · 2.33 KB
/
found path!!.txt
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
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
[[[-0.62375545 0.24017797 1.67528665]]
[[ 0.19291339 0.131436 0.76053451]]]
x: -0.7010711383207197 y: 1.8502866484724565
x: 0.2855549186817361 y: 0.9355345121507754
found path!!
Waiting for serial port connection ...
Running ...
current point 0, x: 0.0, y: 0.0
theta to turn: 31.898197493623837
robots theta: 90.0
Distance to next point: 0.4
turning left
turning
current point 0, x: 0.0, y: 0.0
theta to turn: 0.0
robots theta: 121.89819749362384
Distance to next point: 0.4
drive dist: 0.4
current point 1, x: -0.21136465043633024, y: 0.33959532468208087
theta to turn: -37.95944000429116
robots theta: 121.89819749362384
Distance to next point: 0.40000000000000024
turning right
turning
current point 1, x: -0.21136465043633024, y: 0.33959532468208087
theta to turn: 0.0
robots theta: 83.93875748933267
Distance to next point: 0.40000000000000024
drive dist: 0.40000000000000024
current point 2, x: -0.16912807789606155, y: 0.737359164089399
theta to turn: -254.80887814295355
robots theta: 83.93875748933267
Distance to next point: 0.4
turning right
turning
current point 2, x: -0.16912807789606155, y: 0.737359164089399
theta to turn: 0.0
robots theta: -170.87012065362086
Distance to next point: 0.4
drive dist: 0.4
current point 3, x: -0.564060555563257, y: 0.673889974054783
theta to turn: 261.54820230000195
robots theta: -170.87012065362086
Distance to next point: 0.4000000000000002
turning left
turning
current point 3, x: -0.564060555563257, y: 0.673889974054783
theta to turn: -1.2722218725854067e-14
robots theta: 90.6780816463811
Distance to next point: 0.4000000000000002
drive dist: 0.4000000000000002
current point 4, x: -0.5687943479884658, y: 1.073861962085539
theta to turn: -53.33856366230144
robots theta: 90.6780816463811
Distance to next point: 0.40000000000000024
turning right
turning
current point 4, x: -0.5687943479884658, y: 1.073861962085539
theta to turn: 0.0
robots theta: 37.33951798407966
Distance to next point: 0.40000000000000024
drive dist: 0.40000000000000024
current point 5, x: -0.2507722159863881, y: 1.3164767257683232
theta to turn: -1.1416371016147617
robots theta: 37.33951798407966
Distance to next point: 0.3107531118033075
turning right
turning
current point 5, x: -0.2507722159863881, y: 1.3164767257683232
theta to turn: 0.0
robots theta: 36.1978808824649
Distance to next point: 0.3107531118033075
drive dist: 0.3107531118033075