Diff: Motions.cpp

Revision:

13:711f74b2fa33

Parent:

12:6cd135bf03bd

Child:

14:522bb06f0f0d

diff -r 6cd135bf03bd -r 711f74b2fa33 Motions.cpp
--- a/Motions.cpp	Sun Feb 03 04:53:44 2013 +0000
+++ b/Motions.cpp	Sun Feb 03 21:22:21 2013 +0000
@@ -9,12 +9,15 @@
 extern uint16_t data[0x2000] __attribute__((section("AHBSRAM1")));
 extern Ticker tick;
 
+//Serial serial(USBTX, USBRX);
+
 Motions::Motions(uint16_t* data)
 {
     LocalFileSystem* local = new LocalFileSystem("local");
     read("/local/motion.csv", data);
     set(data);
     playing = false;
+    
     comu = new SCI(USBTX, USBRX);
 }
 
@@ -28,13 +31,15 @@
 
 void Motions::read(const string& filename, uint16_t* data)
 {
-    int srv, mtn;
-    int *pse = new int;
     CSV csv;
-    csv.read(filename, data, &srv, &mtn, pse);
-    servo_size = srv;
-    motion_size = mtn;
-    pose_size = pse;
+    pose_size = new int;
+    csv.read(filename, data, &servo_size, &motion_size, pose_size);
+    
+    //serial.printf("readed!\r\n");
+    //serial.printf("servo_size:%d motion_size:%d\r\n", servo_size, motion_size);
+    //for (int i = 0; i < motion_size; ++i) {
+        //serial.printf("motion %d pose_size:%d\r\n", i, pose_size[i]);
+    //}
 }
 
 void Motions::set(uint16_t* data)
@@ -43,7 +48,7 @@
     size_z = motion_size;
     size_x = servo_size;
     
-    uint16_t*** motions = new uint16_t**[size_z];
+    motions = new uint16_t**[size_z];
     uint16_t* p = data;
     
     for (int i = 0; i < size_z; ++i) {
@@ -56,6 +61,15 @@
     }
 }
 
+bool Motions::checkid(int id)
+{
+    if (id >= 0 && id < motion_size) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
 void Motions::play()
 {
     if (playing) {
@@ -66,9 +80,10 @@
     }
 }
 
-void Motions::setmotion(const int& id)
+void Motions::setmotion(const int id)
 {
     if (!playing) {
+        //serial.printf("start motion! id = %d \r\n", id);
         inter = new Motion(motions[id], pose_size[id], servo_size);
         tick.attach(inter, &Motion::step, 0.02);
         playing = true;
@@ -77,10 +92,11 @@
 
 void Motions::control()
 {
-    char id = 0;
-    id = comu->getid();
-    if (id > 0) {
+    play();
+    int id = comu->getid();
+    //serial.printf("id: %d \r\n", id);
+    if (checkid(id)) { //<- Misterious bug. Why is id starts from 48?
         setmotion(id);
     }
-    play();
+    
 }
\ No newline at end of file