Shundo Kishi / Mbed 2 deprecated Hobby_Humanoid_controlor

Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.

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

OnlineMotion.cpp@18:7077bedc37eb, 2013-04-03 (annotated)

Committer:
syundo0730
Date:
Wed Apr 03 04:56:13 2013 +0000
Revision:
18:7077bedc37eb
Parent:
17:60de3bfdc70b
Child:
19:c2ec475367aa
Real time module. Is step timing suspicious?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syundo0730 17:60de3bfdc70b 1 #include "OnlineMotion.h"
syundo0730 17:60de3bfdc70b 2
syundo0730 17:60de3bfdc70b 3 extern Ticker tick;
syundo0730 17:60de3bfdc70b 4 extern DigitalOut led1;
syundo0730 17:60de3bfdc70b 5
syundo0730 17:60de3bfdc70b 6 OnlineMotion::OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, PWM* _pwm, bool* _playing)
syundo0730 18:7077bedc37eb 7 : pwm(_pwm), playing(_playing), t(0), T(_T), m_NUM_MAX(_size_num - 1), STEP(_TIMESTEP)
syundo0730 17:60de3bfdc70b 8 {
syundo0730 18:7077bedc37eb 9 //pwm = _pwm;
syundo0730 18:7077bedc37eb 10 //playing = _playing;
syundo0730 17:60de3bfdc70b 11 *playing = true;
syundo0730 17:60de3bfdc70b 12
syundo0730 18:7077bedc37eb 13 //t = 0;
syundo0730 18:7077bedc37eb 14 //T = _T;
syundo0730 18:7077bedc37eb 15 //m_NUM_MAX = _size_num - 1;
syundo0730 18:7077bedc37eb 16 //STEP = _TIMESTEP;
syundo0730 17:60de3bfdc70b 17 }
syundo0730 17:60de3bfdc70b 18
syundo0730 17:60de3bfdc70b 19 OnlineMotion::~OnlineMotion()
syundo0730 17:60de3bfdc70b 20 {
syundo0730 17:60de3bfdc70b 21 led1 = !led1;
syundo0730 17:60de3bfdc70b 22 }
syundo0730 17:60de3bfdc70b 23
syundo0730 17:60de3bfdc70b 24 void OnlineMotion::step()
syundo0730 17:60de3bfdc70b 25 {
syundo0730 17:60de3bfdc70b 26 if (t < T) {
syundo0730 17:60de3bfdc70b 27 update();
syundo0730 17:60de3bfdc70b 28 } else {
syundo0730 18:7077bedc37eb 29 *playing = false;
syundo0730 17:60de3bfdc70b 30 tick.detach();
syundo0730 17:60de3bfdc70b 31 delete this;
syundo0730 17:60de3bfdc70b 32 }
syundo0730 17:60de3bfdc70b 33 }
syundo0730 17:60de3bfdc70b 34
syundo0730 17:60de3bfdc70b 35 void OnlineMotion::update()
syundo0730 17:60de3bfdc70b 36 {
syundo0730 17:60de3bfdc70b 37 play_online();
syundo0730 17:60de3bfdc70b 38 t += STEP;
syundo0730 17:60de3bfdc70b 39 }
syundo0730 17:60de3bfdc70b 40
syundo0730 17:60de3bfdc70b 41 void OnlineMotion::invertkinematics(float* pos, uint16_t* theta)
syundo0730 17:60de3bfdc70b 42 {
syundo0730 17:60de3bfdc70b 43 float L1 = 45.0;
syundo0730 17:60de3bfdc70b 44 float L2 = 90.0;
syundo0730 17:60de3bfdc70b 45 float L, h;
syundo0730 17:60de3bfdc70b 46
syundo0730 17:60de3bfdc70b 47 float x = pos[0];
syundo0730 17:60de3bfdc70b 48 float y = pos[1];
syundo0730 17:60de3bfdc70b 49 float z = pos[2];
syundo0730 17:60de3bfdc70b 50
syundo0730 17:60de3bfdc70b 51 L = sqrt(z*z + y*y) - 2 * L1;
syundo0730 17:60de3bfdc70b 52 h = sqrt(x*x + L*L);
syundo0730 17:60de3bfdc70b 53
syundo0730 17:60de3bfdc70b 54 float alpha = atan(y / z);
syundo0730 17:60de3bfdc70b 55 float beta = acos(h / (2 * L2));
syundo0730 17:60de3bfdc70b 56 float gunmma = asin(x / h);
syundo0730 17:60de3bfdc70b 57
syundo0730 17:60de3bfdc70b 58 theta[0] = alpha * RADTOVAL;
syundo0730 17:60de3bfdc70b 59 theta[1] = (beta - gunmma) * RADTOVAL;
syundo0730 17:60de3bfdc70b 60 theta[2] = 2 * beta * RADTOVAL;
syundo0730 17:60de3bfdc70b 61 theta[3] = (beta + gunmma) * RADTOVAL;
syundo0730 17:60de3bfdc70b 62 }
syundo0730 17:60de3bfdc70b 63
syundo0730 17:60de3bfdc70b 64 void OnlineMotion::walk(float* lpos, float* rpos, float h, float stride, float side, float up)
syundo0730 17:60de3bfdc70b 65 {
syundo0730 17:60de3bfdc70b 66 if (t < T / 2) {
syundo0730 17:60de3bfdc70b 67 lpos[0] = stride - (4 * stride / T) * t;
syundo0730 17:60de3bfdc70b 68 rpos[0] = -stride + (4 * stride / T) * t;
syundo0730 17:60de3bfdc70b 69 if (t < T / 6) {
syundo0730 17:60de3bfdc70b 70 lpos[2] = h;
syundo0730 17:60de3bfdc70b 71 } else if (t < T / 3) {
syundo0730 17:60de3bfdc70b 72 lpos[2] = h - up * sin((t-T/6) * M_PI/(T/6));
syundo0730 17:60de3bfdc70b 73 } else {
syundo0730 17:60de3bfdc70b 74 lpos[2] = h;
syundo0730 17:60de3bfdc70b 75 }
syundo0730 17:60de3bfdc70b 76 rpos[2] = h;
syundo0730 17:60de3bfdc70b 77 } else {
syundo0730 17:60de3bfdc70b 78 lpos[0] = -stride + (4 * stride / T) * (t - T/2);
syundo0730 17:60de3bfdc70b 79 rpos[0] = stride - (4 * stride / T) * (t - T/2);
syundo0730 17:60de3bfdc70b 80 if (t < 4*T/6) {
syundo0730 17:60de3bfdc70b 81 rpos[2] = h;
syundo0730 17:60de3bfdc70b 82 } else if (t < 5*T/6) {
syundo0730 17:60de3bfdc70b 83 rpos[2] = h -up * sin((t-4*T/6) * M_PI/(T/6));
syundo0730 17:60de3bfdc70b 84 } else {
syundo0730 17:60de3bfdc70b 85 rpos[2] = h;
syundo0730 17:60de3bfdc70b 86 }
syundo0730 17:60de3bfdc70b 87 lpos[2] = h;
syundo0730 17:60de3bfdc70b 88 }
syundo0730 17:60de3bfdc70b 89 lpos[1] = -side * sin(t * 2 * M_PI/T);
syundo0730 17:60de3bfdc70b 90 rpos[1] = lpos[1];
syundo0730 17:60de3bfdc70b 91 }
syundo0730 17:60de3bfdc70b 92
syundo0730 17:60de3bfdc70b 93 void OnlineMotion::play_online()
syundo0730 17:60de3bfdc70b 94 {
syundo0730 18:7077bedc37eb 95
syundo0730 17:60de3bfdc70b 96 uint16_t ltheta[5], rtheta[5];
syundo0730 17:60de3bfdc70b 97
syundo0730 17:60de3bfdc70b 98 float lpos[3], rpos[3];
syundo0730 17:60de3bfdc70b 99
syundo0730 18:7077bedc37eb 100 walk(lpos, rpos, 250, 20, 20, 15);
syundo0730 17:60de3bfdc70b 101
syundo0730 17:60de3bfdc70b 102 invertkinematics(lpos, ltheta);
syundo0730 17:60de3bfdc70b 103 invertkinematics(rpos, rtheta);
syundo0730 17:60de3bfdc70b 104
syundo0730 18:7077bedc37eb 105
syundo0730 18:7077bedc37eb 106 //uint16_t* pos;
syundo0730 17:60de3bfdc70b 107
syundo0730 18:7077bedc37eb 108 //pos = (uint16_t*)HOMEPOS;
syundo0730 18:7077bedc37eb 109
syundo0730 18:7077bedc37eb 110 /*
syundo0730 17:60de3bfdc70b 111 pos[1] = HOMEPOS[1] + ltheta[0];
syundo0730 17:60de3bfdc70b 112 pos[2] = HOMEPOS[2] - ltheta[1];
syundo0730 17:60de3bfdc70b 113 pos[3] = HOMEPOS[3] - ltheta[2];
syundo0730 17:60de3bfdc70b 114 pos[4] = HOMEPOS[4] + ltheta[3];
syundo0730 17:60de3bfdc70b 115 pos[5] = HOMEPOS[5] + ltheta[0];
syundo0730 17:60de3bfdc70b 116
syundo0730 17:60de3bfdc70b 117 pos[9] = HOMEPOS[9] + rtheta[0];
syundo0730 17:60de3bfdc70b 118 pos[10] = HOMEPOS[10] + rtheta[1];
syundo0730 17:60de3bfdc70b 119 pos[11] = HOMEPOS[11] + rtheta[2];
syundo0730 17:60de3bfdc70b 120 pos[12] = HOMEPOS[12] - rtheta[3];
syundo0730 17:60de3bfdc70b 121 pos[13] = HOMEPOS[13] + rtheta[0];
syundo0730 18:7077bedc37eb 122 */
syundo0730 18:7077bedc37eb 123
syundo0730 18:7077bedc37eb 124 //pos[10] = 1520 + 500 * sin(t * M_PI / T);
syundo0730 18:7077bedc37eb 125 //uint32_t poo = 1520 + 500 * sin(t * M_PI / T);
syundo0730 18:7077bedc37eb 126
syundo0730 18:7077bedc37eb 127 uint32_t pos[] = {
syundo0730 18:7077bedc37eb 128 HOMEPOS[0],
syundo0730 18:7077bedc37eb 129 HOMEPOS[1] + ltheta[0],
syundo0730 18:7077bedc37eb 130 HOMEPOS[2] - ltheta[1],
syundo0730 18:7077bedc37eb 131 HOMEPOS[3] - ltheta[2],
syundo0730 18:7077bedc37eb 132 HOMEPOS[4] + ltheta[3],
syundo0730 18:7077bedc37eb 133 HOMEPOS[5] + ltheta[0],
syundo0730 18:7077bedc37eb 134 HOMEPOS[6],
syundo0730 18:7077bedc37eb 135 HOMEPOS[7],
syundo0730 18:7077bedc37eb 136 HOMEPOS[8],
syundo0730 18:7077bedc37eb 137 HOMEPOS[9] + rtheta[0],
syundo0730 18:7077bedc37eb 138 HOMEPOS[10] + rtheta[1],
syundo0730 18:7077bedc37eb 139 HOMEPOS[11] + rtheta[2],
syundo0730 18:7077bedc37eb 140 HOMEPOS[12] - rtheta[3],
syundo0730 18:7077bedc37eb 141 HOMEPOS[13] + rtheta[0],
syundo0730 18:7077bedc37eb 142 HOMEPOS[14],
syundo0730 18:7077bedc37eb 143 HOMEPOS[15],
syundo0730 18:7077bedc37eb 144 HOMEPOS[16],
syundo0730 18:7077bedc37eb 145 HOMEPOS[17],
syundo0730 18:7077bedc37eb 146 HOMEPOS[18],
syundo0730 18:7077bedc37eb 147 HOMEPOS[19],
syundo0730 18:7077bedc37eb 148 HOMEPOS[20],
syundo0730 18:7077bedc37eb 149 HOMEPOS[21],
syundo0730 18:7077bedc37eb 150 HOMEPOS[22],
syundo0730 18:7077bedc37eb 151 HOMEPOS[23]
syundo0730 18:7077bedc37eb 152 };
syundo0730 17:60de3bfdc70b 153
syundo0730 17:60de3bfdc70b 154 for (int i = 0; i < m_NUM_MAX; ++i) {
syundo0730 17:60de3bfdc70b 155 __disable_irq();
syundo0730 18:7077bedc37eb 156 pwm->SetDuty(i, pos[i]);
syundo0730 17:60de3bfdc70b 157 __enable_irq();
syundo0730 17:60de3bfdc70b 158 }
syundo0730 17:60de3bfdc70b 159 }