File content as of revision 9:d9ce965299d2:

#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();
    }
}