Revision 20:abb7852df747, committed 2013-08-19

Comitter:

syundo0730

Date:

Mon Aug 19 08:10:58 2013 +0000

Parent:

19:c2ec475367aa

Child:

21:a54bcab078ed

Commit message:

delete Motion instance from Control class

Changed in this revision

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controlor.h	Mon Aug 19 08:10:58 2013 +0000
@@ -0,0 +1,54 @@
+#ifndef CONTROLOR_H_2013_02_02_
+#define CONTROLOR_H_2013_02_02_
+
+#include "mbed.h"
+#include <string>
+
+#include "Motion.h"
+#include "OfflineMotion.h"
+#include "OnlineMotion.h"
+
+#include "SCI.h"
+#include "PWM.h"
+
+#include "CSV.h"
+#include "readMotion.h"
+
+const float TIMESTEP = 0.02;
+
+class Controlor
+{
+  public:
+    Controlor(uint16_t* data);
+    ~Controlor();
+    
+  public:
+    void control();
+    
+  private:
+    void read(const string& filename, uint16_t* data);
+    void set(uint16_t* data);
+    bool checkid(int id);
+    
+  public:
+    void setmotion(const int id);
+    void play();
+  
+  private:
+    uint16_t*** motions;
+    int motion_size;
+    int* pose_size;
+    int servo_size;
+    
+    OfflineMotion* offline;
+    OnlineMotion* online;
+    Motion* motion;
+    
+  private:
+    bool playing;
+    bool attached;
+    SCI* comu;
+    PWM* pwm;
+};
+
+#endif
\ No newline at end of file

--- a/Motion.cpp	Sun Apr 07 06:59:33 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,125 +0,0 @@
-#include "Motion.h"
-
-extern Ticker tick;
-extern DigitalOut led1;
-
-Motion::Motion(uint16_t** data, unsigned short int size_idx, unsigned char size_num, PWM* _pwm, bool* _playing)
-{
-    pwm = _pwm;
-    
-    playing = _playing;
-    *playing = true;
-  
-    m_data = data;
-    
-    m_IDX_MAX = size_idx;
-    m_NUM_MAX = size_num - 1;
- 
-    for(int i = 0; i < m_IDX_MAX; ++i) {
-        m_data_num[i] = data[i][m_NUM_MAX];
-    }
-    
-    m_mode = 1;
-    
-    m_idx = 0;
-    m_play = 0;
-    
-    for (int i = 0; i < m_NUM_MAX; ++i) {
-        m_buf[i] = 0.0;
-        m_d_buf[i] = 0.0;
-        m_dd_buf[i] = 0.0;
-    }
-    m_brake_flg = 0.0;
-    
-}
-
-Motion::~Motion()
-{
-    led1 = !led1;
-}
-
-void Motion::step()
-{
-    if (m_idx < m_IDX_MAX - 1) {
-        update();
-    } else {
-        m_idx = 0;
-        m_play = 0;
-        *playing = false;
-        tick.detach();
-        delete this;
-    }
-}
-
-void Motion::update()
-{
-    if (m_play == m_data_num[m_idx]) {  //edge(end)
-        ++m_idx;
-        init_inter();
-        m_play = 1;
-    } else if (m_play > 1) {
-        step_inter();                        //interpolate
-        ++m_play;
-    } else if (m_play == 1) {                //inter para
-        set_inter();
-        step_inter();
-        ++m_play;
-    } else if (m_play == 0) {                //start(called onece)
-        init_inter();
-        ++m_play;
-    } else {
-        return;
-    }
-}
-
-void Motion::set_inter()
-{
-    m_brake_pnt = m_data_num[m_idx] / 2.0;
-    
-    if (m_mode == 0){           //liner
-        for (int i = 0; i < m_NUM_MAX; ++i) {
-            m_d_buf[i] = m_data[m_idx + 1][i] - m_data[m_idx][i];
-            m_d_buf[i] /= m_data_num[m_idx];
-            m_dd_buf[i] = 0.0;
-            m_buf[i] = m_data[m_idx][i];
-        }
-        m_brake_flg = 1.0;
-    } else {                    //accel
-        for (int i = 0; i < m_NUM_MAX; ++i) {
-            m_dd_buf[i] = m_data[m_idx + 1][i] - m_data[m_idx][i];
-            m_dd_buf[i] = m_dd_buf[i] / m_data_num[m_idx] / m_data_num[m_idx] * 4.0;
-            m_d_buf[i] = 0.0;
-            m_buf[i] = m_data[m_idx][i];
-        }
-        m_brake_flg = 1.0;
-    }
-}
-
-void Motion::init_inter()
-{
-    for (int i = 0; i < m_NUM_MAX; ++i) {
-        m_buf[i] = m_data[m_idx][i];
-        m_d_buf[i] = 0.0;
-        m_dd_buf[i] = 0.0;
-        
-        __disable_irq();
-        pwm->SetDuty(i, (uint32_t)m_buf[i]);
-        __enable_irq();
-    }
-    m_brake_flg = 0.0;
-}
-
-void Motion::step_inter()
-{
-    if (m_play > m_brake_pnt) {
-        m_brake_flg = -1.0;
-    }
-    for (int i = 0; i < m_NUM_MAX; ++i) {
-        m_d_buf[i] += (m_dd_buf[i]) * m_brake_flg;
-        m_buf[i] += m_d_buf[i];
-        
-        __disable_irq();
-        pwm->SetDuty(i, (uint32_t)m_buf[i]);
-        __enable_irq();
-    }
-}
\ No newline at end of file

--- a/Motion.h	Sun Apr 07 06:59:33 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,41 +0,0 @@
-#ifndef MOTION_H_2012_09_21_
-#define MOTION_H_2012_09_21_
-
-#include "LPC17xx.h"
-#include "mbed.h"
-#include "PWM.h"
-
-class Motion
-{
-  public:
-    Motion(uint16_t** data, unsigned short int size_idx, unsigned char size_num, PWM* _pwm, bool* playing);
-    ~Motion();
-  public:
-    void step();
-  private:
-    void update();
-    void set_inter();
-    void init_inter();
-    void step_inter();
-  
-  private:
-    PWM* pwm;
-    uint16_t** m_data;
-    unsigned short int m_IDX_MAX;
-    unsigned char m_NUM_MAX;
-    unsigned char m_data_num[500];  //inter
-    unsigned char m_mode;
-    bool* playing;
-  
-  private:
-    unsigned short int m_idx;
-    unsigned char m_play;
-  private:  
-    double m_buf[25];              //buf
-    double m_d_buf[25];            //dbuf
-    double m_dd_buf[25];           //ddbuf
-    double m_brake_flg;
-    unsigned char m_brake_pnt;
-};
-
-#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion/Motion.h	Mon Aug 19 08:10:58 2013 +0000
@@ -0,0 +1,13 @@
+#ifndef MOTION_H_2013_04_7_
+#define MOTION_H_2013_04_7_
+
+#include "mbed.h"
+#include "PWM.h"
+
+class Motion
+{
+  public:
+    virtual void step() = 0;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion/OfflineMotion.cpp	Mon Aug 19 08:10:58 2013 +0000
@@ -0,0 +1,125 @@
+#include "OfflineMotion.h"
+
+//extern Ticker tick;
+extern DigitalOut led1;
+
+OfflineMotion::OfflineMotion(uint16_t** data, unsigned short int size_idx, unsigned char size_num, PWM* _pwm, bool* _playing)
+{
+    pwm = _pwm;
+    
+    playing = _playing;
+    *playing = true;
+  
+    m_data = data;
+    
+    m_IDX_MAX = size_idx;
+    m_NUM_MAX = size_num - 1;
+ 
+    for(int i = 0; i < m_IDX_MAX; ++i) {
+        m_data_num[i] = data[i][m_NUM_MAX];
+    }
+    
+    m_mode = 1;
+    
+    m_idx = 0;
+    m_play = 0;
+    
+    for (int i = 0; i < m_NUM_MAX; ++i) {
+        m_buf[i] = 0.0;
+        m_d_buf[i] = 0.0;
+        m_dd_buf[i] = 0.0;
+    }
+    m_brake_flg = 0.0;
+    
+}
+
+OfflineMotion::~OfflineMotion()
+{
+    led1 = !led1;
+}
+
+void OfflineMotion::step()
+{
+    if (m_idx < m_IDX_MAX - 1) {
+        update();
+    } else {
+        m_idx = 0;
+        m_play = 0;
+        *playing = false;
+        //tick.detach();
+        //delete this;
+    }
+}
+
+void OfflineMotion::update()
+{
+    if (m_play == m_data_num[m_idx]) {  //edge(end)
+        ++m_idx;
+        init_inter();
+        m_play = 1;
+    } else if (m_play > 1) {
+        step_inter();                        //interpolate
+        ++m_play;
+    } else if (m_play == 1) {                //inter para
+        set_inter();
+        step_inter();
+        ++m_play;
+    } else if (m_play == 0) {                //start(called onece)
+        init_inter();
+        ++m_play;
+    } else {
+        return;
+    }
+}
+
+void OfflineMotion::set_inter()
+{
+    m_brake_pnt = m_data_num[m_idx] / 2.0;
+    
+    if (m_mode == 0){           //liner
+        for (int i = 0; i < m_NUM_MAX; ++i) {
+            m_d_buf[i] = m_data[m_idx + 1][i] - m_data[m_idx][i];
+            m_d_buf[i] /= m_data_num[m_idx];
+            m_dd_buf[i] = 0.0;
+            m_buf[i] = m_data[m_idx][i];
+        }
+        m_brake_flg = 1.0;
+    } else {                    //accel
+        for (int i = 0; i < m_NUM_MAX; ++i) {
+            m_dd_buf[i] = m_data[m_idx + 1][i] - m_data[m_idx][i];
+            m_dd_buf[i] = m_dd_buf[i] / m_data_num[m_idx] / m_data_num[m_idx] * 4.0;
+            m_d_buf[i] = 0.0;
+            m_buf[i] = m_data[m_idx][i];
+        }
+        m_brake_flg = 1.0;
+    }
+}
+
+void OfflineMotion::init_inter()
+{
+    for (int i = 0; i < m_NUM_MAX; ++i) {
+        m_buf[i] = m_data[m_idx][i];
+        m_d_buf[i] = 0.0;
+        m_dd_buf[i] = 0.0;
+        
+        __disable_irq();
+        pwm->SetDuty(i, (uint32_t)m_buf[i]);
+        __enable_irq();
+    }
+    m_brake_flg = 0.0;
+}
+
+void OfflineMotion::step_inter()
+{
+    if (m_play > m_brake_pnt) {
+        m_brake_flg = -1.0;
+    }
+    for (int i = 0; i < m_NUM_MAX; ++i) {
+        m_d_buf[i] += (m_dd_buf[i]) * m_brake_flg;
+        m_buf[i] += m_d_buf[i];
+        
+        __disable_irq();
+        pwm->SetDuty(i, (uint32_t)m_buf[i]);
+        __enable_irq();
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion/OfflineMotion.h	Mon Aug 19 08:10:58 2013 +0000
@@ -0,0 +1,42 @@
+#ifndef OFFLINEMOTION_H_2012_09_21_
+#define OFFLINEMOTION_H_2012_09_21_
+
+#include "LPC17xx.h"
+#include "mbed.h"
+#include "PWM.h"
+//#include "Motion.h"
+
+class OfflineMotion //: public Motion
+{
+  public:
+    OfflineMotion(uint16_t** data, unsigned short int size_idx, unsigned char size_num, PWM* _pwm, bool* playing);
+    ~OfflineMotion();
+  public:
+    void step();
+  private:
+    void update();
+    void set_inter();
+    void init_inter();
+    void step_inter();
+  
+  private:
+    PWM* pwm;
+    uint16_t** m_data;
+    unsigned short int m_IDX_MAX;
+    unsigned char m_NUM_MAX;
+    unsigned char m_data_num[500];  //inter
+    unsigned char m_mode;
+    bool* playing;
+  
+  private:
+    unsigned short int m_idx;
+    unsigned char m_play;
+  private:  
+    double m_buf[25];              //buf
+    double m_d_buf[25];            //dbuf
+    double m_dd_buf[25];           //ddbuf
+    double m_brake_flg;
+    unsigned char m_brake_pnt;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion/OnlineMotion.cpp	Mon Aug 19 08:10:58 2013 +0000
@@ -0,0 +1,129 @@
+#include "OnlineMotion.h"
+
+//extern Ticker tick;
+extern DigitalOut led1;
+
+OnlineMotion::OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, PWM* _pwm, bool* _playing)
+    : pwm(_pwm), playing(_playing), t(0), T(_T), m_NUM_MAX(_size_num - 1), STEP(_TIMESTEP)
+{
+    *playing = true;
+}
+
+OnlineMotion::~OnlineMotion()
+{
+    led1 = !led1;
+}
+
+void OnlineMotion::step()
+{
+    if (t < T) {
+        update();
+    } else {
+        *playing = false;
+        //tick.detach();
+        //delete this;
+    }
+}
+
+void OnlineMotion::update()
+{
+    play_online();
+    t += STEP;
+}
+
+void OnlineMotion::invertkinematics(float* pos, uint16_t* theta)
+{
+    float L1 = 45.0;
+    float L2 = 90.0;
+    
+    float x = pos[0];
+    float y = pos[1];
+    float z = pos[2];
+    
+    float L = sqrt(z*z + y*y) - 2 * L1;
+    float h = sqrt(x*x + L*L);
+    
+    float alpha = atan(y / z);
+    float beta = acos(h / (2 * L2));
+    float gunmma = asin(x / h);
+    
+    theta[0] = alpha * RADTOVAL;
+    theta[1] = (beta - gunmma) * RADTOVAL;
+    theta[2] = 2 * beta * RADTOVAL;
+    theta[3] = (beta + gunmma) * RADTOVAL;
+}
+
+void OnlineMotion::walk(float* lpos, float* rpos, float h, float stride, float side, float up)
+{
+    if (t < T / 2) {
+        lpos[0] = stride - (4 * stride / T) * t;
+        rpos[0] = -stride + (4 * stride / T) * t;
+        if (t < T / 6) {
+            lpos[2] = h;
+        } else if (t < T / 3) {
+            lpos[2] = h - up * sin((t-T/6) * M_PI/(T/6));
+        } else {
+            lpos[2] = h;
+        }
+        rpos[2] = h;
+    } else {
+        lpos[0] = -stride + (4 * stride / T) * (t - T/2);
+        rpos[0] = stride - (4 * stride / T) * (t - T/2);
+        if (t < 4*T/6) {
+            rpos[2] = h;
+        } else if (t < 5*T/6) {
+            rpos[2] = h -up * sin((t-4*T/6) * M_PI/(T/6));
+        } else {
+            rpos[2] = h;
+        }
+        lpos[2] = h;
+    }
+    lpos[1] = -side * sin(t * 2 * M_PI/T);
+    rpos[1] = lpos[1];
+}
+
+void OnlineMotion::play_online()
+{
+    
+    uint16_t ltheta[5], rtheta[5];
+    
+    float lpos[3], rpos[3];
+    
+    walk(lpos, rpos, 250, 20, 20, 15);
+    
+    invertkinematics(lpos, ltheta);
+    invertkinematics(rpos, rtheta);
+    
+    uint32_t pos[] = {
+        HOMEPOS[0],
+        HOMEPOS[1] + ltheta[0],
+        HOMEPOS[2] - ltheta[1],
+        HOMEPOS[3] - ltheta[2],
+        HOMEPOS[4] + ltheta[3],
+        HOMEPOS[5] + ltheta[0],
+        HOMEPOS[6],
+        HOMEPOS[7],
+        HOMEPOS[8],
+        HOMEPOS[9] + rtheta[0],
+        HOMEPOS[10] + rtheta[1],
+        HOMEPOS[11] + rtheta[2],
+        HOMEPOS[12] - rtheta[3],
+        HOMEPOS[13] + rtheta[0],
+        HOMEPOS[14],
+        HOMEPOS[15],
+        HOMEPOS[16],
+        HOMEPOS[17],
+        HOMEPOS[18],
+        HOMEPOS[19],
+        HOMEPOS[20],
+        HOMEPOS[21],
+        HOMEPOS[22],
+        HOMEPOS[23]
+    };
+    
+    for (int i = 0; i < m_NUM_MAX; ++i) {
+        __disable_irq();
+        pwm->SetDuty(i, pos[i]);
+        __enable_irq();
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion/OnlineMotion.h	Mon Aug 19 08:10:58 2013 +0000
@@ -0,0 +1,36 @@
+#ifndef ONLINEMOTION_H_2013_02_04_
+#define ONLINEMOTION_H_2013_02_04_
+
+#include <cmath>
+
+#include "LPC17xx.h"
+#include "mbed.h"
+#include "PWM.h"
+//#include "Motion.h"
+
+const float RADTOVAL = (SRV_MAX_DUTY - SRV_MIN_DUTY) / 3.14159265;
+
+const float M_PI = 3.14159265;
+
+class OnlineMotion //: public Motion
+{
+  public:
+    OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, PWM* _pwm, bool* _playing);
+    ~OnlineMotion();
+  public:
+    void step();
+  private:
+    void update();
+    void play_online();
+    void invertkinematics(float* pos, uint16_t* theta);
+    void walk(float* lpos, float* rpos, float h, float stride, float side, float up);
+    
+  private:
+    PWM* pwm;
+    unsigned char m_NUM_MAX;
+    bool* playing;
+    volatile float t;
+    float T ,STEP;
+};
+
+#endif
\ No newline at end of file

--- a/Motions.cpp	Sun Apr 07 06:59:33 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,98 +0,0 @@
-#include <iostream>
-#include <string>
-
-#include "Motions.h"
-
-Ticker tick;
-
-//detach may be better to controled from Motions class
-
-Motions::Motions(uint16_t* data) : playing(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);
-}
-
-Motions::~Motions()
-{
-    for (int i = 0; i < motion_size; i++) {
-        delete[] motions[i];
-    }
-    delete[] motions;
-    
-    delete comu;
-    delete pwm;
-}
-
-void Motions::read(const string& filename, uint16_t* data)
-{
-    CSV csv;
-    pose_size = new int;
-    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 Motions::set(uint16_t* data)
-{
-    int size_z, size_x;
-    size_z = motion_size;
-    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 Motions::checkid(int id)
-{
-    if (id >= 0 && id < motion_size) {
-        return true;
-    } else {
-        return false;
-    }
-}
-
-void Motions::setmotion(const int id)
-{
-    if (!playing) {
-        offline = new Motion(motions[id], pose_size[id], servo_size, pwm, &playing);
-        tick.attach(offline, &Motion::step, TIMESTEP);
-        //online = new OnlineMotion(3.0, TIMESTEP, servo_size, pwm, &playing);
-        //tick.attach(online, &OnlineMotion::step, TIMESTEP);
-    }
-}
-
-void Motions::control()
-{
-    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

--- a/Motions.h	Sun Apr 07 06:59:33 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,51 +0,0 @@
-#ifndef MOTIONS_H_2013_02_02_
-#define MOTIONS_H_2013_02_02_
-
-#include "mbed.h"
-#include <string>
-
-#include "Motion.h"
-#include "OnlineMotion.h"
-
-#include "SCI.h"
-#include "PWM.h"
-
-#include "CSV.h"
-#include "readMotion.h"
-
-const float TIMESTEP = 0.02;
-
-class Motions
-{
-  public:
-    Motions(uint16_t* data);
-    ~Motions();
-    
-  public:
-    void control();
-    
-  private:
-    void read(const string& filename, uint16_t* data);
-    void set(uint16_t* data);
-    bool checkid(int id);
-    
-  public:
-    void setmotion(const int id);
-    void play();
-  
-  private:
-    uint16_t*** motions;
-    int motion_size;
-    int* pose_size;
-    int servo_size;
-    
-    Motion* offline;
-    OnlineMotion* online;
-    
-  private:
-    bool playing;
-    SCI* comu;
-    PWM* pwm;
-};
-
-#endif
\ No newline at end of file

--- a/OnlineMotion.cpp	Sun Apr 07 06:59:33 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,129 +0,0 @@
-#include "OnlineMotion.h"
-
-extern Ticker tick;
-extern DigitalOut led1;
-
-OnlineMotion::OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, PWM* _pwm, bool* _playing)
-    : pwm(_pwm), playing(_playing), t(0), T(_T), m_NUM_MAX(_size_num - 1), STEP(_TIMESTEP)
-{
-    *playing = true;
-}
-
-OnlineMotion::~OnlineMotion()
-{
-    led1 = !led1;
-}
-
-void OnlineMotion::step()
-{
-    if (t < T) {
-        update();
-    } else {
-        *playing = false;
-        tick.detach();
-        delete this;
-    }
-}
-
-void OnlineMotion::update()
-{
-    play_online();
-    t += STEP;
-}
-
-void OnlineMotion::invertkinematics(float* pos, uint16_t* theta)
-{
-    float L1 = 45.0;
-    float L2 = 90.0;
-    
-    float x = pos[0];
-    float y = pos[1];
-    float z = pos[2];
-    
-    float L = sqrt(z*z + y*y) - 2 * L1;
-    float h = sqrt(x*x + L*L);
-    
-    float alpha = atan(y / z);
-    float beta = acos(h / (2 * L2));
-    float gunmma = asin(x / h);
-    
-    theta[0] = alpha * RADTOVAL;
-    theta[1] = (beta - gunmma) * RADTOVAL;
-    theta[2] = 2 * beta * RADTOVAL;
-    theta[3] = (beta + gunmma) * RADTOVAL;
-}
-
-void OnlineMotion::walk(float* lpos, float* rpos, float h, float stride, float side, float up)
-{
-    if (t < T / 2) {
-        lpos[0] = stride - (4 * stride / T) * t;
-        rpos[0] = -stride + (4 * stride / T) * t;
-        if (t < T / 6) {
-            lpos[2] = h;
-        } else if (t < T / 3) {
-            lpos[2] = h - up * sin((t-T/6) * M_PI/(T/6));
-        } else {
-            lpos[2] = h;
-        }
-        rpos[2] = h;
-    } else {
-        lpos[0] = -stride + (4 * stride / T) * (t - T/2);
-        rpos[0] = stride - (4 * stride / T) * (t - T/2);
-        if (t < 4*T/6) {
-            rpos[2] = h;
-        } else if (t < 5*T/6) {
-            rpos[2] = h -up * sin((t-4*T/6) * M_PI/(T/6));
-        } else {
-            rpos[2] = h;
-        }
-        lpos[2] = h;
-    }
-    lpos[1] = -side * sin(t * 2 * M_PI/T);
-    rpos[1] = lpos[1];
-}
-
-void OnlineMotion::play_online()
-{
-    
-    uint16_t ltheta[5], rtheta[5];
-    
-    float lpos[3], rpos[3];
-    
-    walk(lpos, rpos, 250, 20, 20, 15);
-    
-    invertkinematics(lpos, ltheta);
-    invertkinematics(rpos, rtheta);
-    
-    uint32_t pos[] = {
-        HOMEPOS[0],
-        HOMEPOS[1] + ltheta[0],
-        HOMEPOS[2] - ltheta[1],
-        HOMEPOS[3] - ltheta[2],
-        HOMEPOS[4] + ltheta[3],
-        HOMEPOS[5] + ltheta[0],
-        HOMEPOS[6],
-        HOMEPOS[7],
-        HOMEPOS[8],
-        HOMEPOS[9] + rtheta[0],
-        HOMEPOS[10] + rtheta[1],
-        HOMEPOS[11] + rtheta[2],
-        HOMEPOS[12] - rtheta[3],
-        HOMEPOS[13] + rtheta[0],
-        HOMEPOS[14],
-        HOMEPOS[15],
-        HOMEPOS[16],
-        HOMEPOS[17],
-        HOMEPOS[18],
-        HOMEPOS[19],
-        HOMEPOS[20],
-        HOMEPOS[21],
-        HOMEPOS[22],
-        HOMEPOS[23]
-    };
-    
-    for (int i = 0; i < m_NUM_MAX; ++i) {
-        __disable_irq();
-        pwm->SetDuty(i, pos[i]);
-        __enable_irq();
-    }
-}
\ No newline at end of file

--- a/OnlineMotion.h	Sun Apr 07 06:59:33 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,38 +0,0 @@
-#ifndef ONLINEMOTION_H_2013_02_04_
-#define ONLINEMOTION_H_2013_02_04_
-
-#include <cmath>
-
-#include "LPC17xx.h"
-#include "mbed.h"
-#include "PWM.h"
-
-const float RADTOVAL = (SRV_MAX_DUTY - SRV_MIN_DUTY) / 3.14159265;
-
-const float M_PI = 3.14159265;
-
-class OnlineMotion
-{
-  public:
-    OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, PWM* _pwm, bool* _playing);
-    ~OnlineMotion();
-  public:
-    void step();
-  private:
-    void update();
-    void play_online();
-    void invertkinematics(float* pos, uint16_t* theta);
-    void walk(float* lpos, float* rpos, float h, float stride, float side, float up);
-    
-  
-  private:
-    PWM* pwm;
-    //uint16_t** m_data;
-    //unsigned short int m_IDX_MAX;
-    unsigned char m_NUM_MAX;
-    bool* playing;
-    volatile float t;
-    float T ,STEP;
-};
-
-#endif
\ No newline at end of file

--- a/main.cpp	Sun Apr 07 06:59:33 2013 +0000
+++ b/main.cpp	Mon Aug 19 08:10:58 2013 +0000
@@ -1,6 +1,6 @@
 #include "LPC17xx.h"
 #include "mbed.h"
-#include "Motions.h"
+#include "Controlor.h"
 #include "PWM.h"
 
 uint16_t data[0x2000] __attribute__((section("AHBSRAM1"))); //motion data area
@@ -9,10 +9,10 @@
 
 int main(void)
 {
-    Motions motions(data);
+    Controlor controlor(data);
     
     while (1) {
-        motions.control();
+        controlor.control();
         wait(0.02);
     }
 }
\ No newline at end of file

--- a/readMotion.cpp	Sun Apr 07 06:59:33 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,36 +0,0 @@
-#include "readMotion.h"
-
-using namespace std;
-
-bool readMotion(string filename, uint16_t* pos, int &servo_size, int &motion_size, int* pose_size) {
-    fstream file;
-    string line, tmp;
-    stringstream sstr;
-    
-    file.open(filename.c_str(), ios::in);
-    if(! file.is_open()) {
-        return false;
-    }
-    
-    int i = 0, id = 0;
-    while(getline(file, line)) {
-    
-        std::replace(line.begin(), line.end(), ',', ' ');
-        sstr.clear();
-        sstr.str(line);
-        while (sstr >> tmp) {
-            if (tmp == "d") {
-                while (sstr >> pos[i]) ++i;
-            } else if (tmp == "servo_size") {
-                sstr >> servo_size;
-            } else if (tmp == "motion_size") {
-                sstr >> motion_size;
-            } else if (tmp == "pose_size") {
-                sstr >> pose_size[id];
-                ++id;
-            }
-        }
-    }
-    file.close();
-    return true;
-}
\ No newline at end of file

--- a/readMotion.h	Sun Apr 07 06:59:33 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,9 +0,0 @@
-#include "mbed.h"
-
-#include <iostream>
-#include <fstream>
-#include <sstream>
-#include <string>
-#include <algorithm>
-
-bool readMotion(string filename, uint16_t* pos, int &servo_size, int &motion_size, int* pose_size);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/readMotion/readMotion.cpp	Mon Aug 19 08:10:58 2013 +0000
@@ -0,0 +1,36 @@
+#include "readMotion.h"
+
+using namespace std;
+
+bool readMotion(string filename, uint16_t* pos, int &servo_size, int &motion_size, int* pose_size) {
+    fstream file;
+    string line, tmp;
+    stringstream sstr;
+    
+    file.open(filename.c_str(), ios::in);
+    if(! file.is_open()) {
+        return false;
+    }
+    
+    int i = 0, id = 0;
+    while(getline(file, line)) {
+    
+        std::replace(line.begin(), line.end(), ',', ' ');
+        sstr.clear();
+        sstr.str(line);
+        while (sstr >> tmp) {
+            if (tmp == "d") {
+                while (sstr >> pos[i]) ++i;
+            } else if (tmp == "servo_size") {
+                sstr >> servo_size;
+            } else if (tmp == "motion_size") {
+                sstr >> motion_size;
+            } else if (tmp == "pose_size") {
+                sstr >> pose_size[id];
+                ++id;
+            }
+        }
+    }
+    file.close();
+    return true;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/readMotion/readMotion.h	Mon Aug 19 08:10:58 2013 +0000
@@ -0,0 +1,9 @@
+#include "mbed.h"
+
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <algorithm>
+
+bool readMotion(string filename, uint16_t* pos, int &servo_size, int &motion_size, int* pose_size);
\ No newline at end of file