-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmocapRflex.tcl
173 lines (123 loc) · 3.4 KB
/
mocapRflex.tcl
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
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
if { [info exists _MOCAPRFLEX.TCL] == 0 } {
set _MOCAPRFLEX.TCL 1
source moveTools.tcl
source pilo.tcl
source mocap.tcl
## Return close to (0,0) to clean odometry
proc goToclearOdometry { pos } {
global pi
upvar $pos position
set REFobjective(x) [expr $position(xRob)/2]
set REFobjective(y) [expr $position(yRob)/2]
findDistance dx dy d REFobjective position
findTheta dx dy dt position
if {[expr {abs($dt)}] > [expr $pi/2]} {
set dt [expr $dt + $pi]
wrapAngle dt
set d -$d
}
turn $dt
move $d
}
proc areEqualPosition { pos1 pos2 } {
upvar $pos1 position1
upvar $pos2 position2
if { $position1(xRob) == $position2(xRob) } {
if { $position1(yRob) == $position2(yRob) } {
if { $position1(theta) == $position2(theta) } {
return 1
} else {
return 0
}
} else {
return 0
}
} else {
return 0
}
}
proc setPositionArray {destination origin} {
upvar $destination d
upvar $origin o
set d(xRob) $o(xRob)
set d(yRob) $o(yRob)
set d(theta) $o(theta)
}
## pos(from) == 1 from Mocap
## pos(from) == 0 from RFLEX
set rflexOld(xRob) 0
set rflexOld(yRob) 0
set rflexOld(theta) 0
set mocapLast(xRob) 0
set mocapLast(yRob) 0
set mocapLast(theta) 0
set lastPosition(xRob) 0
set lastPosition(yRob) 0
set lastPosition(theta) 0
set distanceWithoutMocap(d) 0
set distanceWithoutMocap(t) 0
set equalPositionsCount 0
set initialized 0
proc getPositionMocapRflex { position } {
upvar $position pos
global mocapLast
global rflexOld
global lastPosition
global initialized
global distanceWithoutMocap
global equalPositionsCount
getPositionMocapTreatError pos
set pos(from) 1
getPositionRFLEX posRflex
if { $equalPositionsCount > 20 } {
set pos(status) 77
set equalPositionsCount 0
}
if { $pos(status) == 0 } {
set cond [areEqualPosition pos mocapLast]
if { $cond == 1 } {
parray pos
mocap::Stop
mocap::ReadMarkers -ack
after 100
puts "$pos(xRob) $pos(yRob) $pos(theta) Doubt equal positions! $equalPositionsCount\n$mocapLast(xRob) $mocapLast(yRob) $mocapLast(theta)"
set equalPositionsCount [expr $equalPositionsCount + 1]
getPositionMocapRflex pos
}
if { $pos(from) == 1} {
set distanceWithoutMocap(d) 0
set distanceWithoutMocap(t) 0
setPositionArray mocapLast pos
set equalPositionsCount 0
if { $initialized == 0} {
set initialized 1
}
}
} else {
if { $initialized == 0 } {
error "Not initialized mocapMeasurement"
}
puts "Position Mocap Status == $pos(status)"
set deslocRflex(xRob) [expr $posRflex(xRob) - $rflexOld(xRob)]
set deslocRflex(yRob) [expr $posRflex(yRob) - $rflexOld(yRob)]
set deslocRflex(theta) [expr $posRflex(theta) - $rflexOld(theta)]
set distanceWithoutMocap(d) [expr $distanceWithoutMocap(d)+sqrt($deslocRflex(yRob)*$deslocRflex(yRob)+$deslocRflex(xRob)*$deslocRflex(xRob))]
set distanceWithoutMocap(t) [expr $distanceWithoutMocap(t) + $deslocRflex(theta)]
rotation deslocRflex [expr $lastPosition(theta) - $rflexOld(theta)]
set pos(xRob) [expr $lastPosition(xRob) + $deslocRflex(xRob)]
set pos(yRob) [expr $lastPosition(yRob) + $deslocRflex(yRob)]
set pos(theta) [expr $lastPosition(theta) + $deslocRflex(theta)]
set pos(from) 0
parray pos
parray distanceWithoutMocap
set condition 0
while { $condition != 1 } {
puts "Press 1 to continue"
gets stdin condition
}
}
setPositionArray rflexOld posRflex
setPositionArray lastPosition pos
}
# ifnded
}