Update Find cmake with correct paths and path order

Findsegway_rmp_400.cmake

@@ -2,12 +2,12 @@ SET(name segway_rmp_400)
 #edit the following line to add the librarie's header files
 SET(header_files segway_RMP400.h segway_RMP400_exceptions.h)
 
-FIND_PATH(${name}_INCLUDE_DIRS ${header_files} /usr/include/iri/${name} /usr/local/include/iri/${name})
+FIND_PATH(${name}_INCLUDE_DIRS ${header_files}.h /usr/local/include/iri/${name} /usr/include/iri/${name})
 
 FIND_LIBRARY(${name}_LIBRARIES
     NAMES ${name}
-    PATHS /usr/lib /usr/lib/iri/${name} /usr/local/lib /usr/local/lib/iri/${name})
-
+    PATHS /usr/local/lib/iri/${name} /usr/lib/iri/${name})
+    
 SET(${name}_INCLUDE_DIR ${${name}_INCLUDE_DIRS})
 SET(${name}_LIBRARY     ${${name}_LIBRARIES})