Diff: Motion.cpp

Revision:

9:d9ce965299d2

Child:

10:be8b10e54ecb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion.cpp	Sat Sep 22 06:16:15 2012 +0000
@@ -0,0 +1,114 @@
+#include "Motion.h"
+#include "PWM.h"
+
+extern PWM pwm;
+extern Ticker tick;
+
+Motion::Motion(uint32_t** data, unsigned char size_idx, unsigned char size_num)
+{
+    m_data = data;
+    
+    //data size
+    m_IDX_MAX = size_idx;
+    m_NUM_MAX = size_num -1;
+    
+    //interpolate num
+    for(int i = 0; i < m_IDX_MAX; ++i) {
+        m_data_num[i] = data[i][m_NUM_MAX];
+    }
+    
+    m_mode = 1;
+    
+    //zero clear
+    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;
+}
+
+void Motion::step()
+{
+    if (m_idx < m_IDX_MAX - 1) {
+        update();
+    } else {
+        tick.detach();
+    }
+}
+
+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