From 326da1598236db6f7a7229feb86c397c0e685d0b Mon Sep 17 00:00:00 2001
From: ipa-fxm <felix.messmer@ipa.fhg.de>
Date: Wed, 26 Apr 2017 13:05:36 +0200
Subject: [PATCH] canopen config for raw3-3 base

---
 cob_bringup/robots/raw3-3.xml                 |  9 +++-
 .../robots/common/raw3_base_controller.yaml   |  8 ++--
 .../robots/raw3-3/config/base_controller.yaml | 46 -------------------
 .../robots/raw3-3/config/base_driver.yaml     | 28 +++++++++++
 .../raw3-3/config/diagnostics_analyzers.yaml  |  4 +-
 5 files changed, 41 insertions(+), 54 deletions(-)
 delete mode 100644 cob_hardware_config/robots/raw3-3/config/base_controller.yaml
 create mode 100644 cob_hardware_config/robots/raw3-3/config/base_driver.yaml

diff --git a/cob_bringup/robots/raw3-3.xml b/cob_bringup/robots/raw3-3.xml
index be3cd37cd..82524af61 100644
--- a/cob_bringup/robots/raw3-3.xml
+++ b/cob_bringup/robots/raw3-3.xml
@@ -24,6 +24,10 @@
 
 		<!-- start hardware -->
 		<group unless="$(arg sim)">
+			<include file="$(find cob_bringup)/tools/pc_monitor.launch">
+				<arg name="robot" value="$(arg robot)"/>
+				<arg name="pc" value="$(arg pc1)"/>
+			</include>
 			<include file="$(find cob_bringup)/drivers/phidgets.launch">
 				<arg name="robot" value="$(arg robot)"/>
 			</include>
@@ -46,8 +50,9 @@
 			<arg name="name" value="base_laser_rear"/>
 			<arg name="sim" value="$(arg sim)"/>
 		</include>
-		<include file="$(find cob_bringup)/components/legacy_base.launch">
-			<arg name="robot" value="$(arg robot)"/>
+		<include file="$(find cob_bringup)/components/raw3_base.launch" >
+			<arg name="robot" value="$(arg robot)" />
+			<arg name="can_device" value="can0" />
 			<arg name="sim" value="$(arg sim)"/>
 		</include>
 		<!--include file="$(find cob_bringup)/components/canopen_generic.launch">
diff --git a/cob_hardware_config/robots/common/raw3_base_controller.yaml b/cob_hardware_config/robots/common/raw3_base_controller.yaml
index 1fb735b0b..b51d3cc0f 100644
--- a/cob_hardware_config/robots/common/raw3_base_controller.yaml
+++ b/cob_hardware_config/robots/common/raw3_base_controller.yaml
@@ -29,16 +29,16 @@ twist_controller:
   wheels: &wheels
     - steer: fl_caster_rotation_joint
       drive: fl_caster_r_wheel_joint
-      steer_neutral_position: -180.0
+      steer_neutral_position: 180.0
     - steer: bl_caster_rotation_joint
       drive: bl_caster_r_wheel_joint
-      steer_neutral_position: -180.0
+      steer_neutral_position: 180.0
     - steer: br_caster_rotation_joint
       drive: br_caster_r_wheel_joint
-      steer_neutral_position: -180.0
+      steer_neutral_position: 180.0
     - steer: fr_caster_rotation_joint
       drive: fr_caster_r_wheel_joint
-      steer_neutral_position: 0.0
+      steer_neutral_position: 180.0
 
 
 ## odometry_controller
diff --git a/cob_hardware_config/robots/raw3-3/config/base_controller.yaml b/cob_hardware_config/robots/raw3-3/config/base_controller.yaml
deleted file mode 100644
index 846954dae..000000000
--- a/cob_hardware_config/robots/raw3-3/config/base_controller.yaml
+++ /dev/null
@@ -1,46 +0,0 @@
-## joint_names
-joint_names: [fr_caster_rotation_joint, fl_caster_rotation_joint, br_caster_rotation_joint, bl_caster_rotation_joint, fr_caster_r_wheel_joint, fl_caster_r_wheel_joint, br_caster_r_wheel_joint, bl_caster_r_wheel_joint]
-
-## joint_state_controller
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-
-## wheel_controller
-twist_controller:
-  type: cob_omni_drive_controller/WheelController
-  publish_rate: 50
-
-  defaults: # default settings for all wheels, can per overwritten per wheel
-    # wheel_radius: 0.080 # Radius of the wheels in [m]
-    # wheel_offset: 0  # Distance of the wheels steering axis to the wheel center in [m], read from URDF is not specified
-    # steer_drive_coupling: 0.0
-    # steer_neutral_position: 0.0  # [deg]
-    steer_ctrl: # Parameters for tuning Impedance-Controller
-      spring: 15.0 #10.0
-      damp: 2.75 #2.5
-      virt_mass: 0.1 #0.1
-      d_phi_max: 12.0 #100.0
-      dd_phi_max: 40.0 #100.0
-  wheels: &wheels
-    - steer: fl_caster_rotation_joint
-      drive: fl_caster_r_wheel_joint
-    - steer: bl_caster_rotation_joint
-      drive: bl_caster_r_wheel_joint
-    - steer: br_caster_rotation_joint
-      drive: br_caster_r_wheel_joint
-    - steer: fr_caster_rotation_joint
-      drive: fr_caster_r_wheel_joint
-
-
-## odometry_controller
-odometry_controller:
-  type: cob_omni_drive_controller/OdometryController
-  publish_rate: 50
-  frame_id: "/odom_combined"
-  child_frame_id: "/base_footprint"
-  cov_pose: 0.1
-  cov_twist: 0.1
-  defaults: # default settings for all wheels, can per overwritten per wheel
-    wheel_radius: 0.080 # Radius of the wheels in [m]
-  wheels: *wheels
diff --git a/cob_hardware_config/robots/raw3-3/config/base_driver.yaml b/cob_hardware_config/robots/raw3-3/config/base_driver.yaml
new file mode 100644
index 000000000..8b0f296d5
--- /dev/null
+++ b/cob_hardware_config/robots/raw3-3/config/base_driver.yaml
@@ -0,0 +1,28 @@
+nodes:
+  ##Wheel 1
+  fl_caster_rotation_joint:
+    motor_layer:
+      homing_event: 19 # -> digin
+      homing_speed: -5093 # -1.0 rs/s
+      homing_offset: -12700
+
+  ##Wheel 2
+  bl_caster_rotation_joint:
+    motor_layer:
+      homing_event: 19 # -> digin
+      homing_speed: -5093 # -1.0 rs/s
+      homing_offset: -12673
+
+  ##Wheel 3
+  br_caster_rotation_joint:
+    motor_layer:
+      homing_event: 17 # -> digin
+      homing_speed: -5093 # -1.0 rs/s
+      homing_offset: 14093
+
+  ##Wheel 4
+  fr_caster_rotation_joint:
+    motor_layer:
+      homing_event: 19 # -> digin
+      homing_speed: -5093 # -1.0 rs/s
+      homing_offset: 13972
diff --git a/cob_hardware_config/robots/raw3-3/config/diagnostics_analyzers.yaml b/cob_hardware_config/robots/raw3-3/config/diagnostics_analyzers.yaml
index 48f703c0c..3b18db41a 100644
--- a/cob_hardware_config/robots/raw3-3/config/diagnostics_analyzers.yaml
+++ b/cob_hardware_config/robots/raw3-3/config/diagnostics_analyzers.yaml
@@ -41,8 +41,8 @@ analyzers:
         analyzers:
           base:
             type: diagnostic_aggregator/GenericAnalyzer
-            path: drives
-            startswith: '/base'
+            path: Base
+            contains: 'base/'
           base_twist_mux:
             type: diagnostic_aggregator/GenericAnalyzer
             path: twist_mux