X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fumbmark.py;h=f031a675b9bf1e25855e27b16100509795a5dd96;hp=d103b5c0abf277b6afb0f0a9b544f9ce503af549;hb=cb29fa4ba7d1492c9f91752a7a07b512fee7dad0;hpb=6925d3469d5f22b6271ece74540c0a30a51942db

diff --git a/scripts/umbmark.py b/scripts/umbmark.py
index d103b5c..f031a67 100755
--- a/scripts/umbmark.py
+++ b/scripts/umbmark.py
@@ -21,10 +21,14 @@
 # beta = (y_cg_cw + y_cg_ccw)/(-4*L)
 # R = (L/2)/sin(beta/2)
 # Ed = Dr/Dl*(R+b/2)/(R-b/2)
+# Da = (Dr + Dl)/2
+# Dl = 2/(Ed + 1) * Da
+# Dr = 2/((1/Ed) + 1) * Da
 # 
 # Wheelbase correction:
 # alpha = (y_cg_cw - y_cg_ccw)/(-4*L) * 180/pi
 # Eb = (90)/(90-alpha)
+# b_new = Eb*b
 
 import sys
 import rospy
@@ -82,7 +86,7 @@ class UMBMark:
 			self.next_pos(2, 0, 0)
 			self.next_pos(0, 0, direction*90*pi/180)
 		final_pose = map(operator.sub, self.odom_pose, init_pose)
-		print "Odom Pose: x=%.2f, y=%.2f, angle=%.2f°" % (final_pose[0], final_pose[1], final_pose[2]*180/pi)
+		print "Odom Pose: x=%.3f, y=%.3f, angle=%.3f°" % (final_pose[0], final_pose[1], final_pose[2]*180/pi)
 
 	def run_cw(self):
 		self.run(-1)