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@12:6cd135bf03bd, 2013-02-03 (annotated)

Committer:
syundo0730
Date:
Sun Feb 03 04:53:44 2013 +0000
Revision:
12:6cd135bf03bd
Parent:
11:1539d181e159
Child:
13:711f74b2fa33
file reading, motion selecting, serial communication

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 Ticker tick;
syundo0730 12:6cd135bf03bd 5 PWM pwm;
syundo0730 9:d9ce965299d2 6
syundo0730 11:1539d181e159 7 Motion::Motion(uint16_t** data, unsigned short int size_idx, unsigned char size_num)
syundo0730 9:d9ce965299d2 8 {
syundo0730 12:6cd135bf03bd 9 //pwm = new PWM();
syundo0730 12:6cd135bf03bd 10
syundo0730 9:d9ce965299d2 11 m_data = data;
syundo0730 9:d9ce965299d2 12
syundo0730 9:d9ce965299d2 13 m_IDX_MAX = size_idx;
syundo0730 12:6cd135bf03bd 14 m_NUM_MAX = size_num - 1;
syundo0730 12:6cd135bf03bd 15
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 10:be8b10e54ecb 21 m_in_interrupt = false;
syundo0730 9:d9ce965299d2 22
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 12:6cd135bf03bd 34 Motion::~Motion()
syundo0730 12:6cd135bf03bd 35 {
syundo0730 12:6cd135bf03bd 36 //delete pwm;
syundo0730 12:6cd135bf03bd 37 }
syundo0730 12:6cd135bf03bd 38
syundo0730 9:d9ce965299d2 39 void Motion::step()
syundo0730 9:d9ce965299d2 40 {
syundo0730 9:d9ce965299d2 41 if (m_idx < m_IDX_MAX - 1) {
syundo0730 10:be8b10e54ecb 42 m_in_interrupt = true;
syundo0730 9:d9ce965299d2 43 update();
syundo0730 9:d9ce965299d2 44 } else {
syundo0730 10:be8b10e54ecb 45 m_in_interrupt = false;
syundo0730 10:be8b10e54ecb 46 m_idx = 0;
syundo0730 10:be8b10e54ecb 47 m_play = 0;
syundo0730 9:d9ce965299d2 48 tick.detach();
syundo0730 9:d9ce965299d2 49 }
syundo0730 9:d9ce965299d2 50 }
syundo0730 9:d9ce965299d2 51
syundo0730 9:d9ce965299d2 52 void Motion::update()
syundo0730 9:d9ce965299d2 53 {
syundo0730 9:d9ce965299d2 54 if (m_play == m_data_num[m_idx]) { //edge(end)
syundo0730 9:d9ce965299d2 55 ++m_idx;
syundo0730 9:d9ce965299d2 56 init_inter();
syundo0730 9:d9ce965299d2 57 m_play = 1;
syundo0730 9:d9ce965299d2 58 } else if (m_play > 1) {
syundo0730 9:d9ce965299d2 59 step_inter(); //interpolate
syundo0730 9:d9ce965299d2 60 ++m_play;
syundo0730 9:d9ce965299d2 61 } else if (m_play == 1) { //inter para
syundo0730 9:d9ce965299d2 62 set_inter();
syundo0730 9:d9ce965299d2 63 step_inter();
syundo0730 9:d9ce965299d2 64 ++m_play;
syundo0730 9:d9ce965299d2 65 } else if (m_play == 0) { //start(called onece)
syundo0730 9:d9ce965299d2 66 init_inter();
syundo0730 9:d9ce965299d2 67 ++m_play;
syundo0730 9:d9ce965299d2 68 } else {
syundo0730 9:d9ce965299d2 69 return;
syundo0730 9:d9ce965299d2 70 }
syundo0730 9:d9ce965299d2 71 }
syundo0730 9:d9ce965299d2 72
syundo0730 9:d9ce965299d2 73 void Motion::set_inter()
syundo0730 9:d9ce965299d2 74 {
syundo0730 9:d9ce965299d2 75 m_brake_pnt = m_data_num[m_idx] / 2.0;
syundo0730 9:d9ce965299d2 76
syundo0730 9:d9ce965299d2 77 if (m_mode == 0){ //liner
syundo0730 9:d9ce965299d2 78 for (int i = 0; i < m_NUM_MAX; ++i) {
syundo0730 9:d9ce965299d2 79 m_d_buf[i] = m_data[m_idx + 1][i] - m_data[m_idx][i];
syundo0730 9:d9ce965299d2 80 m_d_buf[i] /= m_data_num[m_idx];
syundo0730 9:d9ce965299d2 81 m_dd_buf[i] = 0.0;
syundo0730 9:d9ce965299d2 82 m_buf[i] = m_data[m_idx][i];
syundo0730 9:d9ce965299d2 83 }
syundo0730 9:d9ce965299d2 84 m_brake_flg = 1.0;
syundo0730 9:d9ce965299d2 85 } else { //accel
syundo0730 9:d9ce965299d2 86 for (int i = 0; i < m_NUM_MAX; ++i) {
syundo0730 9:d9ce965299d2 87 m_dd_buf[i] = m_data[m_idx + 1][i] - m_data[m_idx][i];
syundo0730 9:d9ce965299d2 88 m_dd_buf[i] = m_dd_buf[i] / m_data_num[m_idx] / m_data_num[m_idx] * 4.0;
syundo0730 9:d9ce965299d2 89 m_d_buf[i] = 0.0;
syundo0730 9:d9ce965299d2 90 m_buf[i] = m_data[m_idx][i];
syundo0730 9:d9ce965299d2 91 }
syundo0730 9:d9ce965299d2 92 m_brake_flg = 1.0;
syundo0730 9:d9ce965299d2 93 }
syundo0730 9:d9ce965299d2 94 }
syundo0730 9:d9ce965299d2 95
syundo0730 9:d9ce965299d2 96 void Motion::init_inter()
syundo0730 9:d9ce965299d2 97 {
syundo0730 9:d9ce965299d2 98 for (int i = 0; i < m_NUM_MAX; ++i) {
syundo0730 9:d9ce965299d2 99 m_buf[i] = m_data[m_idx][i];
syundo0730 9:d9ce965299d2 100 m_d_buf[i] = 0.0;
syundo0730 9:d9ce965299d2 101 m_dd_buf[i] = 0.0;
syundo0730 9:d9ce965299d2 102
syundo0730 9:d9ce965299d2 103 __disable_irq();
syundo0730 12:6cd135bf03bd 104 //pwm->SetDuty(i, (uint32_t)m_buf[i]);
syundo0730 9:d9ce965299d2 105 pwm.SetDuty(i, (uint32_t)m_buf[i]);
syundo0730 9:d9ce965299d2 106 __enable_irq();
syundo0730 9:d9ce965299d2 107 }
syundo0730 9:d9ce965299d2 108 m_brake_flg = 0.0;
syundo0730 9:d9ce965299d2 109 }
syundo0730 9:d9ce965299d2 110
syundo0730 9:d9ce965299d2 111 void Motion::step_inter()
syundo0730 9:d9ce965299d2 112 {
syundo0730 9:d9ce965299d2 113 if (m_play > m_brake_pnt) {
syundo0730 9:d9ce965299d2 114 m_brake_flg = -1.0;
syundo0730 9:d9ce965299d2 115 }
syundo0730 9:d9ce965299d2 116 for (int i = 0; i < m_NUM_MAX; ++i) {
syundo0730 9:d9ce965299d2 117 m_d_buf[i] += (m_dd_buf[i]) * m_brake_flg;
syundo0730 9:d9ce965299d2 118 m_buf[i] += m_d_buf[i];
syundo0730 9:d9ce965299d2 119
syundo0730 9:d9ce965299d2 120 __disable_irq();
syundo0730 12:6cd135bf03bd 121 //pwm->SetDuty(i, (uint32_t)m_buf[i]);
syundo0730 9:d9ce965299d2 122 pwm.SetDuty(i, (uint32_t)m_buf[i]);
syundo0730 9:d9ce965299d2 123 __enable_irq();
syundo0730 9:d9ce965299d2 124 }
syundo0730 10:be8b10e54ecb 125 }
syundo0730 10:be8b10e54ecb 126
syundo0730 10:be8b10e54ecb 127 bool Motion::is_in_interrupt()
syundo0730 10:be8b10e54ecb 128 {
syundo0730 10:be8b10e54ecb 129 return m_in_interrupt;
syundo0730 9:d9ce965299d2 130 }