Shundo Kishi / Mbed 2 deprecated Hobby_Humanoid_controlor

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed

Motion.cpp@9:d9ce965299d2, 2012-09-22 (annotated)

Committer:
syundo0730
Date:
Sat Sep 22 06:16:15 2012 +0000
Revision:
9:d9ce965299d2
Child:
10:be8b10e54ecb
play motion

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syundo0730 9:d9ce965299d2 1 #include "Motion.h"
syundo0730 9:d9ce965299d2 2 #include "PWM.h"
syundo0730 9:d9ce965299d2 3
syundo0730 9:d9ce965299d2 4 extern PWM pwm;
syundo0730 9:d9ce965299d2 5 extern Ticker tick;
syundo0730 9:d9ce965299d2 6
syundo0730 9:d9ce965299d2 7 Motion::Motion(uint32_t** data, unsigned char size_idx, unsigned char size_num)
syundo0730 9:d9ce965299d2 8 {
syundo0730 9:d9ce965299d2 9 m_data = data;
syundo0730 9:d9ce965299d2 10
syundo0730 9:d9ce965299d2 11 //data size
syundo0730 9:d9ce965299d2 12 m_IDX_MAX = size_idx;
syundo0730 9:d9ce965299d2 13 m_NUM_MAX = size_num -1;
syundo0730 9:d9ce965299d2 14
syundo0730 9:d9ce965299d2 15 //interpolate num
syundo0730 9:d9ce965299d2 16 for(int i = 0; i < m_IDX_MAX; ++i) {
syundo0730 9:d9ce965299d2 17 m_data_num[i] = data[i][m_NUM_MAX];
syundo0730 9:d9ce965299d2 18 }
syundo0730 9:d9ce965299d2 19
syundo0730 9:d9ce965299d2 20 m_mode = 1;
syundo0730 9:d9ce965299d2 21
syundo0730 9:d9ce965299d2 22 //zero clear
syundo0730 9:d9ce965299d2 23 m_idx = 0;
syundo0730 9:d9ce965299d2 24 m_play = 0;
syundo0730 9:d9ce965299d2 25
syundo0730 9:d9ce965299d2 26 for (int i = 0; i < m_NUM_MAX; ++i) {
syundo0730 9:d9ce965299d2 27 m_buf[i] = 0.0;
syundo0730 9:d9ce965299d2 28 m_d_buf[i] = 0.0;
syundo0730 9:d9ce965299d2 29 m_dd_buf[i] = 0.0;
syundo0730 9:d9ce965299d2 30 }
syundo0730 9:d9ce965299d2 31 m_brake_flg = 0.0;
syundo0730 9:d9ce965299d2 32 }
syundo0730 9:d9ce965299d2 33
syundo0730 9:d9ce965299d2 34 void Motion::step()
syundo0730 9:d9ce965299d2 35 {
syundo0730 9:d9ce965299d2 36 if (m_idx < m_IDX_MAX - 1) {
syundo0730 9:d9ce965299d2 37 update();
syundo0730 9:d9ce965299d2 38 } else {
syundo0730 9:d9ce965299d2 39 tick.detach();
syundo0730 9:d9ce965299d2 40 }
syundo0730 9:d9ce965299d2 41 }
syundo0730 9:d9ce965299d2 42
syundo0730 9:d9ce965299d2 43 void Motion::update()
syundo0730 9:d9ce965299d2 44 {
syundo0730 9:d9ce965299d2 45 if (m_play == m_data_num[m_idx]) { //edge(end)
syundo0730 9:d9ce965299d2 46 ++m_idx;
syundo0730 9:d9ce965299d2 47 init_inter();
syundo0730 9:d9ce965299d2 48 m_play = 1;
syundo0730 9:d9ce965299d2 49 } else if (m_play > 1) {
syundo0730 9:d9ce965299d2 50 step_inter(); //interpolate
syundo0730 9:d9ce965299d2 51 ++m_play;
syundo0730 9:d9ce965299d2 52 } else if (m_play == 1) { //inter para
syundo0730 9:d9ce965299d2 53 set_inter();
syundo0730 9:d9ce965299d2 54 step_inter();
syundo0730 9:d9ce965299d2 55 ++m_play;
syundo0730 9:d9ce965299d2 56 } else if (m_play == 0) { //start(called onece)
syundo0730 9:d9ce965299d2 57 init_inter();
syundo0730 9:d9ce965299d2 58 ++m_play;
syundo0730 9:d9ce965299d2 59 } else {
syundo0730 9:d9ce965299d2 60 return;
syundo0730 9:d9ce965299d2 61 }
syundo0730 9:d9ce965299d2 62 }
syundo0730 9:d9ce965299d2 63
syundo0730 9:d9ce965299d2 64 void Motion::set_inter()
syundo0730 9:d9ce965299d2 65 {
syundo0730 9:d9ce965299d2 66 m_brake_pnt = m_data_num[m_idx] / 2.0;
syundo0730 9:d9ce965299d2 67
syundo0730 9:d9ce965299d2 68 if (m_mode == 0){ //liner
syundo0730 9:d9ce965299d2 69 for (int i = 0; i < m_NUM_MAX; ++i) {
syundo0730 9:d9ce965299d2 70 m_d_buf[i] = m_data[m_idx + 1][i] - m_data[m_idx][i];
syundo0730 9:d9ce965299d2 71 m_d_buf[i] /= m_data_num[m_idx];
syundo0730 9:d9ce965299d2 72 m_dd_buf[i] = 0.0;
syundo0730 9:d9ce965299d2 73 m_buf[i] = m_data[m_idx][i];
syundo0730 9:d9ce965299d2 74 }
syundo0730 9:d9ce965299d2 75 m_brake_flg = 1.0;
syundo0730 9:d9ce965299d2 76 } else { //accel
syundo0730 9:d9ce965299d2 77 for (int i = 0; i < m_NUM_MAX; ++i) {
syundo0730 9:d9ce965299d2 78 m_dd_buf[i] = m_data[m_idx + 1][i] - m_data[m_idx][i];
syundo0730 9:d9ce965299d2 79 m_dd_buf[i] = m_dd_buf[i] / m_data_num[m_idx] / m_data_num[m_idx] * 4.0;
syundo0730 9:d9ce965299d2 80 m_d_buf[i] = 0.0;
syundo0730 9:d9ce965299d2 81 m_buf[i] = m_data[m_idx][i];
syundo0730 9:d9ce965299d2 82 }
syundo0730 9:d9ce965299d2 83 m_brake_flg = 1.0;
syundo0730 9:d9ce965299d2 84 }
syundo0730 9:d9ce965299d2 85 }
syundo0730 9:d9ce965299d2 86
syundo0730 9:d9ce965299d2 87 void Motion::init_inter()
syundo0730 9:d9ce965299d2 88 {
syundo0730 9:d9ce965299d2 89 for (int i = 0; i < m_NUM_MAX; ++i) {
syundo0730 9:d9ce965299d2 90 m_buf[i] = m_data[m_idx][i];
syundo0730 9:d9ce965299d2 91 m_d_buf[i] = 0.0;
syundo0730 9:d9ce965299d2 92 m_dd_buf[i] = 0.0;
syundo0730 9:d9ce965299d2 93
syundo0730 9:d9ce965299d2 94 __disable_irq();
syundo0730 9:d9ce965299d2 95 pwm.SetDuty(i, (uint32_t)m_buf[i]);
syundo0730 9:d9ce965299d2 96 __enable_irq();
syundo0730 9:d9ce965299d2 97 }
syundo0730 9:d9ce965299d2 98 m_brake_flg = 0.0;
syundo0730 9:d9ce965299d2 99 }
syundo0730 9:d9ce965299d2 100
syundo0730 9:d9ce965299d2 101 void Motion::step_inter()
syundo0730 9:d9ce965299d2 102 {
syundo0730 9:d9ce965299d2 103 if (m_play > m_brake_pnt) {
syundo0730 9:d9ce965299d2 104 m_brake_flg = -1.0;
syundo0730 9:d9ce965299d2 105 }
syundo0730 9:d9ce965299d2 106 for (int i = 0; i < m_NUM_MAX; ++i) {
syundo0730 9:d9ce965299d2 107 m_d_buf[i] += (m_dd_buf[i]) * m_brake_flg;
syundo0730 9:d9ce965299d2 108 m_buf[i] += m_d_buf[i];
syundo0730 9:d9ce965299d2 109
syundo0730 9:d9ce965299d2 110 __disable_irq();
syundo0730 9:d9ce965299d2 111 pwm.SetDuty(i, (uint32_t)m_buf[i]);
syundo0730 9:d9ce965299d2 112 __enable_irq();
syundo0730 9:d9ce965299d2 113 }
syundo0730 9:d9ce965299d2 114 }