Diff: Controlor.cpp

Revision:

20:abb7852df747

Parent:

19:c2ec475367aa

Child:

21:a54bcab078ed

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controlor.cpp	Mon Aug 19 08:10:58 2013 +0000
@@ -0,0 +1,110 @@
+#include <iostream>
+#include <string>
+
+#include "Controlor.h"
+
+Ticker tick;
+
+//detach may be better to controled from Controlor class
+
+Controlor::Controlor(uint16_t* data) : playing(false), attached(false)
+{
+    pwm = new PWM();
+    //comu = new SCI(p28, p27);
+    comu = new SCI(USBTX, USBRX);
+    
+    LocalFileSystem* local = new LocalFileSystem("local");
+    
+    read("/local/motion.csv", data);
+    set(data);
+    
+    // Motion 0 is Home position
+    setmotion(0);
+}
+
+Controlor::~Controlor()
+{
+    for (int i = 0; i < motion_size; i++) {
+        delete[] motions[i];
+    }
+    delete[] motions;
+    
+    delete comu;
+    delete pwm;
+}
+
+void Controlor::read(const string& filename, uint16_t* data)
+{
+    CSV csv;
+    pose_size = new int;//<-This code is suspicious
+    csv.read(filename, data, &servo_size, &motion_size, pose_size);
+    //readMotion(filename, data, servo_size, motion_size, pose_size); // not so good at speed and has bug in handling float motion value.
+}
+
+void Controlor::set(uint16_t* data)
+{
+    int size_z = motion_size;
+    int size_x = servo_size;
+    
+    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;
+    }
+}
+
+bool Controlor::checkid(int id)
+{
+    if (id >= 0 && id < motion_size) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+void Controlor::setmotion(const int id)
+{
+    // TODO : Make OfflineMotion class array and attach by each id. Newing every time is not good!
+    //if (!motion.playing) {
+        //motion = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
+        //tick.attach(motion, &Motion::step, TIMESTEP);
+        attached = true;
+        offline = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
+        tick.attach(offline, &OfflineMotion::step, TIMESTEP);
+        //online = new OnlineMotion(3.0, TIMESTEP, servo_size, pwm, &playing);
+        //tick.attach(online, &OnlineMotion::step, TIMESTEP);
+    //}
+}
+
+void Controlor::control()
+{
+    if (!playing) {
+        if (attached) {
+            tick.detach();
+            delete offline;
+            attached = false;
+        } else {
+            setmotion(1);
+        }
+    }
+    
+    //setmotion(1);
+    /*char head = comu->getheader();
+    if (head == 'A') {
+        int id = comu->getid();
+        if (checkid(id)) {
+            setmotion(id);
+        }
+    } else if (head == 'B') {
+        int id = comu->getid();
+        uint16_t val = comu->getservoval();
+        pwm->SetDuty(id, (uint32_t)val);
+    }*/
+    
+}
\ No newline at end of file