Diff: readMotion/readMotion.cpp

Revision:

20:abb7852df747

Parent:

19:c2ec475367aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/readMotion/readMotion.cpp	Mon Aug 19 08:10:58 2013 +0000
@@ -0,0 +1,36 @@
+#include "readMotion.h"
+
+using namespace std;
+
+bool readMotion(string filename, uint16_t* pos, int &servo_size, int &motion_size, int* pose_size) {
+    fstream file;
+    string line, tmp;
+    stringstream sstr;
+    
+    file.open(filename.c_str(), ios::in);
+    if(! file.is_open()) {
+        return false;
+    }
+    
+    int i = 0, id = 0;
+    while(getline(file, line)) {
+    
+        std::replace(line.begin(), line.end(), ',', ' ');
+        sstr.clear();
+        sstr.str(line);
+        while (sstr >> tmp) {
+            if (tmp == "d") {
+                while (sstr >> pos[i]) ++i;
+            } else if (tmp == "servo_size") {
+                sstr >> servo_size;
+            } else if (tmp == "motion_size") {
+                sstr >> motion_size;
+            } else if (tmp == "pose_size") {
+                sstr >> pose_size[id];
+                ++id;
+            }
+        }
+    }
+    file.close();
+    return true;
+}
\ No newline at end of file