![](/media/cache/profiles/e5815151957be36ad2085b7a1a02c5cc.jpg.50x50_q85.jpg)
Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.
Dependencies: Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed
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