From 483eaf7aa8ff347bee867cc5e79196cbe6e33dc3 Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Tue, 22 Sep 2015 22:44:59 +0200
Subject: [PATCH] avr: calibration from umbmark

---
 avr/motor_ctrl/main.c                 | 4 ++--
 avr/motor_ctrl/main.hex               | 8 ++++----
 config/base_local_planner_params.yaml | 6 +++---
 config/control.yaml                   | 4 ++--
 scripts/umbmark.py                    | 2 +-
 scripts/wt_node.py                    | 4 ++--
 6 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/avr/motor_ctrl/main.c b/avr/motor_ctrl/main.c
index fbdc1ce..d403479 100644
--- a/avr/motor_ctrl/main.c
+++ b/avr/motor_ctrl/main.c
@@ -106,8 +106,8 @@
 #define KI 0.051429
 #define KD 0.000378
 #define PID_T 0.01
-#define STEP_PER_M 4171.4 // wheel diameter=12cm, encoder=48cpr, gear ratio=1:34
-#define WHEEL_DIST 0.252
+#define STEP_PER_M 4171.4 // wheel diameter=12cm, encoder=48cpr, gear ratio=1:34, calculated wheel diameter: 0.12454m
+#define WHEEL_DIST 0.36923 // Real: 0.252
 
 enum mode {
 	MOTOR_MANUAL,
diff --git a/avr/motor_ctrl/main.hex b/avr/motor_ctrl/main.hex
index cecc72d..f6c2b4d 100644
--- a/avr/motor_ctrl/main.hex
+++ b/avr/motor_ctrl/main.hex
@@ -207,7 +207,7 @@
 :100CE000F8948090E9009090EA00A090EB00B0901A
 :100CF000EC006091ED007091EE008091EF0090911A
 :100D0000F0001092F10078942BED3FE049E450E4BC
-:100D10000E94501025E236E041E85EE30E94501048
+:100D10000E94501027EB3BE04DEB5EE30E94501029
 :100D200020E030E040E05FE30E945010A501940114
 :100D30000E945D0E6B017C01A5019401C501B40107
 :100D40000E945D0EA70196010E945C0E23E33BE525
@@ -259,8 +259,8 @@
 :101020004B015C01B7016C0D7D1D882777FD809514
 :10103000982F0E94630F23E33BE542E056E40E94B1
 :10104000C80E2B013C01C090B200D090B300E090DC
-:10105000B400F090B500A50194010E945C0E25E259
-:1010600036E041E85EE30E94C80E9B01AC01C70177
+:10105000B400F090B500A50194010E945C0E27EB4E
+:101060003BE04DEB5EE30E94C80E9B01AC01C70163
 :10107000B6010E945D0E6B017C012BED3FE049EC57
 :1010800050E40E944C1018164CF42BED3FE049EC54
 :1010900050E4C701B6010E945C0E12C02BED3FE088
@@ -281,7 +281,7 @@
 :10118000882777FD8095982F0E94630F23E33BE526
 :1011900042E056E40E94C80E4B015C016D8D7E8DCD
 :1011A000601B710B882777FD8095982F0E94630F35
-:1011B00027EB36E64EE455E40E94C80EF894C09240
+:1011B00029E935E347E955E40E94C80EF894C09246
 :1011C000B200D092B300E092B400F092B500298D45
 :1011D0003A8D4B8D5C8D2093BA003093BB004093C9
 :1011E000BC005093BD004092B6005092B700609290
diff --git a/config/base_local_planner_params.yaml b/config/base_local_planner_params.yaml
index 9cc7b79..3b509c3 100644
--- a/config/base_local_planner_params.yaml
+++ b/config/base_local_planner_params.yaml
@@ -1,13 +1,13 @@
 TrajectoryPlannerROS:
   min_vel_x: 0.1
   max_vel_x: 0.3
-  min_vel_theta: -1.0
-  max_vel_theta: 1.0
+  min_vel_theta: -0.4
+  max_vel_theta: 0.4
   min_in_place_vel_theta: 0.4
 
   acc_lim_x: 2.5
   acc_lim_y: 0.0
-  acc_lim_theta: 3.2
+  acc_lim_theta: 0.3
 
   holonomic_robot: false
   meter_scoring: true
diff --git a/config/control.yaml b/config/control.yaml
index 97d83a5..ea206e9 100644
--- a/config/control.yaml
+++ b/config/control.yaml
@@ -22,8 +22,8 @@ diff_drive_controller:
   estimate_velocity_from_position: false
 
   # Wheel separation and radius multipliers
-  wheel_separation_multiplier: 1.4696 # default: 1.0
-  wheel_radius_multiplier    : 0.98346 # default: 1.0
+  wheel_separation_multiplier: 1.4126 # default: 1.0
+  wheel_radius_multiplier    : 0.98284 # default: 1.0
 
   # Velocity and acceleration limits
   # Whenever a min_* is unspecified, default to -max_*
diff --git a/scripts/umbmark.py b/scripts/umbmark.py
index d103b5c..e6bb5d5 100755
--- a/scripts/umbmark.py
+++ b/scripts/umbmark.py
@@ -82,7 +82,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)
diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index 96d0768..eb9f8bd 100755
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -151,7 +151,7 @@ class MoveBase:
 		odom.pose.covariance[14] = 1e6 # z
 		odom.pose.covariance[21] = 1e6 # rotation about X axis
 		odom.pose.covariance[28] = 1e6 # rotation about Y axis
-		odom.pose.covariance[35] = 0.1 # rotation about Z axis
+		odom.pose.covariance[35] = 0.03 # rotation about Z axis
 
 		# set the velocity
 		odom.child_frame_id = "base_footprint"
@@ -163,7 +163,7 @@ class MoveBase:
 		odom.twist.covariance[14] = 1e6 # z
 		odom.twist.covariance[21] = 1e6 # rotation about X axis
 		odom.twist.covariance[28] = 1e6 # rotation about Y axis
-		odom.twist.covariance[35] = 0.1 # rotation about Z axis
+		odom.twist.covariance[35] = 0.03 # rotation about Z axis
 
 		# publish the message
 		self.pub_odom.publish(odom)
-- 
2.39.2