diff --git a/air_lab1/CMakeLists.txt b/air_lab1/CMakeLists.txt
old mode 100644
new mode 100755
index 557eb555a533f113f448a27dd47243981a799e91..a8dfb0c53425f5b62039d44d32a06cf15345270e
--- a/air_lab1/CMakeLists.txt
+++ b/air_lab1/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.1.0)
 project(air_lab1)
 
 set (CMAKE_CXX_STANDARD 17)
@@ -7,7 +7,10 @@ set (CMAKE_CXX_STANDARD 17)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
-  lrs_march nodelet rospy
+  message_generation
+  lrs_march nodelet
+  rospy
+  std_msgs
 )
 
 find_package(March REQUIRED)
@@ -49,11 +52,10 @@ include_directories(${CMAKE_CURRENT_BINARY_DIR}/modules/ ${CMAKE_CURRENT_SOURCE_
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES
+  ControllerEvaluationStat.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -70,10 +72,10 @@ include_directories(${CMAKE_CURRENT_BINARY_DIR}/modules/ ${CMAKE_CURRENT_SOURCE_
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  std_msgs  # Or other packages containing msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -157,7 +159,6 @@ add_library(air_lab1_nodelets
   )
 
 target_link_libraries(air_lab1_nodelets ${catkin_LIBRARIES} ${MARCH_STDLIB_LIBRARIES} ${MARCH_GRAPH_FLOW_LIBRARIES} march_debugger march_debugger_networking )
-add_dependencies(air_lab1_nodelets lrs_msgs_common_gencpp)
 class_loader_hide_library_symbols(air_lab1_nodelets)
 
 #############
diff --git a/air_lab1/machines/husky_control.mhl b/air_lab1/machines/husky_control.mhl
old mode 100644
new mode 100755
index 360dd8dcb25c40f5b485ca2807b37a6d52984d8f..8c1ba9ef5f5b2fb9dea79daebcbd47a289a19b84
--- a/air_lab1/machines/husky_control.mhl
+++ b/air_lab1/machines/husky_control.mhl
@@ -5,28 +5,28 @@ project
       timeout 0
       title husky_control
       id 0
-      geometry 84.8239 30 3674.09 2258
+      geometry 84.8239 30 4245.33 1272
       minimum_size 80 40
       group
         title velocity control
         id 1
-        geometry 3052.47 406 611.616 482
+        geometry 3052.47 406 1182.86 482
         minimum_size 100 80
         processing_state
           timeout 0
           title controller
           id 2
-          geometry 10 30 591.616 283
+          geometry 10 30 1162.86 335
           minimum_size 80 40
           processing_graph
             title untitled
             id 3
-            geometry 0 0 591.616 283
+            geometry 0 0 1162.86 335
             minimum_size 80 40
             processing_node
               title joint state msg
               id 408
-              geometry 456.069 111 125.547 96
+              geometry 1045.41 111 107.453 96
               minimum_size 80 96
               library ros_sensor_msgs_tools
               description joint_state_assemble
@@ -55,7 +55,7 @@ project
             processing_node
               title append
               id 680
-              geometry 326 30 80 42
+              geometry 915.341 30 80 42
               minimum_size 80 42
               library march_list
               description list_append
@@ -72,7 +72,7 @@ project
             processing_node
               title append
               id 681
-              geometry 194 35 80 42
+              geometry 783.341 35 80 42
               minimum_size 80 42
               library march_list
               description list_append
@@ -89,7 +89,7 @@ project
             processing_node
               title left
               id 684
-              geometry 58 71 80 40
+              geometry 647.341 71 80 40
               minimum_size 80 40
               library march_std_lib
               description output_string
@@ -103,7 +103,7 @@ project
             processing_node
               title right
               id 688
-              geometry 195 94 80 40
+              geometry 784.341 94 80 40
               minimum_size 80 40
               library march_std_lib
               description output_string
@@ -117,7 +117,7 @@ project
             processing_node
               title append left effort
               id 690
-              geometry 10 145 143.625 42
+              geometry 599.341 145 122.563 42
               minimum_size 80 42
               library march_list
               description list_append
@@ -134,7 +134,7 @@ project
             processing_node
               title append right effort
               id 691
-              geometry 236 231 153.953 42
+              geometry 825.341 231 131.094 42
               minimum_size 80 42
               library march_list
               description list_append
@@ -155,7 +155,7 @@ project
           timeout 0
           title initial time
           id 732
-          geometry 12.654 412 125.219 60
+          geometry 12.654 412 113.25 60
           minimum_size 80 60
           library march_ros
           description current_time
@@ -166,95 +166,19 @@ project
       group
         title control mode
         id 59
-        geometry 1009.48 411 1425.45 943
+        geometry 1009.48 411 147.891 160
         minimum_size 100 80
         super_state
           timeout 0
           title control mode
           id 150
-          geometry 10 30 1405.45 903
+          geometry 10 30 127.891 120
           minimum_size 80 40
           group
             title control mode
             id 151
-            geometry 10 30 1385.45 863
+            geometry 10 30 100 80
             minimum_size 100 80
-            super_state
-              timeout 0
-              title position control modes
-              id 886
-              geometry 566.335 243 809.113 610
-              minimum_size 80 40
-              group
-                title mode
-                id 887
-                geometry 10 30 100 80
-                minimum_size 100 80
-                activity_state
-                  timeout 0
-                  title idle
-                  id 889
-                  geometry 10 30 80 40
-                  minimum_size 80 40
-                  mode direct
-                end
-                initial_state 889
-              end
-              group
-                title reach point controller
-                id 817
-                geometry 629.16 231 169.953 369
-                minimum_size 100 80
-                activity_state
-                  timeout 0
-                  title reach_point
-                  id 818
-                  geometry 17.1215 299 133.172 60
-                  minimum_size 80 60
-                  library air_lab1_march
-                  description reach_point
-                  mode direct
-                  input_connector
-                    name cmd_position
-                    queue_size 1
-                  end
-                  input_connector
-                    name cmd_max_vel
-                    queue_size 1
-                  end
-                  input_connector
-                    name current_position
-                    queue_size 1
-                  end
-                  parameters
-                    krho 0.5
-                    kalpha 0.7
-                    kbeta -0.2
-                  end
-                  parameter_references
-                  end
-                end
-                activity_state
-                  timeout 0
-                  title idle
-                  id 819
-                  geometry 10 30 80 40
-                  minimum_size 80 40
-                  library march_std_lib
-                  description output_float64
-                  mode direct
-                  input_connector
-                    name value
-                    queue_size 1
-                  end
-                  parameters
-                  end
-                  parameter_references
-                  end
-                end
-                initial_state 819
-              end
-            end
             activity_state
               timeout 0
               title idle
@@ -269,7 +193,6 @@ project
                 queue_size 1
               end
               parameters
-                value 0
               end
               parameter_references
               end
@@ -282,23 +205,23 @@ project
       group
         title extract wheel velocities
         id 697
-        geometry 2577.61 30 368.856 236
+        geometry 2577.61 30 355.793 236
         minimum_size 100 80
         processing_state
           timeout 0
           title extract wheel velocities
           id 698
-          geometry 10 30 348.856 196
+          geometry 10 30 335.793 196
           minimum_size 80 40
           processing_graph
             title untitled
             id 699
-            geometry 0 0 348.856 196
+            geometry 0 0 335.793 196
             minimum_size 80 40
             processing_node
               title left wheel
               id 700
-              geometry 242.841 30 92.0156 42
+              geometry 242.841 30 80.75 42
               minimum_size 80 42
               library march_list
               description list_at
@@ -319,7 +242,7 @@ project
             processing_node
               title right wheel
               id 701
-              geometry 236.512 144 102.344 42
+              geometry 236.512 144 89.2813 42
               minimum_size 80 42
               library march_list
               description list_at
@@ -340,7 +263,7 @@ project
             processing_node
               title split message
               id 702
-              geometry 10 57 117.922 96
+              geometry 10 57 101.375 96
               minimum_size 80 96
               library ros_sensor_msgs_tools
               description joint_state_dissassemble
@@ -359,23 +282,23 @@ project
       group
         title extract position from odometry
         id 766
-        geometry 10 184 457.38 161
+        geometry 10 184 434.567 161
         minimum_size 100 80
         processing_state
           timeout 0
           title dissassemble odometry message
           id 767
-          geometry 10 30 437.38 121
+          geometry 10 30 414.567 121
           minimum_size 80 40
           processing_graph
             title untitled
             id 768
-            geometry 0 0 437.38 121
+            geometry 0 0 414.567 121
             minimum_size 80 40
             processing_node
               title dissassemble odometry
               id 769
-              geometry 10 33 185.047 78
+              geometry 10 33 155.125 78
               minimum_size 80 78
               library ros_nav_msgs_tools
               description odometry_dissassemble
@@ -388,7 +311,7 @@ project
             processing_node
               title dissassemble pose
               id 770
-              geometry 276.13 30 151.25 42
+              geometry 276.13 30 128.438 42
               minimum_size 80 42
               library ros_geometry_msgs_tools
               description pose_with_covariance_dissassemble
@@ -407,19 +330,19 @@ project
       group
         title debugger
         id 879
-        geometry 2397.47 1994 140.641 80
+        geometry 2387.47 1007 129.109 80
         minimum_size 100 80
         activity_state
           timeout 0
           title debugger
           id 880
-          geometry 10 30 120.641 149
+          geometry 10 30 109.109 40
           minimum_size 80 40
           library march_debugger_networking
           description debugger
           mode direct
           parameters
-            port 0
+            port 2001
             process 1 0
           end
           parameter_references
@@ -430,13 +353,13 @@ project
       group
         title a
         id 881
-        geometry 2839.47 1870 107.844 82
+        geometry 2852.47 935 101.953 82
         minimum_size 100 80
         activity_state
           timeout 0
           title send
           id 882
-          geometry 10 30 87.8438 42
+          geometry 10 30 81.9531 42
           minimum_size 80 42
           library march_debugger
           description send_message
@@ -460,13 +383,13 @@ project
       group
         title b
         id 898
-        geometry 2915.18 2027 107.844 82
+        geometry 2859.18 1060 101.953 82
         minimum_size 100 80
         activity_state
           timeout 0
           title send
           id 900
-          geometry 10 30 87.8438 42
+          geometry 10 30 81.9531 42
           minimum_size 80 42
           library march_debugger
           description send_message
@@ -489,13 +412,13 @@ project
       group
         title c
         id 899
-        geometry 2822.18 2166 107.844 82
+        geometry 2860.18 1180 101.953 82
         minimum_size 100 80
         activity_state
           timeout 0
           title send
           id 901
-          geometry 10 30 87.8438 42
+          geometry 10 30 81.9531 42
           minimum_size 80 42
           library march_debugger
           description send_message
@@ -516,7 +439,7 @@ project
         initial_state 901
       end
     end
-    title husky_control§
+    title husky_control
     connector
       id 35
       input
@@ -530,8 +453,8 @@ project
       id 83
       output
       external_event_connector_model
-        name to_idle
-        position 851.245 858
+        name to_idle_control
+        position 932.348 450
       end
     end
     connector
@@ -540,7 +463,7 @@ project
       external_data_connector_model
         name wheel_velocities
         type ros_sensor_msgs/joint_state
-        position 2551.29 104
+        position 2458.29 115
       end
     end
     connector
@@ -548,7 +471,7 @@ project
       pass_through
       pass_through_data_connector_model
         name linear
-        position 1939.53 579.446
+        position 2156.53 542.446
       end
     end
     connector
@@ -556,7 +479,7 @@ project
       pass_through
       pass_through_data_connector_model
         name angular
-        position 1940.18 690
+        position 2136.18 909
       end
     end
     connector
@@ -580,7 +503,7 @@ project
       pass_through
       pass_through_data_connector_model
         name left
-        position 3176.83 232
+        position 3259.83 133
       end
     end
     connector
@@ -588,7 +511,7 @@ project
       pass_through
       pass_through_data_connector_model
         name right
-        position 3175.83 370
+        position 3287.83 248
       end
     end
     connector
@@ -600,29 +523,12 @@ project
         position 16 276
       end
     end
-    connector
-      id 826
-      output
-      external_data_connector_model
-        name max_velocity
-        type ros_geometry_msgs/twist
-        position 2183.21 1339
-      end
-    end
     connector
       id 829
       pass_through
       pass_through_data_connector_model
         name position
-        position 2078.55 1296
-      end
-    end
-    connector
-      id 869
-      pass_through
-      pass_through_data_connector_model
-        name pose
-        position 2141.56 1170
+        position 1214.55 1022
       end
     end
     connector
@@ -630,7 +536,7 @@ project
       pass_through
       pass_through_data_connector_model
         name msg
-        position 2833.56 1909
+        position 2804.56 963
       end
     end
     connector
@@ -638,7 +544,7 @@ project
       pass_through
       pass_through_data_connector_model
         name msg
-        position 2861.56 2048
+        position 2786.56 1150
       end
     end
     connector
@@ -646,7 +552,7 @@ project
       pass_through
       pass_through_data_connector_model
         name msg
-        position 2773.56 2219
+        position 2783.56 1224
       end
     end
     connection
@@ -699,7 +605,7 @@ project
       id 154
       priority 2
       connector_id 83
-      middle_point 0.396451 0.622273
+      middle_point 0.462952 0.555505
       middle_of_connection_id 153
     end
     connection
@@ -710,7 +616,7 @@ project
         attachment_name value
         attachment_type std_msgs/float64
       end
-      middle_point 0.790915 -0.648488
+      middle_point 0.901445 -0.000819606
       connector_id 205
       data_connection_model 0
     end
@@ -734,7 +640,7 @@ project
       id 678
       priority 0
       connector_id 674
-      middle_point 0.943459 0.304264
+      middle_point 0.0553266 0.00880976
       middle_of_connection_id 115
       data_to_transition_connection_model event 
     end
@@ -742,7 +648,7 @@ project
       id 679
       priority 0
       connector_id 675
-      middle_point 0.997367 0.307008
+      middle_point 0.798818 -0.0100976
       middle_of_connection_id 115
       data_to_transition_connection_model event 
     end
@@ -902,7 +808,7 @@ project
         attachment_name message
         attachment_type ros_sensor_msgs/joint_state
       end
-      middle_point 0.259663 0.791666
+      middle_point 0.596229 0.65625
       connector_id 35
       data_connection_model 0
     end
@@ -960,7 +866,7 @@ project
       id 763
       priority 0
       connector_id 87
-      middle_point 1.01406 0.306427
+      middle_point 0.79119 0.0271986
       middle_of_connection_id 762
       data_to_transition_connection_model event 
     end
@@ -1017,117 +923,6 @@ project
       end
       data_connection_model 0
     end
-    connection
-      id 820
-      priority 0
-      transition_connector_model
-        abstract_state 819
-        position 0.675 1
-      end
-      middle_point -7.32667 0.577899
-      transition_connector_model
-        abstract_state 818
-        position 0.387119 0
-      end
-        transition_connection_model
-        events 
-        guard_expression 
-      end
-    end
-    connection
-      id 821
-      priority 0
-      transition_connector_model
-        abstract_state 818
-        position 0.56082 0
-      end
-      middle_point 1.94177 -1.2
-      transition_connector_model
-        abstract_state 819
-        position 0.9875 1
-      end
-        transition_connection_model
-        events finished
-        guard_expression std::abs(position.position.x - target.pose.position.x) < 0.1 && std::abs(position.position.y - target.pose.position.y) < 0.1
-      end
-    end
-    connection
-      id 822
-      priority 0
-      attached_data_connector_model
-        activity 819
-        attachment_name value
-        attachment_type std_msgs/float64
-      end
-      middle_point 0.318703 -2.9
-      connector_id 259
-      data_connection_model 0
-    end
-    connection
-      id 823
-      priority 0
-      attached_data_connector_model
-        activity 819
-        attachment_name value
-        attachment_type std_msgs/float64
-      end
-      middle_point 0.405342 -1.4
-      connector_id 258
-      data_connection_model 0
-    end
-    connection
-      id 824
-      priority 0
-      attached_data_connector_model
-        activity 818
-        attachment_name linear
-        attachment_type std_msgs/float64
-      end
-      middle_point 0.283896 -6.9
-      connector_id 258
-      data_connection_model 0
-    end
-    connection
-      id 825
-      priority 0
-      attached_data_connector_model
-        activity 818
-        attachment_name angular
-        attachment_type std_msgs/float64
-      end
-      middle_point 0.190555 -2.6
-      connector_id 259
-      data_connection_model 0
-    end
-    connection
-      id 827
-      priority 0
-      connector_id 826
-      middle_point 0.306871 0.347716
-      attached_data_connector_model
-        activity 818
-        attachment_name cmd_max_vel
-        attachment_type ros_geometry_msgs/twist
-      end
-      data_connection_model 0
-    end
-    connection
-      id 828
-      priority 0
-      transition_connector_model
-        abstract_state 818
-        position 0.40422 1
-      end
-      middle_point 0.0768766 7.1
-      transition_connector_model
-        abstract_state 818
-        position 0.87814 1
-      end
-        transition_connection_model
-        events 
-        guard_expression 
-      end
-    end
     connection
       id 830
       priority 0
@@ -1140,62 +935,6 @@ project
       connector_id 829
       data_connection_model 0
     end
-    connection
-      id 831
-      priority 0
-      connector_id 829
-      middle_point 0.540412 0.854368
-      middle_of_connection_id 828
-      data_to_transition_connection_model event 
-    end
-    connection
-      id 832
-      priority 0
-      connector_id 829
-      middle_point 0.295912 0.0733266
-      middle_of_connection_id 821
-      data_to_transition_connection_model data position
-    end
-    connection
-      id 833
-      priority 0
-      connector_id 829
-      middle_point 0.750829 0.544239
-      attached_data_connector_model
-        activity 818
-        attachment_name current_position
-        attachment_type ros_geometry_msgs/pose
-      end
-      data_connection_model 0
-    end
-    connection
-      id 872
-      priority 0
-      connector_id 869
-      middle_point 0.197649 0.471831
-      attached_data_connector_model
-        activity 818
-        attachment_name cmd_position
-        attachment_type ros_geometry_msgs/pose_stamped
-      end
-      data_connection_model 0
-    end
-    connection
-      id 873
-      priority 0
-      connector_id 869
-      middle_point 0.448369 0.255342
-      middle_of_connection_id 820
-      data_to_transition_connection_model event 
-    end
-    connection
-      id 878
-      priority 0
-      connector_id 869
-      middle_point 0.198596 -2.59487
-      middle_of_connection_id 821
-      data_to_transition_connection_model data target
-    end
     connection
       id 883
       priority 0
@@ -1212,23 +951,6 @@ project
       end
       data_connection_model 0
     end
-    connection
-      id 892
-      priority 1
-      transition_connector_model
-        abstract_state 888
-        position 1 0.75
-      end
-      middle_point 0.43856 0.5
-      transition_connector_model
-        abstract_state 886
-        position 0 0.0358515
-      end
-        transition_connection_model
-        events 
-        guard_expression 
-      end
-    end
     connection
       id 905
       priority 0
@@ -1288,7 +1010,7 @@ project
         attachment_name monitor
         attachment_type debugger_msgs/monitor
       end
-      middle_point 0.605777 1
+      middle_point 0.66645 0.754717
       attached_data_connector_model
         activity 900
         attachment_name monitor
@@ -1304,7 +1026,7 @@ project
         attachment_name monitor
         attachment_type debugger_msgs/monitor
       end
-      middle_point 0.914391 0.595238
+      middle_point 0.666867 0.560556
       attached_data_connector_model
         activity 901
         attachment_name monitor
@@ -1316,7 +1038,7 @@ project
       id 910
       priority 0
       connector_id 904
-      middle_point 0.5 0.5
+      middle_point 0.817753 0.454545
       attached_data_connector_model
         activity 901
         attachment_name value
@@ -1332,7 +1054,7 @@ project
       attached_data_connector_model
         activity 900
         attachment_name value
-        attachment_type std_msgs/empty
+        attachment_type std_msgs/float64
       end
       data_connection_model 0
     end
@@ -1344,7 +1066,7 @@ project
       attached_data_connector_model
         activity 882
         attachment_name value
-        attachment_type std_msgs/empty
+        attachment_type std_msgs/float64
       end
       data_connection_model 0
     end
@@ -1360,7 +1082,7 @@ project
       id 914
       priority 0
       connector_id 903
-      middle_point 0.5 0.5
+      middle_point 0.515228 -0.9
       middle_of_connection_id 906
       data_to_transition_connection_model event 
     end
@@ -1368,13 +1090,32 @@ project
       id 915
       priority 0
       connector_id 904
-      middle_point 0.5 0.5
+      middle_point 0.690429 -0.2
       middle_of_connection_id 907
       data_to_transition_connection_model event 
     end
+    connection
+      id 938
+      priority 0
+      connector_id 145
+      middle_point 0.5 0.5
+      connector_id 902
+      data_connection_model 0
+    end
+    connection
+      id 939
+      priority 0
+      connector_id 205
+      middle_point 0.5 0.5
+      connector_id 903
+      data_connection_model 0
+    end
     parameter float64 Kp 0.1
     parameter float64 Ki 0.001
     parameter float64 Kd 0.0001
+    parameter float64 Krho 0.5
+    parameter float64 Kalpha 0.7
+    parameter float64 Kbeta -0.2
     schedule_slot
       duration 1000000
     end
diff --git a/air_lab1/modules/air_lab1.mal b/air_lab1/modules/air_lab1.mal
index 6b7f8f471254d6d510ef54d538eeae89e1e7bab2..a3c4db7f89bc0d709576aec0046678ee37a044f3 100644
--- a/air_lab1/modules/air_lab1.mal
+++ b/air_lab1/modules/air_lab1.mal
@@ -5,6 +5,15 @@ cpp_include air_lab1_march.h
 component
   file components/air_lab1/reach_point.mhl
 end
+component
+  file components/air_lab1/reach_point_controller.mhl
+end
 component
   file components/air_lab1/linear_angular_to_left_right_velocities.mhl
 end
+component
+  file components/air_lab1/velocity_controller.mhl
+end
+component
+  file components/air_lab1/waypoints_controller.mhl
+end
diff --git a/air_lab1/modules/components/air_lab1/reach_point.mhl b/air_lab1/modules/components/air_lab1/reach_point.mhl
index 67a596c619c1abc3e35e32789f547bf7ddca46b7..65b9184fa48e02a070dcc82d096cf3c53e73ab98 100644
--- a/air_lab1/modules/components/air_lab1/reach_point.mhl
+++ b/air_lab1/modules/components/air_lab1/reach_point.mhl
@@ -1,16 +1,16 @@
 project
-  activity_prototypes_libraries ros.mal eigen3_geometry.mal ros_sensor_msgs_tools.mal ros_geometry_msgs_tools.mal control.mal ros_nav_msgs_tools.mal math.mal std_lib.mal
+  activity_prototypes_libraries control.mal eigen3_geometry.mal math.mal ros.mal ros_geometry_msgs_tools.mal ros_nav_msgs_tools.mal ros_sensor_msgs_tools.mal std_lib.mal
   processing_project
     processing_graph
       title untitled
       id 157
-      geometry 0 0 1645 570
+      geometry 0 0 1645 606
       minimum_size 80 40
       processing_node
         title twist dis
         id 183
-        geometry 878.434 73 80 40
-        minimum_size 80 40
+        geometry 878.434 71 80 42
+        minimum_size 80 42
         library ros_geometry_msgs_tools
         description twist_dissassemble
         mode direct
@@ -22,8 +22,8 @@ project
       processing_node
         title linear dis
         id 184
-        geometry 1053.35 38 80 40
-        minimum_size 80 40
+        geometry 1053.35 36 80 60
+        minimum_size 80 60
         library ros_geometry_msgs_tools
         description vector3_dissassemble
         mode direct
@@ -35,8 +35,8 @@ project
       processing_node
         title angular dis
         id 185
-        geometry 1189.35 241 80 40
-        minimum_size 80 40
+        geometry 1189.35 239 88.4219 60
+        minimum_size 80 60
         library ros_geometry_msgs_tools
         description vector3_dissassemble
         mode direct
@@ -48,8 +48,8 @@ project
       processing_node
         title diss point
         id 189
-        geometry 278.068 203 80 40
-        minimum_size 80 40
+        geometry 278.068 201 80.0625 60
+        minimum_size 80 60
         library ros_geometry_msgs_tools
         description point_dissassemble
         mode direct
@@ -61,8 +61,8 @@ project
       processing_node
         title pose dis
         id 190
-        geometry 170.983 264 80 40
-        minimum_size 80 40
+        geometry 170.983 262 80 42
+        minimum_size 80 42
         library ros_geometry_msgs_tools
         description pose_dissassemble
         mode direct
@@ -74,8 +74,8 @@ project
       processing_node
         title diss twist
         id 191
-        geometry 458.983 375 80 40
-        minimum_size 80 40
+        geometry 458.983 373 80 78
+        minimum_size 80 78
         library ros_geometry_msgs_tools
         description quaternion_dissassemble
         mode direct
@@ -87,8 +87,8 @@ project
       processing_node
         title to quaternion
         id 192
-        geometry 633.983 374 94.3125 40
-        minimum_size 80 40
+        geometry 633.983 372 101.813 78
+        minimum_size 80 78
         library march_eigen3_geometry
         description quaternion_create
         mode direct
@@ -112,8 +112,8 @@ project
       processing_node
         title to ypr
         id 193
-        geometry 762.897 373 80 40
-        minimum_size 80 40
+        geometry 762.897 371 80 60
+        minimum_size 80 60
         library march_eigen3_geometry
         description quaternion_to_ypr
         mode direct
@@ -125,8 +125,8 @@ project
       processing_node
         title dx^2
         id 213
-        geometry 663.145 202 80 40
-        minimum_size 80 40
+        geometry 663.145 200 80 42
+        minimum_size 80 42
         library march_math
         description multiplication
         mode direct
@@ -142,8 +142,8 @@ project
       processing_node
         title dy^2
         id 214
-        geometry 654.145 252 80 40
-        minimum_size 80 40
+        geometry 654.145 250 80 42
+        minimum_size 80 42
         library march_math
         description multiplication
         mode direct
@@ -159,8 +159,8 @@ project
       processing_node
         title rho^2
         id 215
-        geometry 798.145 221 80 40
-        minimum_size 80 40
+        geometry 798.145 219 80 42
+        minimum_size 80 42
         library march_math
         description addition
         mode direct
@@ -176,7 +176,7 @@ project
       processing_node
         title rho
         id 216
-        geometry 909.145 212 80 40
+        geometry 909.145 210 80 40
         minimum_size 80 40
         library march_math
         description sqrt
@@ -189,8 +189,8 @@ project
       processing_node
         title dx
         id 224
-        geometry 487.865 203 80 40
-        minimum_size 80 40
+        geometry 487.865 201 80 42
+        minimum_size 80 42
         library march_math
         description substraction
         mode direct
@@ -206,8 +206,8 @@ project
       processing_node
         title dy
         id 225
-        geometry 488.145 252 80 40
-        minimum_size 80 40
+        geometry 488.145 250 80 42
+        minimum_size 80 42
         library march_math
         description substraction
         mode direct
@@ -223,8 +223,8 @@ project
       processing_node
         title atan2(dy,dx)
         id 235
-        geometry 657.094 308 84.9688 40
-        minimum_size 80 40
+        geometry 657.094 306 95.5938 42
+        minimum_size 80 42
         library march_math
         description atan2
         mode direct
@@ -240,8 +240,8 @@ project
       processing_node
         title alpha
         id 238
-        geometry 926.609 309 80 40
-        minimum_size 80 40
+        geometry 926.609 307 80 42
+        minimum_size 80 42
         library march_math
         description substraction
         mode direct
@@ -257,8 +257,8 @@ project
       processing_node
         title beta
         id 241
-        geometry 927.609 372 80 40
-        minimum_size 80 40
+        geometry 927.609 370 80 42
+        minimum_size 80 42
         library march_math
         description substraction
         mode direct
@@ -278,8 +278,8 @@ project
       processing_node
         title v=krho*rho
         id 243
-        geometry 1042.29 235 80.0625 40
-        minimum_size 80 40
+        geometry 1042.29 233 90.0156 42
+        minimum_size 80 42
         library march_math
         description multiplication
         mode direct
@@ -299,8 +299,8 @@ project
       processing_node
         title kalpha*alpha
         id 244
-        geometry 1198 306 92.2656 40
-        minimum_size 80 40
+        geometry 1198 304 100.688 42
+        minimum_size 80 42
         library march_math
         description multiplication
         mode direct
@@ -320,8 +320,8 @@ project
       processing_node
         title kbeta*beta
         id 245
-        geometry 1238 389 80 40
-        minimum_size 80 40
+        geometry 1238 387 88.4375 42
+        minimum_size 80 42
         library march_math
         description multiplication
         mode direct
@@ -341,8 +341,8 @@ project
       processing_node
         title angular
         id 246
-        geometry 1351.99 322 80 40
-        minimum_size 80 40
+        geometry 1351.99 320 80 42
+        minimum_size 80 42
         library march_math
         description addition
         mode direct
@@ -358,7 +358,7 @@ project
       processing_node
         title angle_mod
         id 248
-        geometry 1036.61 303 80 40
+        geometry 1036.61 301 88.9375 40
         minimum_size 80 40
         library march_math
         description angle_mod
@@ -371,7 +371,7 @@ project
       processing_node
         title angle_mod
         id 249
-        geometry 1039 400 80 40
+        geometry 1039 398 88.9375 40
         minimum_size 80 40
         library march_math
         description angle_mod
@@ -384,8 +384,8 @@ project
       processing_node
         title clamp
         id 256
-        geometry 1405.61 107 80 40
-        minimum_size 80 40
+        geometry 1405.61 105 80 60
+        minimum_size 80 60
         library march_math
         description clamp
         mode direct
@@ -405,8 +405,8 @@ project
       processing_node
         title clamp
         id 257
-        geometry 1555 238 80 40
-        minimum_size 80 40
+        geometry 1555 236 80 60
+        minimum_size 80 60
         library march_math
         description clamp
         mode direct
@@ -426,8 +426,8 @@ project
       processing_node
         title -v_max
         id 269
-        geometry 1268.82 32 80 40
-        minimum_size 80 40
+        geometry 1268.82 30 80 42
+        minimum_size 80 42
         library march_math
         description substraction
         mode direct
@@ -447,8 +447,8 @@ project
       processing_node
         title -omega_max
         id 273
-        geometry 1385.15 251 89.9062 40
-        minimum_size 80 40
+        geometry 1385.15 249 98.5938 42
+        minimum_size 80 42
         library march_math
         description substraction
         mode direct
@@ -464,8 +464,8 @@ project
       processing_node
         title pose stamped dis
         id 330
-        geometry 10 254 119.625 40
-        minimum_size 80 40
+        geometry 10 252 122.266 42
+        minimum_size 80 42
         library ros_geometry_msgs_tools
         description pose_stamped_dissassemble
         mode direct
@@ -477,8 +477,8 @@ project
       processing_node
         title diss pose
         id 484
-        geometry 174 489 80 40
-        minimum_size 80 40
+        geometry 174 487 80 42
+        minimum_size 80 42
         library ros_geometry_msgs_tools
         description pose_dissassemble
         mode direct
@@ -490,8 +490,8 @@ project
       processing_node
         title diss point
         id 485
-        geometry 300 454 80 40
-        minimum_size 80 40
+        geometry 300 452 80.0625 60
+        minimum_size 80 60
         library ros_geometry_msgs_tools
         description point_dissassemble
         mode direct
@@ -503,8 +503,8 @@ project
       processing_node
         title diss quat
         id 486
-        geometry 301 520 80 40
-        minimum_size 80 40
+        geometry 301 518 80 78
+        minimum_size 80 78
         library ros_geometry_msgs_tools
         description quaternion_dissassemble
         mode direct
@@ -516,8 +516,8 @@ project
       processing_node
         title to quat
         id 487
-        geometry 430 520 80 40
-        minimum_size 80 40
+        geometry 430 518 80 78
+        minimum_size 80 78
         library march_eigen3_geometry
         description quaternion_create
         mode direct
@@ -541,8 +541,8 @@ project
       processing_node
         title to ypr
         id 488
-        geometry 561 520 80 40
-        minimum_size 80 40
+        geometry 561 518 80 60
+        minimum_size 80 60
         library march_eigen3_geometry
         description quaternion_to_ypr
         mode direct
@@ -554,7 +554,7 @@ project
       processing_node
         title krho
         id 500
-        geometry 909 259 80 40
+        geometry 909 257 80 40
         minimum_size 80 40
         library march_std_lib
         description output_float64
@@ -563,6 +563,8 @@ project
           name value
           queue_size 1
         end
+        parameters
+        end
         parameter_references
           value krho
         end
@@ -570,7 +572,7 @@ project
       processing_node
         title kalpha
         id 501
-        geometry 1092 353 80 40
+        geometry 1092 351 80 40
         minimum_size 80 40
         library march_std_lib
         description output_float64
@@ -579,6 +581,8 @@ project
           name value
           queue_size 1
         end
+        parameters
+        end
         parameter_references
           value kalpha
         end
@@ -586,7 +590,7 @@ project
       processing_node
         title kbeta
         id 502
-        geometry 1129 442 80 40
+        geometry 1129 440 80 40
         minimum_size 80 40
         library march_std_lib
         description output_float64
@@ -595,26 +599,12 @@ project
           name value
           queue_size 1
         end
+        parameters
+        end
         parameter_references
           value kbeta
         end
       end
-      processing_node
-        title new processing node
-        id 506
-        geometry 295 30 143.875 40
-        minimum_size 80 40
-        library march_std_lib
-        description print
-        mode direct
-        input_connector
-          name value
-          queue_size 1
-        end
-        parameters
-          text run
-        end
-      end
     end
     title reach_point
     connector
diff --git a/air_lab1/modules/components/air_lab1/reach_point_controller.mhl b/air_lab1/modules/components/air_lab1/reach_point_controller.mhl
new file mode 100644
index 0000000000000000000000000000000000000000..7338032c740ec16130dddd33dbadb46e748f2781
--- /dev/null
+++ b/air_lab1/modules/components/air_lab1/reach_point_controller.mhl
@@ -0,0 +1,325 @@
+project
+  activity_prototypes_libraries ../../air_lab1.mal control.mal eigen3_geometry.mal math.mal ros.mal ros_geometry_msgs_tools.mal ros_nav_msgs_tools.mal ros_sensor_msgs_tools.mal std_lib.mal
+  component
+    super_state
+      timeout 0
+      title reach_point_controller
+      id 0
+      geometry 10 30 515.016 513
+      minimum_size 80 40
+      group
+        title reach_point_controller
+        id 1
+        geometry 10 30 495.016 473
+        minimum_size 100 80
+        activity_state
+          timeout 0
+          title reach point
+          id 2
+          geometry 368 403 117.016 60
+          minimum_size 80 60
+          library air_lab1_march
+          description reach_point
+          mode direct
+          input_connector
+            name cmd_position
+            queue_size 1
+          end
+          input_connector
+            name cmd_max_vel
+            queue_size 1
+          end
+          input_connector
+            name current_position
+            queue_size 1
+          end
+          parameters
+          end
+          parameter_references
+            krho krho
+            kalpha kalpha
+            kbeta kbeta
+          end
+        end
+        activity_state
+          timeout 0
+          title idle
+          id 3
+          geometry 10 30 80 40
+          minimum_size 80 40
+          library march_std_lib
+          description output_float64
+          mode direct
+          input_connector
+            name value
+            queue_size 1
+          end
+          parameters
+          end
+          parameter_references
+          end
+        end
+        activity_state
+          timeout 0
+          title point_reached
+          id 19
+          geometry 187 89 133.813 40
+          minimum_size 80 40
+          mode direct
+        end
+        initial_state 3
+      end
+    end
+    title reach_point_controller
+    connector
+      id 4
+      input
+      external_data_connector_model
+        name vel_linear
+        type std_msgs/float64
+        position 696 102
+      end
+    end
+    connector
+      id 5
+      input
+      external_data_connector_model
+        name vel_angular
+        type std_msgs/float64
+        position 700 201
+      end
+    end
+    connector
+      id 6
+      output
+      external_data_connector_model
+        name cmd_pose
+        type ros_geometry_msgs/pose_stamped
+        position 106 285
+      end
+    end
+    connector
+      id 7
+      output
+      external_data_connector_model
+        name max_velocity
+        type ros_geometry_msgs/twist
+        position 111 317
+      end
+    end
+    connector
+      id 17
+      output
+      external_data_connector_model
+        name current_pose
+        type ros_geometry_msgs/pose
+        position 113 512
+      end
+    end
+    connector
+      id 29
+      input
+      external_event_connector_model
+        name point_reached
+        position 285 219
+      end
+    end
+    connection
+      id 8
+      priority 0
+      attached_data_connector_model
+        activity 2
+        attachment_name angular
+        attachment_type std_msgs/float64
+      end
+      middle_point 0.670025 1.2
+      connector_id 5
+      data_connection_model 0
+    end
+    connection
+      id 9
+      priority 0
+      attached_data_connector_model
+        activity 2
+        attachment_name linear
+        attachment_type std_msgs/float64
+      end
+      middle_point 0.485359 0.5
+      connector_id 4
+      data_connection_model 0
+    end
+    connection
+      id 10
+      priority 0
+      attached_data_connector_model
+        activity 3
+        attachment_name value
+        attachment_type std_msgs/float64
+      end
+      middle_point 0.509059 0.5
+      connector_id 4
+      data_connection_model 0
+    end
+    connection
+      id 11
+      priority 0
+      attached_data_connector_model
+        activity 3
+        attachment_name value
+        attachment_type std_msgs/float64
+      end
+      middle_point 0.889831 0.166667
+      connector_id 5
+      data_connection_model 0
+    end
+    connection
+      id 12
+      priority 0
+      connector_id 6
+      middle_point 0.369749 0.5
+      attached_data_connector_model
+        activity 2
+        attachment_name cmd_position
+        attachment_type ros_geometry_msgs/pose_stamped
+      end
+      data_connection_model 0
+    end
+    connection
+      id 13
+      priority 0
+      connector_id 7
+      middle_point 0.299175 0.445055
+      attached_data_connector_model
+        activity 2
+        attachment_name cmd_max_vel
+        attachment_type ros_geometry_msgs/twist
+      end
+      data_connection_model 0
+    end
+    connection
+      id 18
+      priority 0
+      connector_id 17
+      middle_point 0.354359 0.5
+      attached_data_connector_model
+        activity 2
+        attachment_name current_position
+        attachment_type ros_geometry_msgs/pose
+      end
+      data_connection_model 0
+    end
+    connection
+      id 20
+      priority 0
+      transition_connector_model
+        abstract_state 2
+        position 0.632394 0
+      end
+      middle_point 7.2 -6.8
+      transition_connector_model
+        abstract_state 19
+        position 1 0.675
+      end
+        transition_connection_model
+        events finished
+        guard_expression std::abs(position.position.x - target.pose.position.x) < 0.1 && std::abs(position.position.y - target.pose.position.y) < 0.1
+      end
+    end
+    connection
+      id 21
+      priority 0
+      transition_connector_model
+        abstract_state 19
+        position 0 0.6
+      end
+      middle_point 0.5 0.5
+      transition_connector_model
+        abstract_state 3
+        position 1 0.775
+      end
+        transition_connection_model
+        events finished
+        guard_expression 
+      end
+    end
+    connection
+      id 22
+      priority 0
+      transition_connector_model
+        abstract_state 3
+        position 0.75 1
+      end
+      middle_point 0.680921 0.775281
+      transition_connector_model
+        abstract_state 2
+        position 0.239284 0
+      end
+        transition_connection_model
+        events 
+        guard_expression 
+      end
+    end
+    connection
+      id 23
+      priority 0
+      connector_id 6
+      middle_point 0.767919 -0.00961538
+      middle_of_connection_id 22
+      data_to_transition_connection_model event 
+    end
+    connection
+      id 24
+      priority 0
+      connector_id 6
+      middle_point 0.281699 0.4
+      middle_of_connection_id 20
+      data_to_transition_connection_model data target
+    end
+    connection
+      id 25
+      priority 0
+      connector_id 17
+      middle_point 0.184099 0.9
+      middle_of_connection_id 20
+      data_to_transition_connection_model data position
+    end
+    connection
+      id 26
+      priority 0
+      transition_connector_model
+        abstract_state 2
+        position 0.358926 1
+      end
+      middle_point 0.579983 3.615
+      transition_connector_model
+        abstract_state 2
+        position 1 0.783333
+      end
+        transition_connection_model
+        events 
+        guard_expression 
+      end
+    end
+    connection
+      id 27
+      priority 0
+      connector_id 17
+      middle_point 0.0795219 0.6
+      middle_of_connection_id 26
+      data_to_transition_connection_model event 
+    end
+    connection
+      id 30
+      priority 0
+      attached_event_connector_model
+        activity 19
+        attachment_name finished
+      end
+      middle_point 0.5 0.5
+      connector_id 29
+    end
+    parameter float64 krho 0
+    parameter float64 kalpha 0
+    parameter float64 kbeta 0
+  end
+end
diff --git a/air_lab1/modules/components/air_lab1/velocity_controller.mhl b/air_lab1/modules/components/air_lab1/velocity_controller.mhl
new file mode 100644
index 0000000000000000000000000000000000000000..b3e2d32d704faf4c9eeffb07c9e5a7e72a12f51c
--- /dev/null
+++ b/air_lab1/modules/components/air_lab1/velocity_controller.mhl
@@ -0,0 +1,194 @@
+project
+  activity_prototypes_libraries ros_geometry_msgs_tools.mal
+  component
+    super_state
+      timeout 0
+      title untitled
+      id 0
+      geometry 10 30 265 265
+      minimum_size 80 40
+      group
+        title untitled
+        id 1
+        geometry 10 30 245 225
+        minimum_size 100 80
+        processing_state
+          timeout 0
+          title decompose velocity
+          id 15
+          geometry 10 30 225 185
+          minimum_size 80 40
+          processing_graph
+            title untitled
+            id 16
+            geometry 0 0 225 185
+            minimum_size 80 40
+            processing_node
+              title twist
+              id 17
+              geometry 10 57 80 42
+              minimum_size 80 42
+              library ros_geometry_msgs_tools
+              description twist_dissassemble
+              mode direct
+              input_connector
+                name message
+                queue_size 1
+              end
+            end
+            processing_node
+              title linear
+              id 24
+              geometry 131 30 80 60
+              minimum_size 80 60
+              library ros_geometry_msgs_tools
+              description vector3_dissassemble
+              mode direct
+              input_connector
+                name message
+                queue_size 1
+              end
+            end
+            processing_node
+              title angular
+              id 25
+              geometry 135 115 80 60
+              minimum_size 80 60
+              library ros_geometry_msgs_tools
+              description vector3_dissassemble
+              mode direct
+              input_connector
+                name message
+                queue_size 1
+              end
+            end
+          end
+          activity_mode task
+          task_priority 2
+        end
+        initial_state 15
+      end
+    end
+    title velocity_controller
+    connector
+      id 6
+      input
+      external_data_connector_model
+        name linear
+        type std_msgs/float64
+        position 345 132
+      end
+    end
+    connector
+      id 7
+      input
+      external_data_connector_model
+        name angular
+        type std_msgs/float64
+        position 345 253
+      end
+    end
+    connector
+      id 8
+      output
+      external_data_connector_model
+        name cmd_vel
+        type ros_geometry_msgs/twist
+        position 47 70
+      end
+    end
+    connection
+      id 2
+      priority 0
+      transition_connector_model
+        abstract_state 0
+        position 0.1 0
+      end
+      middle_point 0.511111 -1.5
+      transition_connector_model
+        abstract_state 0
+        position 0.85 0
+      end
+        transition_connection_model
+        events 
+        guard_expression 
+      end
+    end
+    connection
+      id 9
+      priority 0
+      connector_id 8
+      middle_point 0.5 0.5
+      attached_data_connector_model
+        activity 17
+        attachment_name message
+        attachment_type ros_geometry_msgs/twist
+      end
+      data_connection_model 0
+    end
+    connection
+      id 14
+      priority 0
+      connector_id 8
+      middle_point 0.517092 -0.5
+      middle_of_connection_id 2
+      data_to_transition_connection_model event 
+    end
+    connection
+      id 26
+      priority 0
+      attached_data_connector_model
+        activity 17
+        attachment_name linear
+        attachment_type ros_geometry_msgs/vector3
+      end
+      middle_point 0.5 0.5
+      attached_data_connector_model
+        activity 24
+        attachment_name message
+        attachment_type ros_geometry_msgs/vector3
+      end
+      data_connection_model 0
+    end
+    connection
+      id 27
+      priority 0
+      attached_data_connector_model
+        activity 24
+        attachment_name x
+        attachment_type std_msgs/float64
+      end
+      middle_point 0.5 0.5
+      connector_id 6
+      data_connection_model 0
+    end
+    connection
+      id 28
+      priority 0
+      attached_data_connector_model
+        activity 17
+        attachment_name angular
+        attachment_type ros_geometry_msgs/vector3
+      end
+      middle_point 0.5 0.5
+      attached_data_connector_model
+        activity 25
+        attachment_name message
+        attachment_type ros_geometry_msgs/vector3
+      end
+      data_connection_model 0
+    end
+    connection
+      id 29
+      priority 0
+      attached_data_connector_model
+        activity 25
+        attachment_name z
+        attachment_type std_msgs/float64
+      end
+      middle_point 0.5 0.5
+      connector_id 7
+      data_connection_model 0
+    end
+  end
+end
diff --git a/air_lab1/modules/components/air_lab1/waypoints_controller.mhl b/air_lab1/modules/components/air_lab1/waypoints_controller.mhl
new file mode 100644
index 0000000000000000000000000000000000000000..5004a71790ed09c3a73eebfcdd0e19f85ebd6ec9
--- /dev/null
+++ b/air_lab1/modules/components/air_lab1/waypoints_controller.mhl
@@ -0,0 +1,473 @@
+project
+  activity_prototypes_libraries ../../air_lab1.mal control.mal eigen3_geometry.mal list.mal math.mal ros.mal ros_geometry_msgs_tools.mal ros_nav_msgs_tools.mal ros_sensor_msgs_tools.mal std_lib.mal
+  component
+    super_state
+      timeout 0
+      title untitled
+      id 0
+      geometry 10 30 788.125 245
+      minimum_size 80 40
+      group
+        title untitled
+        id 1
+        geometry 10 30 768.125 205
+        minimum_size 100 80
+        activity_state
+          timeout 0
+          title idle
+          id 2
+          geometry 10 30 80 40
+          minimum_size 80 40
+          library march_std_lib
+          description output_float64
+          mode direct
+          input_connector
+            name value
+            queue_size 1
+          end
+          parameters
+          end
+          parameter_references
+          end
+        end
+        activity_state
+          timeout 0
+          title decompose
+          id 3
+          geometry 94 153 118.25 42
+          minimum_size 80 42
+          library ros_nav_msgs_tools
+          description path_dissassemble
+          mode direct
+          input_connector
+            name message
+            queue_size 1
+          end
+        end
+        activity_state
+          timeout 0
+          title pop
+          id 4
+          geometry 347 48 80 42
+          minimum_size 80 42
+          library march_list
+          description list_pop
+          mode direct
+          input_connector
+            name list
+            queue_size 1
+          end
+        end
+        activity_state
+          timeout 0
+          title reach point controller
+          id 5
+          geometry 586 77 172.125 60
+          minimum_size 80 60
+          library air_lab1_march
+          description reach_point
+          mode direct
+          input_connector
+            name cmd_position
+            queue_size 1
+          end
+          input_connector
+            name cmd_max_vel
+            queue_size 1
+          end
+          input_connector
+            name current_position
+            queue_size 1
+          end
+          parameters
+          end
+          parameter_references
+            krho krho
+            kalpha kalpha
+            kbeta kbeta
+          end
+        end
+        initial_state 2
+      end
+    end
+    title waypoints_controller
+    connector
+      id 7
+      output
+      external_data_connector_model
+        name waypoints
+        type ros_nav_msgs/path
+        position 68 201
+      end
+    end
+    connector
+      id 8
+      output
+      external_data_connector_model
+        name max_velocity
+        type ros_geometry_msgs/twist
+        position 480 211
+      end
+    end
+    connector
+      id 9
+      output
+      external_data_connector_model
+        name current_pose
+        type ros_geometry_msgs/pose
+        position 479 233
+      end
+    end
+    connector
+      id 22
+      input
+      external_event_connector_model
+        name waypoints_finished
+        position 346 242
+      end
+    end
+    connector
+      id 24
+      input
+      external_data_connector_model
+        name linear
+        type std_msgs/float64
+        position 911 94
+      end
+    end
+    connector
+      id 25
+      input
+      external_data_connector_model
+        name angular
+        type std_msgs/float64
+        position 889 218
+      end
+    end
+    connector
+      id 47
+      pass_through
+      pass_through_data_connector_model
+        name idle
+        position 762 93
+      end
+    end
+    connection
+      id 10
+      priority 0
+      transition_connector_model
+        abstract_state 0
+        position 0.331469 0
+      end
+      middle_point 0.5 -1.4
+      transition_connector_model
+        abstract_state 0
+        position 0.80122 0
+      end
+        transition_connection_model
+        events 
+        guard_expression 
+      end
+    end
+    connection
+      id 12
+      priority 0
+      transition_connector_model
+        abstract_state 2
+        position 0.7375 1
+      end
+      middle_point 0.5 0.5
+      transition_connector_model
+        abstract_state 3
+        position 0.591966 0
+      end
+        transition_connection_model
+        events 
+        guard_expression 
+      end
+    end
+    connection
+      id 14
+      priority 0
+      attached_data_connector_model
+        activity 3
+        attachment_name poses
+        attachment_type ros_geometry_msgs/pose_stamped_list
+      end
+      middle_point 0.5 0.5
+      attached_data_connector_model
+        activity 4
+        attachment_name list
+        attachment_type ros_geometry_msgs/pose_stamped_list
+      end
+      data_connection_model 0
+    end
+    connection
+      id 15
+      priority 0
+      attached_data_connector_model
+        activity 4
+        attachment_name list
+        attachment_type ros_geometry_msgs/pose_stamped_list
+      end
+      middle_point 0.1 -2.1
+      attached_data_connector_model
+        activity 4
+        attachment_name list
+        attachment_type ros_geometry_msgs/pose_stamped_list
+      end
+      data_connection_model 0
+    end
+    connection
+      id 23
+      priority 0
+      attached_event_connector_model
+        activity 4
+        attachment_name empty_list
+      end
+      middle_point 0.5 0.5
+      connector_id 22
+    end
+    connection
+      id 28
+      priority 0
+      connector_id 7
+      middle_point 0.5 0.5
+      middle_of_connection_id 10
+      data_to_transition_connection_model event 
+    end
+    connection
+      id 29
+      priority 0
+      connector_id 7
+      middle_point 0.5 0.5
+      middle_of_connection_id 12
+      data_to_transition_connection_model event 
+    end
+    connection
+      id 30
+      priority 0
+      transition_connector_model
+        abstract_state 3
+        position 0.811839 0
+      end
+      middle_point 0.157895 0.9
+      transition_connector_model
+        abstract_state 4
+        position 0 0.52381
+      end
+        transition_connection_model
+        events finished
+        guard_expression 
+      end
+    end
+    connection
+      id 31
+      priority 0
+      transition_connector_model
+        abstract_state 4
+        position 0.825 1
+      end
+      middle_point 0.625561 2.73077
+      transition_connector_model
+        abstract_state 5
+        position 0.517066 1
+      end
+        transition_connection_model
+        events 
+        guard_expression 
+      end
+    end
+    connection
+      id 32
+      priority 0
+      transition_connector_model
+        abstract_state 5
+        position 0.395061 0
+      end
+      middle_point 6.1 -2.9
+      transition_connector_model
+        abstract_state 4
+        position 0.525 0
+      end
+        transition_connection_model
+        events point_reached,finished
+        guard_expression std::abs(position.position.x - target.pose.position.x) < 0.1 && std::abs(position.position.y - target.pose.position.y) < 0.1
+      end
+    end
+    connection
+      id 33
+      priority 0
+      attached_data_connector_model
+        activity 4
+        attachment_name value
+        attachment_type ros_geometry_msgs/pose_stamped
+      end
+      middle_point 0.371094 0.404983
+      middle_of_connection_id 31
+      data_to_transition_connection_model event 
+    end
+    connection
+      id 36
+      priority 0
+      connector_id 9
+      middle_point 0.5 0.5
+      middle_of_connection_id 32
+      data_to_transition_connection_model data position
+    end
+    connection
+      id 37
+      priority 0
+      attached_data_connector_model
+        activity 4
+        attachment_name value
+        attachment_type ros_geometry_msgs/pose_stamped
+      end
+      middle_point 0.5 0.5
+      middle_of_connection_id 32
+      data_to_transition_connection_model data target
+    end
+    connection
+      id 38
+      priority 0
+      attached_data_connector_model
+        activity 5
+        attachment_name linear
+        attachment_type std_msgs/float64
+      end
+      middle_point 0.5 0.5
+      connector_id 24
+      data_connection_model 0
+    end
+    connection
+      id 39
+      priority 0
+      attached_data_connector_model
+        activity 5
+        attachment_name angular
+        attachment_type std_msgs/float64
+      end
+      middle_point 0.5 0.5
+      connector_id 25
+      data_connection_model 0
+    end
+    connection
+      id 40
+      priority 0
+      connector_id 8
+      middle_point 0.5 0.5
+      attached_data_connector_model
+        activity 5
+        attachment_name cmd_max_vel
+        attachment_type ros_geometry_msgs/twist
+      end
+      data_connection_model 0
+    end
+    connection
+      id 41
+      priority 0
+      connector_id 9
+      middle_point 0.673228 0.8
+      attached_data_connector_model
+        activity 5
+        attachment_name current_position
+        attachment_type ros_geometry_msgs/pose
+      end
+      data_connection_model 0
+    end
+    connection
+      id 42
+      priority 0
+      attached_data_connector_model
+        activity 4
+        attachment_name value
+        attachment_type ros_geometry_msgs/pose_stamped
+      end
+      middle_point 0.5 0.5
+      attached_data_connector_model
+        activity 5
+        attachment_name cmd_position
+        attachment_type ros_geometry_msgs/pose_stamped
+      end
+      data_connection_model 0
+    end
+    connection
+      id 43
+      priority 0
+      connector_id 7
+      middle_point 0.5 0.5
+      attached_data_connector_model
+        activity 3
+        attachment_name message
+        attachment_type ros_nav_msgs/path
+      end
+      data_connection_model 0
+    end
+    connection
+      id 44
+      priority 0
+      transition_connector_model
+        abstract_state 5
+        position 1 0.85
+      end
+      middle_point 1.2 4.7
+      transition_connector_model
+        abstract_state 5
+        position 0.737836 1
+      end
+        transition_connection_model
+        events 
+        guard_expression 
+      end
+    end
+    connection
+      id 45
+      priority 0
+      connector_id 9
+      middle_point 0.5 0.5
+      middle_of_connection_id 44
+      data_to_transition_connection_model event 
+    end
+    connection
+      id 48
+      priority 0
+      attached_data_connector_model
+        activity 2
+        attachment_name value
+        attachment_type std_msgs/float64
+      end
+      middle_point 0.5 0.5
+      connector_id 47
+      data_connection_model 0
+    end
+    connection
+      id 49
+      priority 0
+      connector_id 47
+      middle_point 0.5 0.5
+      connector_id 24
+      data_connection_model 0
+    end
+    connection
+      id 50
+      priority 0
+      connector_id 47
+      middle_point 0.5 0.5
+      connector_id 25
+      data_connection_model 0
+    end
+    connection
+      id 51
+      priority 0
+      attached_event_connector_model
+        activity 4
+        attachment_name empty_list
+      end
+      middle_point 0.5 0.5
+      middle_of_connection_id 10
+    end
+    parameter float64 krho 0
+    parameter float64 kalpha 0
+    parameter float64 kbeta 0
+  end
+end
diff --git a/air_lab1/msg/ControllerEvaluationStat.msg b/air_lab1/msg/ControllerEvaluationStat.msg
new file mode 100644
index 0000000000000000000000000000000000000000..8f7c73a4e5291e81397248d6a0d2ef6c107b4bbc
--- /dev/null
+++ b/air_lab1/msg/ControllerEvaluationStat.msg
@@ -0,0 +1,3 @@
+int32 samples           # Number of samples used for computing the error
+float64 last_error      # Last error
+float64 average_error   # Average error
diff --git a/air_lab1/src/autotune.py b/air_lab1/src/autotune.py
index 11d27a9b1a53d0bb538e9bd58fd403545f13cc67..113ff6e5012d3b15653ec55a75b3de55f263d6c9 100755
--- a/air_lab1/src/autotune.py
+++ b/air_lab1/src/autotune.py
@@ -9,17 +9,51 @@ import air_lab1.msg
 import subprocess
 import threading
 
-class auto_tune:
+class exhaustive_config:
+  def __init__(self):
+    pass
+  def next_step(self, tuner):
+    tuner.increment_count = 50
+    if tuner.value_index == 0:
+      tuner.start = 0
+      tuner.end   = 1.0
+    elif tuner.value_index == 1:
+      tuner.start = 0
+      tuner.end   = 0.01
+    elif tuner.value_index == 2:
+      tuner.start = 0
+      tuner.end   = 0.001
+      
+class student_config:
   def __init__(self):
+    pass
+  def next_step(self, tuner):
+    tuner.increment_count = 10
+    if tuner.value_index == 0:
+      tuner.start = 0.4
+      tuner.end   = 0.8
+    elif tuner.value_index == 1:
+      tuner.start = 0.008
+      tuner.end   = 0.01
+    elif tuner.value_index == 2:
+      tuner.start = 0.0008
+      tuner.end   = 0.001
+
+
+
+class auto_tune:
+  def __init__(self, config):
+    self.config = config
     self.evaluation_stat_sub = rospy.Subscriber("evaluation_stat", air_lab1.msg.ControllerEvaluationStat, self.evaluation_stat_callback)
     self.velocity_pub =  rospy.Publisher("cmd_vel", geometry_msgs.msg.Twist, queue_size = 1)
     self.to_vel_ctrl_pub = rospy.Publisher("to_vel_control", std_msgs.msg.Empty, queue_size = 1)
     
-    self.increment_count = 10
+    self.increment_count = 40
     
-    self.start = 0.3
-    self.end = 0.8
+    self.start = 0.2
+    self.end = 1.0
     self.value_index = 0
+    self.config.next_step(self)
     
     
     self.best_values = [0, 0, 0]
@@ -38,6 +72,9 @@ class auto_tune:
     self.increment = (self.end - self.start) / self.increment_count
     self.values = list(self.best_values)
     self.values[self.value_index] = self.start
+    if self.start == 0:
+      self.values[self.value_index] += self.increment_count
+    
     
   def evaluation_stat_callback(self, msg):
     self.sum_error += msg.last_error
@@ -48,7 +85,7 @@ class auto_tune:
     
   def step0(self):
     print("Start testing: {}".format(self.values))
-    self.controller_process = subprocess.Popen(["rosrun", "nodelet", "nodelet", "standalone", "air_lab1/husky_control_node", "__ns:=/husky0", "_Kp:={}".format(self.values[0]), "_Ki:={}".format(self.values[1]), "_Kd:={}".format(self.values[2])])
+    self.controller_process = subprocess.Popen(["rosrun", "nodelet", "nodelet", "standalone", "air_lab1/husky_control_node", "__ns:=/husky0", "_Kp:={}".format(self.values[0]), "_Kd:={}".format(self.values[1]), "_Ki:={}".format(self.values[2])])
     self.timer = threading.Timer(self.init_step_time, self.step1)
     self.timer.start()
 
@@ -82,14 +119,12 @@ class auto_tune:
     if self.values[self.value_index] > self.end:
       if self.value_index == 0:
         self.value_index = 1
-        self.start = 0.006
-        self.end = 0.009
+        self.config.next_step(self)
       elif self.value_index == 1:
         self.value_index = 2
-        self.start = 0.0002
-        self.end = 0.0006
+        self.config.next_step(self)
       else:
-        print("Best values are [Kp, Ki, Kd] = {} with error {}".format(self.best_values, self.lowest_error))
+        print("Best values are [Kp, Kd, Ki] = {} with error {}".format(self.best_values, self.lowest_error))
         rospy.signal_shutdown("Timed out waiting for Action Server")
         sys.exit(1)     
       
@@ -105,7 +140,7 @@ class auto_tune:
 
 if __name__ == '__main__':
     rospy.init_node('autotune', anonymous=False)
-    at = auto_tune()
+    at = auto_tune(student_config())
     rospy.spin()
     #while not rospy.is_shutdown():
         #obj.data = 536