diff --git a/src/segway_RMP400.cpp b/src/segway_RMP400.cpp
index 75e07f3496f43dc309bf6719d2817fe896cc9bbf..8661dfa2a78bd3144c48f9ba111af673308f86bd 100644
--- a/src/segway_RMP400.cpp
+++ b/src/segway_RMP400.cpp
@@ -12,6 +12,10 @@ CSegwayRMP400::CSegwayRMP400() :
     use_correction_wheel_factor_(true)
 {
     scan_ftdi_for_segways();
+    for (unsigned int i = 0;i < NUM_SEGWAY_200 ;i++) {
+      CSegwayRMP200 * segway = new CSegwayRMP200();
+      segways_.push_back(segway);
+    }
 }
 
 CSegwayRMP400::CSegwayRMP400(const std::string serial1, const std::string serial2) :
@@ -55,20 +59,22 @@ CSegwayRMP400::scan_ftdi_for_segways()
 void
 CSegwayRMP400::start()
 {
-    for (unsigned int i = 0;i < NUM_SEGWAY_200 ;i++) {
+  for (unsigned int i = 0;i < segways_.size();i++) {
+//     for (unsigned int i = 0;i < NUM_SEGWAY_200 ;i++) {
 
         try
         {
-            CSegwayRMP200 * segway = new CSegwayRMP200(serial_ftdi_segway_devices_[i]);
-            segways_.push_back(segway);
+//             CSegwayRMP200 * segway = new CSegwayRMP200();
+            segways_[i]->connect(serial_ftdi_segway_devices_[i]);
+//             segways_.push_back(segway);
         }
         catch (CSegwayRMP200Exception & e)
         {
             // Something failed while starting engines
             // clean up previous (if any) started engine
             for (unsigned int i = 0; i < segways_.size(); i++) {
-                segways_[i]->stop();
-                delete segways_[i];
+                segways_[i]->close();
+               // delete segways_[i];
             }
 
             status_ = rmp400_off;
@@ -79,6 +85,14 @@ CSegwayRMP400::start()
     status_ = rmp400_connected;
 }
 
+void
+CSegwayRMP400::close()
+{
+  for (unsigned int i = 0;i < segways_.size();i++) {
+    segways_[i]->close();
+  }
+}
+
 std::ostream &
 operator<< (std::ostream& data, CSegwayRMP400 & segway)
 {
@@ -271,6 +285,6 @@ CSegwayRMP400::stop()
 
 CSegwayRMP400::~CSegwayRMP400()
 {
-    for (unsigned int i=0;i < segways_.size(); i++) 
+    for (unsigned int i=0;i < segways_.size(); i++)
         delete segways_[i];
 }
diff --git a/src/segway_RMP400.h b/src/segway_RMP400.h
index 5c18443d451a29e3a7469820611e21b58a5f1f19..67b66e38fc278f0674847a57b4a1cefda279a6e6 100644
--- a/src/segway_RMP400.h
+++ b/src/segway_RMP400.h
@@ -42,7 +42,7 @@ class CSegwayRMP400
     /**
      * \brief yaw displacement
      *
-     * This value has the rotation of the whole platform in revolutions. 
+     * This value has the rotation of the whole platform in revolutions.
      * TODO: should be a thread periodically updating this value
      */
     float yaw_displacement_;
@@ -50,7 +50,7 @@ class CSegwayRMP400
     /**
      * \brief yaw rate
      *
-     * This value has the yaw rate of the platform in degrees per second. 
+     * This value has the yaw rate of the platform in degrees per second.
      * TODO: should be a thread periodically updating this value
      */
     float yaw_rate_;
@@ -150,7 +150,7 @@ class CSegwayRMP400
     /**
      * \brief function to return the total forward displacement
      *
-     * This function returns the current forward displacement in meters. This 
+     * This function returns the current forward displacement in meters. This
      * function only returns the value of the internal attribute, but it does
      * not access the hardware platform. This value is periodically updated by
      * the feedback thread.
@@ -165,9 +165,9 @@ class CSegwayRMP400
     /**
      * \brief function to return the total yaw displacement
      *
-     * This function returns the current yaw displacement in revolutions per 
+     * This function returns the current yaw displacement in revolutions per
      * second. This function only returns the value of the internal attribute,
-     * but it does not access the hardware platform. This value is periodically 
+     * but it does not access the hardware platform. This value is periodically
      * updated by the feedback thread.
      *
      * Both yaw displacement from segways are sum and a mean is calculated
@@ -180,9 +180,9 @@ class CSegwayRMP400
     /**
      * \brief function to return the yaw rate
      *
-     * This function returns the current yaw rate in revolutions per second. 
-     * This function only returns the value of the internal attribute, but it 
-     * does not access the hardware platform. This value is periodically updated 
+     * This function returns the current yaw rate in revolutions per second.
+     * This function only returns the value of the internal attribute, but it
+     * does not access the hardware platform. This value is periodically updated
      * by the feedback thread.
      *
      * Both yaw rate from segways are sum and a mean is calculated
@@ -225,6 +225,9 @@ class CSegwayRMP400
     * This function stops the vehicle
     */
     void stop();
+
+    void close();
+
   friend std::ostream& operator<< (std::ostream& out, CSegwayRMP400& segway);
 };