TCL/rfidLocMap.tcl at master · andrescodas/TCL · GitHub

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
if { [info exists _RFIDLOCMAP.TCL] == 0 } {
set _RFIDLOCMAP.TCL 1

source connectSpace.tcl

set home $env(HOME)
set rfidLocMapPath /workspace/rfidLocMapBin/Linux/

lm rfidLocMap path $home$rfidLocMapPath

proc getPositionRFIDLocMap { position } {

  upvar $position pos

  set format "status = OK\n"
  append format "RobotPosition.xRob = %f\n"
  append format "RobotPosition.yRob = %f\n"
  append format "RobotPosition.theta = %f\n"

  scan [rfidLocMap::RobotPositionRobotPositionPosterRead] $format xRob yRob theta

  set pos(xRob) $xRob
  set pos(yRob) $yRob
  set pos(theta) $theta

}

##  Only for the first tag... for more tags generate a List as the readings of the rfid
proc getTagPosition { position } {

  upvar $position pos

  set format "status = %s\n"
  append format "TagsPosition.nbTags = %d\n"
  append format "TagsPosition.tags\[0\].tagId = %s\n"
  append format "TagsPosition.tags\[0\].tag_position.x = %f\n"
  append format "TagsPosition.tags\[0\].tag_position.y = %f\n"

  scan [rfidLocMap::TagsPositionTagsPositionPosterRead] $format status nbTags tagId x y

  set pos(nbTags) $nbTags
  set pos(tagId) $tagId
  set pos(x) $x
  set pos(y) $y

}


# ifndef

}