Diff: Motions.cpp

Revision:

12:6cd135bf03bd

Child:

13:711f74b2fa33

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motions.cpp	Sun Feb 03 04:53:44 2013 +0000
@@ -0,0 +1,86 @@
+#include <iostream>
+#include <string>
+#include "mbed.h"
+#include "CSV.h"
+#include "Motions.h"
+#include "Motion.h"
+#include "SCI.h"
+
+extern uint16_t data[0x2000] __attribute__((section("AHBSRAM1")));
+extern Ticker tick;
+
+Motions::Motions(uint16_t* data)
+{
+    LocalFileSystem* local = new LocalFileSystem("local");
+    read("/local/motion.csv", data);
+    set(data);
+    playing = false;
+    comu = new SCI(USBTX, USBRX);
+}
+
+Motions::~Motions()
+{
+    for (int i = 0; i < motion_size; i++) {
+        delete[] motions[i];
+    }
+    delete[] motions;
+}
+
+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;
+}
+
+void Motions::set(uint16_t* data)
+{
+    int size_z, size_x;
+    size_z = motion_size;
+    size_x = servo_size;
+    
+    uint16_t*** motions = new uint16_t**[size_z];
+    uint16_t* p = data;
+    
+    for (int i = 0; i < size_z; ++i) {
+        int size_y = pose_size[i];
+        motions[i] = new uint16_t*[size_y];
+        for (int j = 0; j < size_y; ++j) {
+            motions[i][j] = p + size_x * j;
+        }
+        p += size_x * size_y;
+    }
+}
+
+void Motions::play()
+{
+    if (playing) {
+        if (!inter->is_in_interrupt()) {
+            delete inter;
+            playing = false;
+        }
+    }
+}
+
+void Motions::setmotion(const int& id)
+{
+    if (!playing) {
+        inter = new Motion(motions[id], pose_size[id], servo_size);
+        tick.attach(inter, &Motion::step, 0.02);
+        playing = true;
+    }
+}
+
+void Motions::control()
+{
+    char id = 0;
+    id = comu->getid();
+    if (id > 0) {
+        setmotion(id);
+    }
+    play();
+}
\ No newline at end of file