Diff: Motions.cpp

Revision:

16:e65c192b7ecf

Parent:

15:e37a8c413e51

Child:

17:60de3bfdc70b

--- a/Motions.cpp	Thu Feb 21 15:21:12 2013 +0000
+++ b/Motions.cpp	Wed Feb 27 12:32:44 2013 +0000
@@ -4,23 +4,27 @@
 #include "CSV.h"
 #include "Motions.h"
 #include "Motion.h"
-#include "SCI.h"
+//#include "SCI.h"
+//#include "PWM.h"
 
 extern uint16_t data[0x2000] __attribute__((section("AHBSRAM1")));
 extern Ticker tick;
 
 bool g_motion_playing = false;
 
-//Serial serial(USBTX, USBRX);
+Serial serial(USBTX, USBRX);
+//Serial device(p28, p27);
 
 Motions::Motions(uint16_t* data)
 {
+    pwm = new PWM();
     LocalFileSystem* local = new LocalFileSystem("local");
     read("/local/motion.csv", data);
     set(data);
     playing = false;
     
-    comu = new SCI(USBTX, USBRX);
+    //comu = new SCI(USBTX, USBRX);
+    comu = new SCI(p28, p27);
 }
 
 Motions::~Motions()
@@ -29,6 +33,9 @@
         delete[] motions[i];
     }
     delete[] motions;
+    
+    delete comu;
+    delete pwm;
 }
 
 void Motions::read(const string& filename, uint16_t* data)
@@ -72,21 +79,10 @@
     }
 }
 
-/*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, TIMESTEP);
-        playing = true;
-    }
-}*/
-
 void Motions::setmotion(const int id)
 {
     if (!g_motion_playing) {
-        //serial.printf("start motion! id = %d \r\n", id);
-        inter = new Motion(motions[id], pose_size[id], servo_size);
+        inter = new Motion(motions[id], pose_size[id], servo_size, pwm);
         tick.attach(inter, &Motion::step, TIMESTEP);
         playing = true;
     }
@@ -94,11 +90,16 @@
 
 void Motions::control()
 {
-    //int id = comu->getid();
-    //serial.printf("id: %d \r\n", id);
-    //if (checkid(id)) { //<- Misterious bug. Why is id starts from 48?
-        //setmotion(id);
-        setmotion(0);
-    //}
+    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