Diff: Motion/OfflineMotion.cpp

Revision:

23:0927e605af4b

Parent:

22:bf5aa20b9df0

--- a/Motion/OfflineMotion.cpp	Fri Sep 06 08:36:21 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,125 +0,0 @@
-#include "OfflineMotion.h"
-
-//extern Ticker tick;
-extern DigitalOut led1;
-
-OfflineMotion::OfflineMotion(uint16_t** data, unsigned short int size_idx, unsigned char size_num, Adafruit_PWMServoDriver* _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