Diff: Controlor.cpp

Revision:

22:bf5aa20b9df0

Parent:

21:a54bcab078ed

Child:

23:0927e605af4b

--- a/Controlor.cpp	Wed Sep 04 03:59:40 2013 +0000
+++ b/Controlor.cpp	Fri Sep 06 08:36:21 2013 +0000
@@ -2,11 +2,11 @@
 #include <string>
 
 #include "Controlor.h"
+#include "HomePosition.h"
 
 Ticker tick;
 
-const float TIMESTEP = 0.01;
-
+const float TIMESTEP = 0.005;
 
 //detach may be better to controled from Controlor class
 
@@ -18,8 +18,12 @@
     pwm->setPrescale(64);    //This value is decided for 10ms interval.
     pwm->setI2Cfreq(400000); //400kHz
     
+    for (int i = 0; i < 16; ++i) {
+        pwm->setDuty(i, HOMEPOS[i]);
+    }
+    
     //comu = new SCI(p28, p27);
-    comu = new SCI(USBTX, USBRX);
+    comu = new Console(USBTX, USBRX);
     
     LocalFileSystem* local = new LocalFileSystem("local");
     
@@ -27,7 +31,7 @@
     set(data);
     
     // Motion 0 is Home position
-    setmotion(0);
+    //setmotion(0);
 }
 
 Controlor::~Controlor()
@@ -83,36 +87,37 @@
         //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(1.0, TIMESTEP, servo_size, pwm, &playing);
-        //tick.attach(online, &OnlineMotion::step, TIMESTEP);
+        //offline = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
+        //tick.attach(offline, &OfflineMotion::step, TIMESTEP);
+        online = new OnlineMotion(0.5, TIMESTEP, servo_size, pwm, &playing);
+        tick.attach(online, &OnlineMotion::step, TIMESTEP);
     //}
 }
 
 void Controlor::control()
 {
-    if (!playing) {
+    /*if (!playing) {
         if (attached) {
             tick.detach();
-            delete offline;
+            delete online;
             attached = false;
         } else {
             setmotion(1);
         }
-    }
+    }*/
     
     //setmotion(1);
-    /*char head = comu->getheader();
-    if (head == 'A') {
+    char head = comu->getheader();
+    if (head == 'a') {
         int id = comu->getid();
         if (checkid(id)) {
-            setmotion(id);
+            //setmotion(id);
         }
-    } else if (head == 'B') {
+    } else if (head == 'b') {
         int id = comu->getid();
-        uint16_t val = comu->getservoval();
-        pwm->SetDuty(id, (uint32_t)val);
-    }*/
+        uint16_t val = comu->get_int_cr();
+        //wait(1);
+        pwm->setDuty(id, val);
+    }
     
 }
\ No newline at end of file