TCL/pilo.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
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
if { [info exists _PILO.TCL] == 0 } {

set _PILO.TCL 1

source connectSpace.tcl

lm rflex on space
lm pom on space
lm pilo on space


pom::SetModel /usr/local/openrobots/data/robots/rackham/geom/b21r.geom
pilo::Init  pomPos
rflex::InitClient /dev/ttyR0 115200
rflex::SetPos 0 0 0 0
rflex::TrackSpeedStart -ack piloSpeedRef


puts "I--- GetWdogRef ---I"
puts [rflex::GetWdogRef]
puts "F--- GetWdogRef ---F"
puts "I--- GetMode ---I"
puts [rflex::GetMode]
puts "F--- GetMode ---F"

proc move {d} {

	if {$d != 0} then {
		pilo::Move $d 20 20
	}
}

proc turn {dt} {

	if {$dt != 0} then {

		pilo::Turn $dt 0 20 20
      	}
}


proc getPositionRFLEX { position } {

  upvar $position pos

  set format "status = OK\n"
  append format "Position.xRef = %f\nPosition.yRef = %f\nPosition.zRef = %f\n"
  append format "Position.xRob = %f\nPosition.yRob = %f\nPosition.zRob = %f\n"
  append format "Position.theta = %f\n"

  scan [rflex::RobotPositionPosterRead] $format xRef yRef zRef xRob yRob zRob theta

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

proc getCovRFLEX { covariance } {

   upvar $covariance cov

  set format "status = OK\n"
  append format "PosError.var\[0\] = %f\n"
  append format "PosError.var\[1\] = %f\n"
  append format "PosError.var\[2\] = %f\n"
  append format "PosError.var\[3\] = %f\n"
  append format "PosError.var\[4\] = %f\n"
  append format "PosError.var\[5\] = %f\n"

  scan [rflex::RobotPosErrorPosterRead] $format var0 var1 var2 var3 var4 var5

  set cov(0) $var0
  set cov(1) $var1
  set cov(2) $var2
  set cov(3) $var3
  set cov(4) $var4
  set cov(5) $var5
}

proc clearOdometryErr {  } {

	rflex::SetPosErr 0 0 0 0 0 0

}

#ifndef
}