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@10:be8b10e54ecb, 2012-09-22 (annotated)

Committer:
syundo0730
Date:
Sat Sep 22 06:58:21 2012 +0000
Revision:
10:be8b10e54ecb
Parent:
9:d9ce965299d2
Child:
11:1539d181e159
repeat

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